From 880d9a8e3cbeb9254d7c9b35712bd52295681191 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 16 Dec 2013 21:33:12 +0000 Subject: [PATCH] Switched to new Eigen built-in special comma initializer --- examples/CameraResectioning.cpp | 2 +- examples/LocalizationExample.cpp | 8 +- examples/OdometryExample.cpp | 4 +- examples/PlanarSLAMExample.cpp | 6 +- examples/Pose2SLAMExample.cpp | 4 +- examples/Pose2SLAMExample_graph.cpp | 4 +- examples/Pose2SLAMExample_graphviz.cpp | 4 +- examples/Pose2SLAMwSPCG.cpp | 6 +- examples/SFMExample.cpp | 2 +- examples/SelfCalibrationExample.cpp | 4 +- examples/VisualISAM2Example.cpp | 2 +- examples/VisualISAMExample.cpp | 2 +- examples/easyPoint2KalmanFilter.cpp | 8 +- gtsam/base/LieScalar.h | 4 +- gtsam/base/LieVector.h | 2 +- gtsam/base/SpecialCommaInitializer.h | 218 ---------------- gtsam/base/Vector.h | 1 - gtsam/base/tests/testCholesky.cpp | 6 +- gtsam/base/tests/testLieMatrix.cpp | 6 +- gtsam/base/tests/testLieVector.cpp | 4 +- gtsam/base/tests/testMatrix.cpp | 240 +++++++++--------- gtsam/base/tests/testNumericalDerivative.cpp | 22 +- gtsam/base/tests/testSerializationBase.cpp | 18 +- gtsam/base/tests/testSymmetricBlockMatrix.cpp | 22 +- gtsam/base/tests/testVector.cpp | 66 ++--- .../discrete/tests/testDiscreteMarginals.cpp | 6 +- gtsam/geometry/Cal3Bundler.cpp | 6 +- gtsam/geometry/Cal3DS2.cpp | 16 +- gtsam/geometry/Cal3_S2.cpp | 4 +- gtsam/geometry/Cal3_S2.h | 4 +- gtsam/geometry/CalibratedCamera.cpp | 6 +- gtsam/geometry/PinholeCamera.h | 2 +- gtsam/geometry/Point2.cpp | 4 +- gtsam/geometry/Pose2.cpp | 22 +- gtsam/geometry/Pose2.h | 2 +- gtsam/geometry/Pose3.cpp | 2 +- gtsam/geometry/Pose3.h | 2 +- gtsam/geometry/Rot2.cpp | 12 +- gtsam/geometry/StereoCamera.cpp | 6 +- gtsam/geometry/tests/testCalibratedCamera.cpp | 2 +- gtsam/geometry/tests/testPinholeCamera.cpp | 2 +- gtsam/geometry/tests/testPoint2.cpp | 6 +- gtsam/geometry/tests/testPoint3.cpp | 2 +- gtsam/geometry/tests/testPose2.cpp | 52 ++-- gtsam/geometry/tests/testPose3.cpp | 20 +- gtsam/geometry/tests/testRot3M.cpp | 50 ++-- gtsam/geometry/tests/testRot3Q.cpp | 56 ++-- gtsam/geometry/tests/testSimpleCamera.cpp | 4 +- gtsam/geometry/tests/testStereoCamera.cpp | 2 +- gtsam/geometry/tests/testStereoPoint2.cpp | 4 +- gtsam/linear/tests/testErrors.cpp | 4 +- .../tests/testGaussianBayesNetUnordered.cpp | 60 ++--- .../tests/testGaussianBayesTreeUnordered.cpp | 54 ++-- .../testGaussianConditionalUnordered.cpp | 44 ++-- gtsam/linear/tests/testGaussianDensity.cpp | 4 +- .../testGaussianFactorGraphUnordered.cpp | 80 +++--- .../tests/testHessianFactorUnordered.cpp | 142 +++++------ .../tests/testJacobianFactorUnordered.cpp | 70 ++--- gtsam/linear/tests/testKalmanFilter.cpp | 34 +-- gtsam/linear/tests/testNoiseModel.cpp | 70 ++--- .../linear/tests/testSerializationLinear.cpp | 34 +-- .../tests/testVectorValuesUnordered.cpp | 78 +++--- gtsam/navigation/tests/testImuBias.cpp | 4 +- gtsam/nonlinear/WhiteNoiseFactor.h | 12 +- .../tests/testLinearContainerFactor.cpp | 54 ++-- gtsam/nonlinear/tests/testValues.cpp | 16 +- gtsam/slam/BoundingConstraint.h | 8 +- gtsam/slam/dataset.cpp | 8 +- gtsam/slam/tests/testGeneralSFMFactor.cpp | 6 +- .../testGeneralSFMFactor_Cal3Bundler.cpp | 6 +- gtsam/slam/tests/testPoseRotationPrior.cpp | 10 +- gtsam/slam/tests/testPoseTranslationPrior.cpp | 12 +- gtsam/slam/tests/testProjectionFactor.cpp | 12 +- gtsam/slam/tests/testRangeFactor.cpp | 8 +- gtsam/slam/tests/testStereoFactor.cpp | 14 +- gtsam_unstable/dynamics/PoseRTV.cpp | 6 +- gtsam_unstable/dynamics/SimpleHelicopter.h | 4 +- .../dynamics/tests/testIMUSystem.cpp | 12 +- gtsam_unstable/dynamics/tests/testPoseRTV.cpp | 10 +- .../dynamics/tests/testSimpleHelicopter.cpp | 16 +- .../dynamics/tests/testVelocityConstraint.cpp | 4 +- ...ConcurrentFilteringAndSmoothingExample.cpp | 16 +- .../examples/FixedLagSmootherExample.cpp | 6 +- gtsam_unstable/geometry/BearingS2.cpp | 2 +- gtsam_unstable/geometry/InvDepthCamera3.h | 4 +- gtsam_unstable/geometry/triangulation.h | 4 +- .../tests/testBatchFixedLagSmoother.cpp | 4 +- .../tests/testConcurrentBatchFilter.cpp | 8 +- .../tests/testConcurrentBatchSmoother.cpp | 8 +- .../tests/testConcurrentIncrementalFilter.cpp | 10 +- .../testConcurrentIncrementalSmootherDL.cpp | 8 +- .../testConcurrentIncrementalSmootherGN.cpp | 8 +- .../tests/testIncrementalFixedLagSmoother.cpp | 4 +- gtsam_unstable/slam/AHRS.cpp | 10 +- .../slam/EquivInertialNavFactor_GlobalVel.h | 10 +- .../EquivInertialNavFactor_GlobalVel_NoBias.h | 4 +- .../slam/InertialNavFactor_GlobalVelocity.h | 10 +- gtsam_unstable/slam/InvDepthFactorVariant3.h | 4 +- gtsam_unstable/slam/Mechanization_bRn2.cpp | 8 +- gtsam_unstable/slam/tests/testAHRS.cpp | 12 +- .../slam/tests/testBetweenFactorEM.cpp | 32 +-- .../testEquivInertialNavFactor_GlobalVel.cpp | 6 +- .../testInertialNavFactor_GlobalVelocity.cpp | 148 +++++------ .../slam/tests/testInvDepthFactorVariant1.cpp | 6 +- .../slam/tests/testInvDepthFactorVariant2.cpp | 6 +- .../slam/tests/testInvDepthFactorVariant3.cpp | 6 +- .../slam/tests/testMultiProjectionFactor.cpp | 12 +- .../tests/testRelativeElevationFactor.cpp | 10 +- .../slam/tests/testSmartRangeFactor.cpp | 14 +- .../testTransformBtwRobotsUnaryFactorEM.cpp | 26 +- tests/smallExample.h | 44 ++-- tests/testBoundingConstraint.cpp | 4 +- tests/testDoglegOptimizer.cpp | 44 ++-- tests/testExtendedKalmanFilter.cpp | 14 +- tests/testGaussianBayesTree.cpp | 18 +- tests/testGaussianFactorGraphB.cpp | 40 +-- tests/testGaussianISAM2.cpp | 12 +- tests/testGaussianJunctionTreeB.cpp | 6 +- tests/testGradientDescentOptimizer.cpp | 6 +- tests/testIterative.cpp | 6 +- tests/testMarginals.cpp | 6 +- tests/testNonlinearEquality.cpp | 16 +- tests/testNonlinearFactor.cpp | 94 +++---- tests/testNonlinearISAM.cpp | 10 +- tests/testSubgraphPreconditioner.cpp | 30 +-- 125 files changed, 1196 insertions(+), 1415 deletions(-) delete mode 100644 gtsam/base/SpecialCommaInitializer.h diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 63377f8aa..2048a84c8 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -70,7 +70,7 @@ int main(int argc, char* argv[]) { /* 2. add factors to the graph */ // add measurement factors - SharedDiagonal measurementNoise = Diagonal::Sigmas((Vec(2) << 0.5, 0.5)); + SharedDiagonal measurementNoise = Diagonal::Sigmas((Vector(2) << 0.5, 0.5)); boost::shared_ptr factor; graph.push_back( boost::make_shared(measurementNoise, X(1), calib, diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index c69c76e6c..a34a96c9b 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -93,8 +93,8 @@ public: // Consequently, the Jacobians are: // [ derror_x/dx derror_x/dy derror_x/dtheta ] = [1 0 0] // [ derror_y/dx derror_y/dy derror_y/dtheta ] = [0 1 0] - if (H) (*H) = (Mat(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0); - return (Vec(2) << q.x() - mx_, q.y() - my_); + if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0); + return (Vector(2) << q.x() - mx_, q.y() - my_); } // The second is a 'clone' function that allows the factor to be copied. Under most @@ -118,14 +118,14 @@ int main(int argc, char** argv) { // 2a. Add odometry factors // For simplicity, we will use the same noise model for each odometry factor - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1)); + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses graph.push_back(BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)); graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)); // 2b. Add "GPS-like" measurements // We will use our custom UnaryFactor for this. - noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1)); // 10cm std on x,y + noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // 10cm std on x,y graph.push_back(boost::make_shared(1, 0.0, 0.0, unaryNoise)); graph.push_back(boost::make_shared(2, 2.0, 0.0, unaryNoise)); graph.push_back(boost::make_shared(3, 4.0, 0.0, unaryNoise)); diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index 08b2d48f2..f2d4c6497 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -64,13 +64,13 @@ int main(int argc, char** argv) { // Add a prior on the first pose, setting it to the origin // A prior factor consists of a mean and a noise model (covariance matrix) Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); graph.push_back(PriorFactor(1, priorMean, priorNoise)); // Add odometry factors Pose2 odometry(2.0, 0.0, 0.0); // For simplicity, we will use the same noise model for each odometry factor - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1)); + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses graph.push_back(BetweenFactor(1, 2, odometry, odometryNoise)); graph.push_back(BetweenFactor(2, 3, odometry, odometryNoise)); diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index 94cf99c0e..a1c75eab7 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -80,18 +80,18 @@ int main(int argc, char** argv) { // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta graph.push_back(PriorFactor(x1, prior, priorNoise)); // add directly to graph // Add two odometry factors Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta graph.push_back(BetweenFactor(x1, x2, odometry, odometryNoise)); graph.push_back(BetweenFactor(x2, x3, odometry, odometryNoise)); // Add Range-Bearing measurements to two different landmarks // create a noise model for the landmark measurements - noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range + noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range // create the measurement values - indices are (pose id, landmark id) Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90), diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index 23ab3cf72..e1a51a493 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -71,11 +71,11 @@ int main(int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin // A prior factor consists of a mean and a noise model (covariance matrix) - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); graph.push_back(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); // For simplicity, we will use the same noise model for odometry and loop closures - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1)); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // 2b. Add odometry factors // Create odometry (Between) factors between consecutive poses diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index c7d9c2c7e..449144fef 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -31,13 +31,13 @@ int main (int argc, char** argv) { // we are in build/examples, data is in examples/Data NonlinearFactorGraph::shared_ptr graph; Values::shared_ptr initial; - SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vec(3) << 0.05, 0.05, 5.0 * M_PI / 180.0)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0)); boost::tie(graph, initial) = load2D("../../examples/Data/w100.graph", model); initial->print("Initial estimate:\n"); // Add a Gaussian prior on first poses Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.01, 0.01, 0.01)); + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.01, 0.01, 0.01)); graph->push_back(PriorFactor(0, priorMean, priorNoise)); // Single Step Optimization using Levenberg-Marquardt diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp index 912413e1e..4b0dc6252 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -32,11 +32,11 @@ int main (int argc, char** argv) { NonlinearFactorGraph graph; // 2a. Add a prior on the first pose, setting it to the origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); graph.push_back(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); // For simplicity, we will use the same noise model for odometry and loop closures - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1)); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // 2b. Add odometry factors graph.push_back(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 32eb6f104..2b2f63064 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -68,12 +68,12 @@ int main(int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin // A prior factor consists of a mean and a noise model (covariance matrix) Pose2 prior(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); graph.push_back(PriorFactor(1, prior, priorNoise)); // 2b. Add odometry factors // For simplicity, we will use the same noise model for each odometry factor - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1)); + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses graph.push_back(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); @@ -85,7 +85,7 @@ int main(int argc, char** argv) { // these constraints may be identified in many ways, such as appearance-based techniques // with camera images. // We will use another Between Factor to enforce this constraint, with the distance set to zero, - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1)); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); graph.push_back(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), model)); graph.print("\nFactor Graph:\n"); // print diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 5e342af88..9fa4bd026 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -80,7 +80,7 @@ int main(int argc, char* argv[]) { NonlinearFactorGraph graph; // Add a prior on pose x1. This indirectly specifies where the origin is. - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vec(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph // Simulated measurements from each camera pose, adding them to the factor graph diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index 7726bd75d..c69afe3b7 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/examples/SelfCalibrationExample.cpp @@ -53,7 +53,7 @@ int main(int argc, char* argv[]) { NonlinearFactorGraph graph; // Add a prior on pose x1. - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vec(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // Simulated measurements from each camera pose, adding them to the factor graph @@ -74,7 +74,7 @@ int main(int argc, char* argv[]) { graph.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph // Add a prior on the calibration. - noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vec(5) << 500, 500, 0.1, 100, 100)); + noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100)); graph.push_back(PriorFactor(Symbol('K', 0), K, calNoise)); // Create the initial estimate to the solution diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index 5d9515d14..de4de455a 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -103,7 +103,7 @@ int main(int argc, char* argv[]) { // adding it to iSAM. if( i == 0) { // Add a prior on pose x0 - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vec(6) << Vector3::Constant(0.3),Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // Add a prior on landmark l0 diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index a468f2be2..b5645e214 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -97,7 +97,7 @@ int main(int argc, char* argv[]) { // adding it to iSAM. if( i == 0) { // Add a prior on pose x0 - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vec(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // Add a prior on landmark l0 diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index ca60669cc..bb25440b1 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -37,7 +37,7 @@ int main() { // Create the Kalman Filter initialization point Point2 x_initial(0.0, 0.0); - SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1)); + SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // Create Key for initial pose Symbol x0('x',0); @@ -57,8 +57,8 @@ int main() { // For the purposes of this example, let us assume we are using a constant-position model and // the controls are driving the point to the right at 1 m/s. Then, F = [1 0 ; 0 1], B = [1 0 ; 0 1] // and u = [1 ; 0]. Let us also assume that the process noise Q = [0.1 0 ; 0 0.1]. - Vector u = (Vec(2) << 1.0, 0.0); - SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1), true); + Vector u = (Vector(2) << 1.0, 0.0); + SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1), true); // This simple motion can be modeled with a BetweenFactor // Create Key for next pose @@ -83,7 +83,7 @@ int main() { // For the purposes of this example, let us assume we have something like a GPS that returns // the current position of the robot. Then H = [1 0 ; 0 1]. Let us also assume that the measurement noise // R = [0.25 0 ; 0 0.25]. - SharedDiagonal R = noiseModel::Diagonal::Sigmas((Vec(2) << 0.25, 0.25), true); + SharedDiagonal R = noiseModel::Diagonal::Sigmas((Vector(2) << 0.25, 0.25), true); // This simple measurement can be modeled with a PriorFactor Point2 z1(1.0, 0.0); diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 1191eae81..21a2e10ad 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -58,7 +58,7 @@ namespace gtsam { LieScalar retract(const Vector& v) const { return LieScalar(value() + v(0)); } /** @return the local coordinates of another object */ - Vector localCoordinates(const LieScalar& t2) const { return (Vec(1) << (t2.value() - value())); } + Vector localCoordinates(const LieScalar& t2) const { return (Vector(1) << (t2.value() - value())); } // Group requirements @@ -96,7 +96,7 @@ namespace gtsam { static LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); } /** Logmap around identity - just returns with default cast back */ - static Vector Logmap(const LieScalar& p) { return (Vec(1) << p.value()); } + static Vector Logmap(const LieScalar& p) { return (Vector(1) << p.value()); } /// Left-trivialized derivative of the exponential map static Matrix dexpL(const Vector& v) { diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index efc1c9fe7..f1b05b34b 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -39,7 +39,7 @@ struct LieVector : public Vector, public DerivedValue { LieVector(const Eigen::Matrix& v) : Vector(v) {} /** wrap a double */ - LieVector(double d) : Vector((Vec(1) << d)) {} + LieVector(double d) : Vector((Vector(1) << d)) {} /** constructor with size and initial data, row order ! */ GTSAM_EXPORT LieVector(size_t m, const double* const data); diff --git a/gtsam/base/SpecialCommaInitializer.h b/gtsam/base/SpecialCommaInitializer.h deleted file mode 100644 index fbcbd09cc..000000000 --- a/gtsam/base/SpecialCommaInitializer.h +++ /dev/null @@ -1,218 +0,0 @@ -/** - * @file SpecialCommaInitializer.h - * @brief A special comma initializer for Eigen that is implicitly convertible to Vector and Matrix. - * @author Richard Roberts - * @created Oct 10, 2013 - */ - -#pragma once - -#include - -namespace Eigen { - namespace internal { - // Row-vectors not tested - //template - //inline void resizeHelper(XprType& xpr, DenseIndex sizeIncrement, - // typename boost::enable_if_c< - // XprType::ColsAtCompileTime == Dynamic && XprType::RowsAtCompileTime == 1>::type* = 0) - //{ - // xpr.conservativeResize(xpr.cols() + sizeIncrement); - //} - - template - inline void resizeHelper(XprType& xpr, typename XprType::Index sizeIncrement, - typename boost::enable_if_c< - XprType::RowsAtCompileTime == Dynamic && XprType::ColsAtCompileTime == 1>::type* = 0) - { - xpr.conservativeResize(xpr.rows() + sizeIncrement); - } - - template - inline void resizeHelper(XprType& xpr, typename XprType::Index sizeIncrement, - typename boost::enable_if_c< - XprType::ColsAtCompileTime == Dynamic>::type* = 0) - { - assert(false); - } - } - - /// A special comma initializer for Eigen that is implicitly convertible to Vector and Matrix. - template - class SpecialCommaInitializer : - public CommaInitializer, - public MatrixBase > - { - private: - bool dynamic_; - - public: - typedef MatrixBase > Base; - typedef CommaInitializer CommaBase; - - EIGEN_DENSE_PUBLIC_INTERFACE(SpecialCommaInitializer) - typedef typename internal::conditional::ret, - XprType, const XprType&>::type ExpressionTypeNested; - typedef typename XprType::InnerIterator InnerIterator; - - // Forward to base class - inline SpecialCommaInitializer(XprType& xpr, const typename XprType::Scalar& s, bool dynamic) : - CommaBase(xpr, s), dynamic_(dynamic) {} - - // Forward to base class - template - inline SpecialCommaInitializer(XprType& xpr, const DenseBase& other, bool dynamic) : - CommaBase(xpr, other), dynamic_(dynamic) {} - - inline Index rows() const { return CommaBase::m_xpr.rows(); } - inline Index cols() const { return CommaBase::m_xpr.cols(); } - inline Index outerStride() const { return CommaBase::m_xpr.outerStride(); } - inline Index innerStride() const { return CommaBase::m_xpr.innerStride(); } - - inline CoeffReturnType coeff(Index row, Index col) const - { - return CommaBase::m_xpr.coeff(row, col); - } - - inline CoeffReturnType coeff(Index index) const - { - return CommaBase::m_xpr.coeff(index); - } - - inline const Scalar& coeffRef(Index row, Index col) const - { - return CommaBase::m_xpr.const_cast_derived().coeffRef(row, col); - } - - inline const Scalar& coeffRef(Index index) const - { - return CommaBase::m_xpr.const_cast_derived().coeffRef(index); - } - - inline Scalar& coeffRef(Index row, Index col) - { - return CommaBase::m_xpr.const_cast_derived().coeffRef(row, col); - } - - inline Scalar& coeffRef(Index index) - { - return CommaBase::m_xpr.const_cast_derived().coeffRef(index); - } - - template - inline const PacketScalar packet(Index row, Index col) const - { - return CommaBase::m_xpr.template packet(row, col); - } - - template - inline void writePacket(Index row, Index col, const PacketScalar& x) - { - CommaBase::m_xpr.const_cast_derived().template writePacket(row, col, x); - } - - template - inline const PacketScalar packet(Index index) const - { - return CommaBase::m_xpr.template packet(index); - } - - template - inline void writePacket(Index index, const PacketScalar& x) - { - CommaBase::m_xpr.const_cast_derived().template writePacket(index, x); - } - - const XprType& _expression() const { return CommaBase::m_xpr; } - - /// Override base class comma operators to return this class instead of the base class. - SpecialCommaInitializer& operator,(const typename XprType::Scalar& s) - { - // If dynamic, resize the underlying object - if(dynamic_) - { - // Dynamic expansion currently only tested for column-vectors - assert(XprType::RowsAtCompileTime == Dynamic); - // Current col should be zero and row should be at the end - assert(CommaBase::m_col == 1); - assert(CommaBase::m_row == CommaBase::m_xpr.rows() - CommaBase::m_currentBlockRows); - resizeHelper(CommaBase::m_xpr, 1); - } - (void) CommaBase::operator,(s); - return *this; - } - - /// Override base class comma operators to return this class instead of the base class. - template - SpecialCommaInitializer& operator,(const DenseBase& other) - { - // If dynamic, resize the underlying object - if(dynamic_) - { - // Dynamic expansion currently only tested for column-vectors - assert(XprType::RowsAtCompileTime == Dynamic); - // Current col should be zero and row should be at the end - assert(CommaBase::m_col == 1); - assert(CommaBase::m_row == CommaBase::m_xpr.rows() - CommaBase::m_currentBlockRows); - resizeHelper(CommaBase::m_xpr, other.size()); - } - (void) CommaBase::operator,(other); - return *this; - } - }; - - namespace internal { - template - struct traits > : traits - { - }; - } - -} - -namespace gtsam { - class Vec - { - Eigen::VectorXd vector_; - bool dynamic_; - - public: - Vec(Eigen::VectorXd::Index size) : vector_(size), dynamic_(false) {} - - Vec() : dynamic_(true) {} - - Eigen::SpecialCommaInitializer operator<< (double s) - { - if(dynamic_) - vector_.resize(1); - return Eigen::SpecialCommaInitializer(vector_, s, dynamic_); - } - - template - Eigen::SpecialCommaInitializer operator<<(const Eigen::DenseBase& other) - { - if(dynamic_) - vector_.resize(other.size()); - return Eigen::SpecialCommaInitializer(vector_, other, dynamic_); - } - }; - - class Mat - { - Eigen::MatrixXd matrix_; - - public: - Mat(Eigen::MatrixXd::Index rows, Eigen::MatrixXd::Index cols) : matrix_(rows, cols) {} - - Eigen::SpecialCommaInitializer operator<< (double s) - { - return Eigen::SpecialCommaInitializer(matrix_, s, false); - } - - template - Eigen::SpecialCommaInitializer operator<<(const Eigen::DenseBase& other) - { - return Eigen::SpecialCommaInitializer(matrix_, other, false); - } - }; -} diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index b56ba7a7c..2c6066042 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -25,7 +25,6 @@ #include #include #include -#include namespace gtsam { diff --git a/gtsam/base/tests/testCholesky.cpp b/gtsam/base/tests/testCholesky.cpp index cda681c2f..b30326633 100644 --- a/gtsam/base/tests/testCholesky.cpp +++ b/gtsam/base/tests/testCholesky.cpp @@ -27,7 +27,7 @@ TEST(cholesky, choleskyPartial) { // choleskyPartial should only use the upper triangle, so this represents a // symmetric matrix. - Matrix ABC = (Mat(7,7) << + Matrix ABC = (Matrix(7,7) << 4.0375, 3.4584, 3.5735, 2.4815, 2.1471, 2.7400, 2.2063, 0., 4.7267, 3.8423, 2.3624, 2.8091, 2.9579, 2.5914, 0., 0., 5.1600, 2.0797, 3.4690, 3.2419, 2.9992, @@ -55,7 +55,7 @@ TEST(cholesky, choleskyPartial) { /* ************************************************************************* */ TEST(cholesky, BadScalingCholesky) { - Matrix A = (Mat(2,2) << + Matrix A = (Matrix(2,2) << 1e-40, 0.0, 0.0, 1.0); @@ -70,7 +70,7 @@ TEST(cholesky, BadScalingCholesky) { /* ************************************************************************* */ TEST(cholesky, BadScalingSVD) { - Matrix A = (Mat(2,2) << + Matrix A = (Matrix(2,2) << 1.0, 0.0, 0.0, 1e-40); diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp index 8be94c6cc..6f30f0378 100644 --- a/gtsam/base/tests/testLieMatrix.cpp +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -27,7 +27,7 @@ GTSAM_CONCEPT_LIE_INST(LieMatrix) /* ************************************************************************* */ TEST( LieMatrix, construction ) { - Matrix m = (Mat(2,2) << 1.0,2.0, 3.0,4.0); + Matrix m = (Matrix(2,2) << 1.0,2.0, 3.0,4.0); LieMatrix lie1(m), lie2(m); EXPECT(lie1.dim() == 4); @@ -37,7 +37,7 @@ TEST( LieMatrix, construction ) { /* ************************************************************************* */ TEST( LieMatrix, other_constructors ) { - Matrix init = (Mat(2,2) << 10.0,20.0, 30.0,40.0); + 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}; @@ -62,7 +62,7 @@ TEST(LieMatrix, retract) { EXPECT(assert_equal(expectedUpdate, actualUpdate)); - Vector expectedLogmap = (Vec() << 1, 2, 3, 4); + Vector expectedLogmap = (Vector() << 1, 2, 3, 4); Vector actualLogmap = LieMatrix::Logmap(LieMatrix(2,2, 1.0, 2.0, 3.0, 4.0)); EXPECT(assert_equal(expectedLogmap, actualLogmap)); } diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index d5a582c0f..67eed6656 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -27,7 +27,7 @@ GTSAM_CONCEPT_LIE_INST(LieVector) /* ************************************************************************* */ TEST( testLieVector, construction ) { - Vector v = (Vec(3) << 1.0, 2.0, 3.0); + Vector v = (Vector(3) << 1.0, 2.0, 3.0); LieVector lie1(v), lie2(v); EXPECT(lie1.dim() == 3); @@ -37,7 +37,7 @@ TEST( testLieVector, construction ) { /* ************************************************************************* */ TEST( testLieVector, other_constructors ) { - Vector init = (Vec(2) << 10.0, 20.0); + Vector init = (Vector(2) << 10.0, 20.0); LieVector exp(init); LieVector a(2,10.0,20.0); double data[] = {10,20}; diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index e8f7eb2f3..84d812489 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -32,7 +32,7 @@ static const double tol = 1e-9; /* ************************************************************************* */ TEST( matrix, constructor_data ) { - Matrix A = (Mat(2, 2) << -5, 3, 0, -5); + Matrix A = (Matrix(2, 2) << -5, 3, 0, -5); Matrix B(2, 2); B(0, 0) = -5; @@ -58,7 +58,7 @@ TEST( matrix, constructor_vector ) /* ************************************************************************* */ TEST( matrix, Matrix_ ) { - Matrix A = (Mat(2, 2) << -5.0, 3.0, 00.0, -5.0); + Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0); Matrix B(2, 2); B(0, 0) = -5; B(0, 1) = 3; @@ -94,17 +94,17 @@ TEST( matrix, special_comma_initializer) expected(1,0) = 3; expected(1,1) = 4; - Matrix actual1 = (Mat(2,2) << 1, 2, 3, 4); - Matrix actual2((Mat(2,2) << 1, 2, 3, 4)); + Matrix actual1 = (Matrix(2,2) << 1, 2, 3, 4); + Matrix actual2((Matrix(2,2) << 1, 2, 3, 4)); - Matrix submat1 = (Mat(1,2) << 3, 4); - Matrix actual3 = (Mat(2,2) << 1, 2, submat1); + Matrix submat1 = (Matrix(1,2) << 3, 4); + Matrix actual3 = (Matrix(2,2) << 1, 2, submat1); - Matrix submat2 = (Mat(1,2) << 1, 2); - Matrix actual4 = (Mat(2,2) << submat2, 3, 4); + Matrix submat2 = (Matrix(1,2) << 1, 2); + Matrix actual4 = (Matrix(2,2) << submat2, 3, 4); - Matrix actual5 = testFcn1((Mat(2,2) << 1, 2, 3, 4)); - Matrix actual6 = testFcn2((Mat(2,2) << 1, 2, 3, 4)); + Matrix actual5 = testFcn1((Matrix(2,2) << 1, 2, 3, 4)); + Matrix actual6 = testFcn2((Matrix(2,2) << 1, 2, 3, 4)); EXPECT(assert_equal(expected, actual1)); EXPECT(assert_equal(expected, actual2)); @@ -117,7 +117,7 @@ TEST( matrix, special_comma_initializer) /* ************************************************************************* */ TEST( matrix, col_major ) { - Matrix A = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0); + Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0); const double * const a = &A(0, 0); EXPECT_DOUBLES_EQUAL(1, a[0], tol); EXPECT_DOUBLES_EQUAL(3, a[1], tol); @@ -128,8 +128,8 @@ TEST( matrix, col_major ) /* ************************************************************************* */ TEST( matrix, collect1 ) { - Matrix A = (Mat(2, 2) << -5.0, 3.0, 00.0, -5.0); - Matrix B = (Mat(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); + Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0); + Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); Matrix AB = collect(2, &A, &B); Matrix C(2, 5); for (int i = 0; i < 2; i++) @@ -145,8 +145,8 @@ TEST( matrix, collect1 ) /* ************************************************************************* */ TEST( matrix, collect2 ) { - Matrix A = (Mat(2, 2) << -5.0, 3.0, 00.0, -5.0); - Matrix B = (Mat(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); + Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0); + Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); vector matrices; matrices.push_back(&A); matrices.push_back(&B); @@ -172,7 +172,7 @@ TEST( matrix, collect3 ) matrices.push_back(&A); matrices.push_back(&B); Matrix AB = collect(matrices, 2, 3); - Matrix exp = (Mat(2, 6) << + Matrix exp = (Matrix(2, 6) << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0); @@ -182,8 +182,8 @@ TEST( matrix, collect3 ) /* ************************************************************************* */ TEST( matrix, stack ) { - Matrix A = (Mat(2, 2) << -5.0, 3.0, 00.0, -5.0); - Matrix B = (Mat(3, 2) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); + Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0); + Matrix B = (Matrix(3, 2) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); Matrix AB = stack(2, &A, &B); Matrix C(5, 2); for (int i = 0; i < 2; i++) @@ -205,19 +205,19 @@ TEST( matrix, stack ) /* ************************************************************************* */ TEST( matrix, column ) { - Matrix A = (Mat(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., + Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., -0.1); Vector a1 = column(A, 0); - Vector exp1 = (Vec(4) << -1., 0., 1., 0.); + Vector exp1 = (Vector(4) << -1., 0., 1., 0.); EXPECT(assert_equal(a1, exp1)); Vector a2 = column(A, 3); - Vector exp2 = (Vec(4) << 0., 1., 0., 0.); + Vector exp2 = (Vector(4) << 0., 1., 0., 0.); EXPECT(assert_equal(a2, exp2)); Vector a3 = column(A, 6); - Vector exp3 = (Vec(4) << -0.2, 0.3, 0.2, -0.1); + Vector exp3 = (Vector(4) << -0.2, 0.3, 0.2, -0.1); EXPECT(assert_equal(a3, exp3)); } @@ -230,7 +230,7 @@ TEST( matrix, insert_column ) insertColumn(big, col, j); - Matrix expected = (Mat(5, 6) << + Matrix expected = (Matrix(5, 6) << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, @@ -253,7 +253,7 @@ TEST( matrix, insert_subcolumn ) Vector col2 = ones(1); insertColumn(big, col2, 4, 5); // check 2 - Matrix expected = (Mat(5, 6) << + Matrix expected = (Matrix(5, 6) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, @@ -266,19 +266,19 @@ TEST( matrix, insert_subcolumn ) /* ************************************************************************* */ TEST( matrix, row ) { - Matrix A = (Mat(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., + Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., -0.1); Vector a1 = row(A, 0); - Vector exp1 = (Vec(7) << -1., 0., 1., 0., 0., 0., -0.2); + Vector exp1 = (Vector(7) << -1., 0., 1., 0., 0., 0., -0.2); EXPECT(assert_equal(a1, exp1)); Vector a2 = row(A, 2); - Vector exp2 = (Vec(7) << 1., 0., 0., 0., -1., 0., 0.2); + Vector exp2 = (Vector(7) << 1., 0., 0., 0., -1., 0., 0.2); EXPECT(assert_equal(a2, exp2)); Vector a3 = row(A, 3); - Vector exp3 = (Vec(7) << 0., 1., 0., 0., 0., -1., -0.1); + Vector exp3 = (Vector(7) << 0., 1., 0., 0., 0., -1., -0.1); EXPECT(assert_equal(a3, exp3)); } @@ -301,12 +301,12 @@ TEST( matrix, zeros ) /* ************************************************************************* */ TEST( matrix, insert_sub ) { - Matrix big = zeros(5, 6), small = (Mat(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0, + Matrix big = zeros(5, 6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0); insertSub(big, small, 1, 2); - Matrix expected = (Mat(5, 6) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + Matrix expected = (Matrix(5, 6) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); @@ -323,7 +323,7 @@ TEST( matrix, diagMatrices ) Matrix actual = diag(Hs); - Matrix expected = (Mat(9, 9) << + Matrix expected = (Matrix(9, 9) << 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, @@ -339,7 +339,7 @@ TEST( matrix, diagMatrices ) /* ************************************************************************* */ TEST( matrix, stream_read ) { - Matrix expected = (Mat(3,4) << + Matrix expected = (Matrix(3,4) << 1.1, 2.3, 4.2, 7.6, -0.3, -8e-2, 5.1, 9.0, 1.2, 3.4, 4.5, 6.7); @@ -374,7 +374,7 @@ TEST( matrix, scale_columns ) A(2, 2) = 1.; A(2, 3) = 1.; - Vector v = (Vec(4) << 2., 3., 4., 5.); + Vector v = (Vector(4) << 2., 3., 4., 5.); Matrix actual = vector_scale(A, v); @@ -412,7 +412,7 @@ TEST( matrix, scale_rows ) A(2, 2) = 1.; A(2, 3) = 1.; - Vector v = (Vec(3) << 2., 3., 4.); + Vector v = (Vector(3) << 2., 3., 4.); Matrix actual = vector_scale(v, A); @@ -450,7 +450,7 @@ TEST( matrix, scale_rows_mask ) A(2, 2) = 1.; A(2, 3) = 1.; - Vector v = (Vec(3) << 2., std::numeric_limits::infinity(), 4.); + Vector v = (Vector(3) << 2., std::numeric_limits::infinity(), 4.); Matrix actual = vector_scale(v, A, true); @@ -533,18 +533,18 @@ TEST( matrix, equal_nan ) /* ************************************************************************* */ TEST( matrix, addition ) { - Matrix A = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0); - Matrix B = (Mat(2, 2) << 4.0, 3.0, 2.0, 1.0); - Matrix C = (Mat(2, 2) << 5.0, 5.0, 5.0, 5.0); + Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0); + Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0); + Matrix C = (Matrix(2, 2) << 5.0, 5.0, 5.0, 5.0); EQUALITY(A+B,C); } /* ************************************************************************* */ TEST( matrix, addition_in_place ) { - Matrix A = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0); - Matrix B = (Mat(2, 2) << 4.0, 3.0, 2.0, 1.0); - Matrix C = (Mat(2, 2) << 5.0, 5.0, 5.0, 5.0); + Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0); + Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0); + Matrix C = (Matrix(2, 2) << 5.0, 5.0, 5.0, 5.0); A += B; EQUALITY(A,C); } @@ -552,18 +552,18 @@ TEST( matrix, addition_in_place ) /* ************************************************************************* */ TEST( matrix, subtraction ) { - Matrix A = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0); - Matrix B = (Mat(2, 2) << 4.0, 3.0, 2.0, 1.0); - Matrix C = (Mat(2, 2) << -3.0, -1.0, 1.0, 3.0); + Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0); + Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0); + Matrix C = (Matrix(2, 2) << -3.0, -1.0, 1.0, 3.0); EQUALITY(A-B,C); } /* ************************************************************************* */ TEST( matrix, subtraction_in_place ) { - Matrix A = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0); - Matrix B = (Mat(2, 2) << 4.0, 3.0, 2.0, 1.0); - Matrix C = (Mat(2, 2) << -3.0, -1.0, 1.0, 3.0); + Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0); + Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0); + Matrix C = (Matrix(2, 2) << -3.0, -1.0, 1.0, 3.0); A -= B; EQUALITY(A,C); } @@ -613,10 +613,10 @@ TEST( matrix, matrix_vector_multiplication ) { Vector result(2); - Matrix A = (Mat(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Vector v = (Vec(3) << 1., 2., 3.); - Vector Av = (Vec(2) << 14., 32.); - Vector AtAv = (Vec(3) << 142., 188., 234.); + Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Vector v = (Vector(3) << 1., 2., 3.); + Vector Av = (Vector(2) << 14., 32.); + Vector AtAv = (Vector(3) << 142., 188., 234.); EQUALITY(A*v,Av); EQUALITY(A^Av,AtAv); @@ -650,12 +650,12 @@ TEST( matrix, scalar_divide ) /* ************************************************************************* */ TEST( matrix, zero_below_diagonal ) { - Matrix A1 = (Mat(3, 4) << + Matrix A1 = (Matrix(3, 4) << 1.0, 2.0, 3.0, 4.0, 1.0, 2.0, 3.0, 4.0, 1.0, 2.0, 3.0, 4.0); - Matrix expected1 = (Mat(3, 4) << + Matrix expected1 = (Matrix(3, 4) << 1.0, 2.0, 3.0, 4.0, 0.0, 2.0, 3.0, 4.0, 0.0, 0.0, 3.0, 4.0); @@ -671,13 +671,13 @@ TEST( matrix, zero_below_diagonal ) { zeroBelowDiagonal(actual1c, 4); EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10)); - Matrix A2 = (Mat(5, 3) << + Matrix A2 = (Matrix(5, 3) << 1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 1.0, 2.0, 3.0); - Matrix expected2 = (Mat(5, 3) << + Matrix expected2 = (Matrix(5, 3) << 1.0, 2.0, 3.0, 0.0, 2.0, 3.0, 0.0, 0.0, 3.0, @@ -692,7 +692,7 @@ TEST( matrix, zero_below_diagonal ) { zeroBelowDiagonal(actual2c); EXPECT(assert_equal(Matrix(expected2), actual2c, 1e-10)); - Matrix expected2_partial = (Mat(5, 3) << + Matrix expected2_partial = (Matrix(5, 3) << 1.0, 2.0, 3.0, 0.0, 2.0, 3.0, 0.0, 2.0, 3.0, @@ -735,14 +735,14 @@ TEST( matrix, inverse ) EXPECT(assert_equal(expected, Ainv, 1e-4)); // These two matrices failed before version 2003 because we called LU incorrectly - Matrix lMg((Mat(3, 3) << 0.0, 1.0, -2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0)); - EXPECT(assert_equal((Mat(3, 3) << + Matrix lMg((Matrix(3, 3) << 0.0, 1.0, -2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0)); + EXPECT(assert_equal((Matrix(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0), inverse(lMg))); - Matrix gMl((Mat(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0)); - EXPECT(assert_equal((Mat(3, 3) << + Matrix gMl((Matrix(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0)); + EXPECT(assert_equal((Matrix(3, 3) << 0.0, 1.0,-2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0), @@ -783,19 +783,19 @@ TEST( matrix, inverse2 ) TEST( matrix, backsubtitution ) { // TEST ONE 2x2 matrix U1*x=b1 - Vector expected1 = (Vec(2) << 3.6250, -0.75); - Matrix U22 = (Mat(2, 2) << 2., 3., 0., 4.); + Vector expected1 = (Vector(2) << 3.6250, -0.75); + Matrix U22 = (Matrix(2, 2) << 2., 3., 0., 4.); Vector b1 = U22 * expected1; EXPECT( assert_equal(expected1 , backSubstituteUpper(U22, b1), 0.000001)); // TEST TWO 3x3 matrix U2*x=b2 - Vector expected2 = (Vec(3) << 5.5, -8.5, 5.); - Matrix U33 = (Mat(3, 3) << 3., 5., 6., 0., 2., 3., 0., 0., 1.); + Vector expected2 = (Vector(3) << 5.5, -8.5, 5.); + Matrix U33 = (Matrix(3, 3) << 3., 5., 6., 0., 2., 3., 0., 0., 1.); Vector b2 = U33 * expected2; EXPECT( assert_equal(expected2 , backSubstituteUpper(U33, b2), 0.000001)); // TEST THREE Lower triangular 3x3 matrix L3*x=b3 - Vector expected3 = (Vec(3) << 1., 1., 1.); + Vector expected3 = (Vector(3) << 1., 1., 1.); Matrix L3 = trans(U33); Vector b3 = L3 * expected3; EXPECT( assert_equal(expected3 , backSubstituteLower(L3, b3), 0.000001)); @@ -809,11 +809,11 @@ TEST( matrix, householder ) { // check in-place householder, with v vectors below diagonal - Matrix expected1 = (Mat(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, + Matrix expected1 = (Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, -0.618034, 0, 4.4721, 0, -4.4721, 0, 0, 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894); - Matrix A1 = (Mat(4, 7) << -5, 0, 5, 0, 0, 0, -1, + Matrix A1 = (Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1, 00,-5, 0, 5, 0, 0, 1.5, 10, 0, 0, 0,-10,0, 2, 00, 10,0, 0, 0, -10, -1 ); @@ -822,10 +822,10 @@ TEST( matrix, householder ) // in-place, with zeros below diagonal - Matrix expected = (Mat(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, + Matrix expected = (Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, 0, 0, 4.4721, 0, -4.4721, 0.894); - Matrix A2 = (Mat(4, 7) << -5, 0, 5, 0, 0, 0, -1, + Matrix A2 = (Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1, 00,-5, 0, 5, 0, 0, 1.5, 10, 0, 0, 0,-10,0, 2, 00, 10,0, 0, 0, -10, -1); @@ -838,11 +838,11 @@ TEST( matrix, householder_colMajor ) { // check in-place householder, with v vectors below diagonal - Matrix expected1((Mat(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, + Matrix expected1((Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, -0.618034, 0, 4.4721, 0, -4.4721, 0, 0, 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894)); - Matrix A1((Mat(4, 7) << -5, 0, 5, 0, 0, 0, -1, + Matrix A1((Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1, 00,-5, 0, 5, 0, 0, 1.5, 10, 0, 0, 0,-10,0, 2, 00, 10,0, 0, 0, -10, -1)); @@ -851,10 +851,10 @@ TEST( matrix, householder_colMajor ) // in-place, with zeros below diagonal - Matrix expected((Mat(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, + Matrix expected((Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, 0, 0, 4.4721, 0, -4.4721, 0.894)); - Matrix A2((Mat(4, 7) << -5, 0, 5, 0, 0, 0, -1, + Matrix A2((Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1, 00,-5, 0, 5, 0, 0, 1.5, 10, 0, 0, 0,-10,0, 2, 00, 10,0, 0, 0, -10, -1)); @@ -869,10 +869,10 @@ TEST( matrix, eigen_QR ) // in-place, with zeros below diagonal - Matrix expected((Mat(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, + Matrix expected((Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, 0, 0, 4.4721, 0, -4.4721, 0.894)); - Matrix A((Mat(4, 7) << -5, 0, 5, 0, 0, 0, -1, + Matrix A((Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1, 00,-5, 0, 5, 0, 0, 1.5, 10, 0, 0, 0,-10,0, 2, 00, 10,0, 0, 0, -10, -1)); @@ -882,7 +882,7 @@ TEST( matrix, eigen_QR ) EXPECT(assert_equal(expected, actual, 1e-3)); // use shiny new in place QR inside gtsam - A = Matrix((Mat(4, 7) << -5, 0, 5, 0, 0, 0, -1, + A = Matrix((Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1, 00,-5, 0, 5, 0, 0, 1.5, 10, 0, 0, 0,-10,0, 2, 00, 10,0, 0, 0, -10, -1)); @@ -897,16 +897,16 @@ TEST( matrix, eigen_QR ) TEST( matrix, qr ) { - Matrix A = (Mat(6, 4) << -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00, + Matrix A = (Matrix(6, 4) << -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00, 0, 0, -10, 10, 0, -10, 0); - Matrix expectedQ = (Mat(6, 6) << -0.3333, 0, 0.2981, 0, 0, -0.8944, 0000000, -0.4472, 0, + Matrix expectedQ = (Matrix(6, 6) << -0.3333, 0, 0.2981, 0, 0, -0.8944, 0000000, -0.4472, 0, 0.3651, -0.8165, 0, 00.6667, 0, 0.7454, 0, 0, 0, 0000000, 0.8944, 0, 0.1826, -0.4082, 0, 0000000, 0, 0, -0.9129, -0.4082, 0, 00.6667, 0, -0.5963, 0, 0, -0.4472); - Matrix expectedR = (Mat(6, 4) << 15, 0, -8.3333, 0, 00, 11.1803, 0, -2.2361, 00, 0, + Matrix expectedR = (Matrix(6, 4) << 15, 0, -8.3333, 0, 00, 11.1803, 0, -2.2361, 00, 0, 7.4536, 0, 00, 0, 0, 10.9545, 00, 0, 0, 0, 00, 0, 0, 0); Matrix Q, R; @@ -919,11 +919,11 @@ TEST( matrix, qr ) /* ************************************************************************* */ TEST( matrix, sub ) { - Matrix A = (Mat(4, 6) << -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10, + Matrix A = (Matrix(4, 6) << -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10, 0, 00, 10, 0, 0, 0, -10); Matrix actual = sub(A, 1, 3, 1, 5); - Matrix expected = (Mat(2, 4) << -5, 0, 5, 0, 00, 0, 0, -10); + Matrix expected = (Matrix(2, 4) << -5, 0, 5, 0, 00, 0, 0, -10); EQUALITY(actual,expected); } @@ -931,15 +931,15 @@ TEST( matrix, sub ) /* ************************************************************************* */ TEST( matrix, trans ) { - Matrix A = (Mat(2, 2) << 1.0, 3.0, 2.0, 4.0); - Matrix B = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0); + Matrix A = (Matrix(2, 2) << 1.0, 3.0, 2.0, 4.0); + Matrix B = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0); EQUALITY(trans(A),B); } /* ************************************************************************* */ TEST( matrix, col_major_access ) { - Matrix A = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0); + Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0); const double* a = &A(0, 0); DOUBLES_EQUAL(2.0,a[2],1e-9); } @@ -948,16 +948,16 @@ TEST( matrix, col_major_access ) TEST( matrix, weighted_elimination ) { // create a matrix to eliminate - Matrix A = (Mat(4, 6) << -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0., + Matrix A = (Matrix(4, 6) << -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0., 1., 0., 0., 0., -1., 0., 0., 1., 0., 0., 0., -1.); - Vector b = (Vec(4) << -0.2, 0.3, 0.2, -0.1); - Vector sigmas = (Vec(4) << 0.2, 0.2, 0.1, 0.1); + Vector b = (Vector(4) << -0.2, 0.3, 0.2, -0.1); + Vector sigmas = (Vector(4) << 0.2, 0.2, 0.1, 0.1); // expected values - Matrix expectedR = (Mat(4, 6) << 1., 0., -0.2, 0., -0.8, 0., 0., 1., 0., + Matrix expectedR = (Matrix(4, 6) << 1., 0., -0.2, 0., -0.8, 0., 0., 1., 0., -0.2, 0., -0.8, 0., 0., 1., 0., -1., 0., 0., 0., 0., 1., 0., -1.); - Vector d = (Vec(4) << 0.2, -0.14, 0.0, 0.2); - Vector newSigmas = (Vec(4) << 0.0894427, 0.0894427, 0.223607, 0.223607); + Vector d = (Vector(4) << 0.2, -0.14, 0.0, 0.2); + Vector newSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607); Vector r; double di, sigma; @@ -982,11 +982,11 @@ TEST( matrix, weighted_elimination ) /* ************************************************************************* */ TEST( matrix, inverse_square_root ) { - Matrix measurement_covariance = (Mat(3, 3) << 0.25, 0.0, 0.0, 0.0, 0.25, + Matrix measurement_covariance = (Matrix(3, 3) << 0.25, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.01); Matrix actual = inverse_square_root(measurement_covariance); - Matrix expected = (Mat(3, 3) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, + Matrix expected = (Matrix(3, 3) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 10.0); EQUALITY(expected,actual); @@ -998,14 +998,14 @@ TEST( matrix, inverse_square_root ) // use the same inverse routing inside inverse_square_root() // as we use here to check it. - Matrix M = (Mat(5, 5) << + Matrix M = (Matrix(5, 5) << 0.0785892, 0.0137923, -0.0142219, -0.0171880, 0.0028726, 0.0137923, 0.0908911, 0.0020775, -0.0101952, 0.0175868, -0.0142219, 0.0020775, 0.0973051, 0.0054906, 0.0047064, -0.0171880,-0.0101952, 0.0054906, 0.0892453, -0.0059468, 0.0028726, 0.0175868, 0.0047064, -0.0059468, 0.0816517); - expected = (Mat(5, 5) << + expected = (Matrix(5, 5) << 3.567126953241796, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, -0.590030436566913, 3.362022286742925, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.618207860252376, -0.168166020746503, 3.253086082942785, 0.000000000000000, 0.000000000000000, @@ -1020,13 +1020,13 @@ TEST( matrix, inverse_square_root ) // we are checking against was generated via chol(M)' on octave TEST( matrix, LLt ) { - Matrix M = (Mat(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463, + Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463, 0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363, 0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463, 0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363, -0.0021113, 0.0036415, 0.0909464); - Matrix expected = (Mat(5, 5) << + Matrix expected = (Matrix(5, 5) << 0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, -0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000, @@ -1039,8 +1039,8 @@ TEST( matrix, LLt ) /* ************************************************************************* */ TEST( matrix, multiplyAdd ) { - Matrix A = (Mat(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); - Vector x = (Vec(4) << 1., 2., 3., 4.), e = (Vec(3) << 5., 6., 7.), + Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); + Vector x = (Vector(4) << 1., 2., 3., 4.), e = (Vector(3) << 5., 6., 7.), expected = e + A * x; multiplyAdd(1, A, x, e); @@ -1050,8 +1050,8 @@ TEST( matrix, multiplyAdd ) /* ************************************************************************* */ TEST( matrix, transposeMultiplyAdd ) { - Matrix A = (Mat(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); - Vector x = (Vec(4) << 1., 2., 3., 4.), e = (Vec(3) << 5., 6., 7.), + Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); + Vector x = (Vector(4) << 1., 2., 3., 4.), e = (Vector(3) << 5., 6., 7.), expected = x + trans(A) * e; transposeMultiplyAdd(1, A, e, x); @@ -1061,31 +1061,31 @@ TEST( matrix, transposeMultiplyAdd ) /* ************************************************************************* */ TEST( matrix, linear_dependent ) { - Matrix A = (Mat(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Matrix B = (Mat(2, 3) << -1.0, -2.0, -3.0, 8.0, 10.0, 12.0); + Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Matrix B = (Matrix(2, 3) << -1.0, -2.0, -3.0, 8.0, 10.0, 12.0); EXPECT(linear_dependent(A, B)); } /* ************************************************************************* */ TEST( matrix, linear_dependent2 ) { - Matrix A = (Mat(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Matrix B = (Mat(2, 3) << 0.0, -2.0, -3.0, 8.0, 10.0, 12.0); + Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.0, 10.0, 12.0); EXPECT(linear_dependent(A, B)); } /* ************************************************************************* */ TEST( matrix, linear_dependent3 ) { - Matrix A = (Mat(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Matrix B = (Mat(2, 3) << 0.0, -2.0, -3.0, 8.1, 10.0, 12.0); + Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.1, 10.0, 12.0); EXPECT(linear_independent(A, B)); } /* ************************************************************************* */ TEST( matrix, svd1 ) { - Vector v = (Vec(3) << 2., 1., 0.); + Vector v = (Vector(3) << 2., 1., 0.); Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1) * Matrix(trans(V1)); Matrix U, V; @@ -1098,7 +1098,7 @@ TEST( matrix, svd1 ) /* ************************************************************************* */ /// Sample A matrix for SVD -static Matrix sampleA = (Mat(3, 2) << 0.,-2., 0., 0., 3., 0.); +static Matrix sampleA = (Matrix(3, 2) << 0.,-2., 0., 0., 3., 0.); static Matrix sampleAt = trans(sampleA); /* ************************************************************************* */ @@ -1107,9 +1107,9 @@ TEST( matrix, svd2 ) Matrix U, V; Vector s; - Matrix expectedU = (Mat(3, 2) << 0.,-1.,0.,0.,1.,0.); - Vector expected_s = (Vec(2) << 3.,2.); - Matrix expectedV = (Mat(2, 2) << 1.,0.,0.,1.); + Matrix expectedU = (Matrix(3, 2) << 0.,-1.,0.,0.,1.,0.); + Vector expected_s = (Vector(2) << 3.,2.); + Matrix expectedV = (Matrix(2, 2) << 1.,0.,0.,1.); svd(sampleA, U, s, V); @@ -1124,9 +1124,9 @@ TEST( matrix, svd3 ) Matrix U, V; Vector s; - Matrix expectedU = (Mat(2, 2) << -1.,0.,0.,-1.); - Vector expected_s = (Vec(2) << 3.0, 2.0); - Matrix expectedV = (Mat(3, 2) << 0.,1.,0.,0.,-1.,0.); + Matrix expectedU = (Matrix(2, 2) << -1.,0.,0.,-1.); + Vector expected_s = (Vector(2) << 3.0, 2.0); + Matrix expectedV = (Matrix(3, 2) << 0.,1.,0.,0.,-1.,0.); svd(sampleAt, U, s, V); Matrix S = diag(s); @@ -1145,19 +1145,19 @@ TEST( matrix, svd4 ) Matrix U, V; Vector s; - Matrix A = (Mat(3, 2) << + Matrix A = (Matrix(3, 2) << 0.8147, 0.9134, 0.9058, 0.6324, 0.1270, 0.0975); - Matrix expectedU = (Mat(3, 2) << + Matrix expectedU = (Matrix(3, 2) << 0.7397, 0.6724, 0.6659, -0.7370, 0.0970, -0.0689); - Vector expected_s = (Vec(2) << 1.6455, 0.1910); + Vector expected_s = (Vector(2) << 1.6455, 0.1910); - Matrix expectedV = (Mat(2, 2) << + Matrix expectedV = (Matrix(2, 2) << 0.7403, -0.6723, 0.6723, 0.7403); @@ -1173,7 +1173,7 @@ TEST( matrix, svd4 ) /* ************************************************************************* */ TEST( matrix, DLT ) { - Matrix A = (Mat(8, 9) << + Matrix A = (Matrix(8, 9) << 0.21, -0.42, -10.71, 0.18, -0.36, -9.18, -0.61, 1.22, 31.11, 0.44, -0.66, -15.84, 0.34, -0.51, -12.24, -1.64, 2.46, 59.04, 0.69, -8.28, -12.19, -0.48, 5.76, 8.48, -1.89, 22.68, 33.39, @@ -1187,7 +1187,7 @@ TEST( matrix, DLT ) double error; Vector actual; boost::tie(rank,error,actual) = DLT(A); - Vector expected = (Vec(9) << -0.0, 0.2357, 0.4714, -0.2357, 0.0, - 0.4714,-0.4714, 0.4714, 0.0); + Vector expected = (Vector(9) << -0.0, 0.2357, 0.4714, -0.2357, 0.0, - 0.4714,-0.4714, 0.4714, 0.0); EXPECT_LONGS_EQUAL(8,rank); EXPECT_DOUBLES_EQUAL(0,error,1e-8); EXPECT(assert_equal(expected, actual, 1e-4)); diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index 55563c7bf..bd3c2c217 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -31,7 +31,7 @@ double f(const LieVector& x) { TEST(testNumericalDerivative, numericalHessian) { LieVector center = ones(2); - Matrix expected = (Mat(2,2) << + Matrix expected = (Matrix(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 = (Mat(2,2) << + Matrix expected = (Matrix(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 = (Mat(1, 1) << -sin(center1(0))*cos(center2(0))); + Matrix expected11 = (Matrix(1, 1) << -sin(center1(0))*cos(center2(0))); Matrix actual11 = numericalHessian211(f3, center1, center2); EXPECT(assert_equal(expected11, actual11, 1e-5)); - Matrix expected12 = (Mat(1, 1) <<-cos(center1(0))*sin(center2(0))); + Matrix expected12 = (Matrix(1, 1) <<-cos(center1(0))*sin(center2(0))); Matrix actual12 = numericalHessian212(f3, center1, center2); EXPECT(assert_equal(expected12, actual12, 1e-5)); - Matrix expected22 = (Mat(1, 1) <<-sin(center1(0))*cos(center2(0))); + Matrix expected22 = (Matrix(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 = (Mat(1, 1) << -sin(x)*cos(y)*z*z); + Matrix expected11 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z); Matrix actual11 = numericalHessian311(f4, center1, center2, center3); EXPECT(assert_equal(expected11, actual11, 1e-5)); - Matrix expected12 = (Mat(1, 1) << -cos(x)*sin(y)*z*z); + Matrix expected12 = (Matrix(1, 1) << -cos(x)*sin(y)*z*z); Matrix actual12 = numericalHessian312(f4, center1, center2, center3); EXPECT(assert_equal(expected12, actual12, 1e-5)); - Matrix expected13 = (Mat(1, 1) << cos(x)*cos(y)*2*z); + Matrix expected13 = (Matrix(1, 1) << cos(x)*cos(y)*2*z); Matrix actual13 = numericalHessian313(f4, center1, center2, center3); EXPECT(assert_equal(expected13, actual13, 1e-5)); - Matrix expected22 = (Mat(1, 1) << -sin(x)*cos(y)*z*z); + Matrix expected22 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z); Matrix actual22 = numericalHessian322(f4, center1, center2, center3); EXPECT(assert_equal(expected22, actual22, 1e-5)); - Matrix expected23 = (Mat(1, 1) << -sin(x)*sin(y)*2*z); + Matrix expected23 = (Matrix(1, 1) << -sin(x)*sin(y)*2*z); Matrix actual23 = numericalHessian323(f4, center1, center2, center3); EXPECT(assert_equal(expected23, actual23, 1e-5)); - Matrix expected33 = (Mat(1, 1) << sin(x)*cos(y)*2); + Matrix expected33 = (Matrix(1, 1) << sin(x)*cos(y)*2); Matrix actual33 = numericalHessian333(f4, center1, center2, center3); EXPECT(assert_equal(expected33, actual33, 1e-5)); } diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp index d2704fbf3..9b28eacf6 100644 --- a/gtsam/base/tests/testSerializationBase.cpp +++ b/gtsam/base/tests/testSerializationBase.cpp @@ -30,9 +30,9 @@ using namespace std; using namespace gtsam; using namespace gtsam::serializationTestHelpers; -Vector v1 = (Vec(2) << 1.0, 2.0); -Vector v2 = (Vec(2) << 3.0, 4.0); -Vector v3 = (Vec(2) << 5.0, 6.0); +Vector v1 = (Vector(2) << 1.0, 2.0); +Vector v2 = (Vector(2) << 3.0, 4.0); +Vector v3 = (Vector(2) << 5.0, 6.0); /* ************************************************************************* */ TEST (Serialization, FastList) { @@ -84,23 +84,23 @@ TEST (Serialization, FastVector) { /* ************************************************************************* */ TEST (Serialization, matrix_vector) { - EXPECT(equality((Vec(4) << 1.0, 2.0, 3.0, 4.0))); + EXPECT(equality((Vector(4) << 1.0, 2.0, 3.0, 4.0))); EXPECT(equality(Vector2(1.0, 2.0))); EXPECT(equality(Vector3(1.0, 2.0, 3.0))); EXPECT(equality((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished())); - EXPECT(equality((Mat(2, 2) << 1.0, 2.0, 3.0, 4.0))); + EXPECT(equality((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0))); - EXPECT(equalityXML((Vec(4) << 1.0, 2.0, 3.0, 4.0))); + EXPECT(equalityXML((Vector(4) << 1.0, 2.0, 3.0, 4.0))); EXPECT(equalityXML(Vector2(1.0, 2.0))); EXPECT(equalityXML(Vector3(1.0, 2.0, 3.0))); EXPECT(equalityXML((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished())); - EXPECT(equalityXML((Mat(2, 2) << 1.0, 2.0, 3.0, 4.0))); + EXPECT(equalityXML((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0))); - EXPECT(equalityBinary((Vec(4) << 1.0, 2.0, 3.0, 4.0))); + EXPECT(equalityBinary((Vector(4) << 1.0, 2.0, 3.0, 4.0))); EXPECT(equalityBinary(Vector2(1.0, 2.0))); EXPECT(equalityBinary(Vector3(1.0, 2.0, 3.0))); EXPECT(equalityBinary((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished())); - EXPECT(equalityBinary((Mat(2, 2) << 1.0, 2.0, 3.0, 4.0))); + EXPECT(equalityBinary((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0))); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testSymmetricBlockMatrix.cpp b/gtsam/base/tests/testSymmetricBlockMatrix.cpp index a4b330000..ed9d43e5a 100644 --- a/gtsam/base/tests/testSymmetricBlockMatrix.cpp +++ b/gtsam/base/tests/testSymmetricBlockMatrix.cpp @@ -25,7 +25,7 @@ using boost::assign::list_of; static SymmetricBlockMatrix testBlockMatrix( list_of(3)(2)(1), - (Mat(6, 6) << + (Matrix(6, 6) << 1, 2, 3, 4, 5, 6, 2, 8, 9, 10, 11, 12, 3, 9, 15, 16, 17, 18, @@ -37,7 +37,7 @@ static SymmetricBlockMatrix testBlockMatrix( TEST(SymmetricBlockMatrix, ReadBlocks) { // On the diagonal - Matrix expected1 = (Mat(2, 2) << + Matrix expected1 = (Matrix(2, 2) << 22, 23, 23, 29); Matrix actual1 = testBlockMatrix(1, 1); @@ -48,7 +48,7 @@ TEST(SymmetricBlockMatrix, ReadBlocks) EXPECT(assert_equal(Matrix(expected1.triangularView()), actual1t)); // Above the diagonal - Matrix expected2 = (Mat(3, 2) << + Matrix expected2 = (Matrix(3, 2) << 4, 5, 10, 11, 16, 17); @@ -56,7 +56,7 @@ TEST(SymmetricBlockMatrix, ReadBlocks) EXPECT(assert_equal(expected2, actual2)); // Below the diagonal - Matrix expected3 = (Mat(2, 3) << + Matrix expected3 = (Matrix(2, 3) << 4, 10, 16, 5, 11, 17); Matrix actual3 = testBlockMatrix(1, 0); @@ -99,7 +99,7 @@ TEST(SymmetricBlockMatrix, WriteBlocks) TEST(SymmetricBlockMatrix, Ranges) { // On the diagonal - Matrix expected1 = (Mat(3, 3) << + Matrix expected1 = (Matrix(3, 3) << 22, 23, 24, 23, 29, 30, 24, 30, 36); @@ -109,7 +109,7 @@ TEST(SymmetricBlockMatrix, Ranges) EXPECT(assert_equal(expected1, actual1a)); // Above the diagonal - Matrix expected2 = (Mat(3, 1) << + Matrix expected2 = (Matrix(3, 1) << 24, 30, 36); @@ -119,7 +119,7 @@ TEST(SymmetricBlockMatrix, Ranges) EXPECT(assert_equal(expected2, actual2a)); // Below the diagonal - Matrix expected3 = (Mat(3, 3) << + Matrix expected3 = (Matrix(3, 3) << 4, 10, 16, 5, 11, 17, 6, 12, 18); @@ -132,7 +132,7 @@ TEST(SymmetricBlockMatrix, Ranges) /* ************************************************************************* */ TEST(SymmetricBlockMatrix, expressions) { - SymmetricBlockMatrix expected1(list_of(2)(3)(1), (Mat(6, 6) << + SymmetricBlockMatrix expected1(list_of(2)(3)(1), (Matrix(6, 6) << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 6, 8, 0, @@ -140,7 +140,7 @@ TEST(SymmetricBlockMatrix, expressions) 0, 0, 0, 0, 16, 0, 0, 0, 0, 0, 0, 0)); - SymmetricBlockMatrix expected2(list_of(2)(3)(1), (Mat(6, 6) << + SymmetricBlockMatrix expected2(list_of(2)(3)(1), (Matrix(6, 6) << 0, 0, 10, 15, 20, 0, 0, 0, 12, 18, 24, 0, 0, 0, 0, 0, 0, 0, @@ -148,8 +148,8 @@ TEST(SymmetricBlockMatrix, expressions) 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)); - Matrix a = (Mat(1, 3) << 2, 3, 4); - Matrix b = (Mat(1, 2) << 5, 6); + Matrix a = (Matrix(1, 3) << 2, 3, 4); + Matrix b = (Matrix(1, 2) << 5, 6); SymmetricBlockMatrix bm1(list_of(2)(3)(1)); bm1.full().triangularView().setZero(); diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 047826087..9a01b4d68 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -26,7 +26,7 @@ using namespace gtsam; /* ************************************************************************* */ TEST( TestVector, Vector_variants ) { - Vector a = (Vec(2) << 10.0,20.0); + Vector a = (Vector(2) << 10.0,20.0); double data[] = {10,20}; Vector b = Vector_(2, data); EXPECT(assert_equal(a, b)); @@ -56,18 +56,18 @@ TEST( TestVector, special_comma_initializer) expected(1) = 2; expected(2) = 3; - Vector actual1 = (Vec(3) << 1, 2, 3); - Vector actual2((Vec(3) << 1, 2, 3)); - Vector actual3 = (Vec() << 1, 2, 3); + Vector actual1 = (Vector(3) << 1, 2, 3); + Vector actual2((Vector(3) << 1, 2, 3)); + Vector actual3 = (Vector() << 1, 2, 3); - Vector subvec1 = (Vec() << 2, 3); - Vector actual4 = (Vec() << 1, subvec1); + Vector subvec1 = (Vector() << 2, 3); + Vector actual4 = (Vector() << 1, subvec1); - Vector subvec2 = (Vec() << 1, 2); - Vector actual5 = (Vec() << subvec2, 3); + Vector subvec2 = (Vector() << 1, 2); + Vector actual5 = (Vector() << subvec2, 3); - Vector actual6 = testFcn1((Vec() << 1, 2, 3)); - Vector actual7 = testFcn2((Vec() << 1, 2, 3)); + Vector actual6 = testFcn1((Vector() << 1, 2, 3)); + Vector actual7 = testFcn2((Vector() << 1, 2, 3)); EXPECT(assert_equal(expected, actual1)); EXPECT(assert_equal(expected, actual2)); @@ -153,7 +153,7 @@ TEST( TestVector, subInsert ) size_t i = 2; subInsert(big, small, i); - Vector expected = (Vec(6) << 0.0, 0.0, 1.0, 1.0, 1.0, 0.0); + Vector expected = (Vector(6) << 0.0, 0.0, 1.0, 1.0, 1.0, 0.0); EXPECT(assert_equal(expected, big)); } @@ -251,13 +251,13 @@ TEST( TestVector, weightedPseudoinverse_constraint ) /* ************************************************************************* */ TEST( TestVector, weightedPseudoinverse_nan ) { - Vector a = (Vec(4) << 1., 0., 0., 0.); - Vector sigmas = (Vec(4) << 0.1, 0.1, 0., 0.); + Vector a = (Vector(4) << 1., 0., 0., 0.); + Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.); Vector weights = reciprocal(emul(sigmas,sigmas)); Vector pseudo; double precision; boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights); - Vector expected = (Vec(4) << 1., 0., 0.,0.); + Vector expected = (Vector(4) << 1., 0., 0.,0.); EXPECT(assert_equal(expected, pseudo)); DOUBLES_EQUAL(100, precision, 1e-5); } @@ -265,31 +265,31 @@ TEST( TestVector, weightedPseudoinverse_nan ) /* ************************************************************************* */ TEST( TestVector, ediv ) { - Vector a = (Vec(3) << 10., 20., 30.); - Vector b = (Vec(3) << 2.0, 5.0, 6.0); + Vector a = (Vector(3) << 10., 20., 30.); + Vector b = (Vector(3) << 2.0, 5.0, 6.0); Vector actual(ediv(a,b)); - Vector c = (Vec(3) << 5.0, 4.0, 5.0); + Vector c = (Vector(3) << 5.0, 4.0, 5.0); EXPECT(assert_equal(c,actual)); } /* ************************************************************************* */ TEST( TestVector, dot ) { - Vector a = (Vec(3) << 10., 20., 30.); - Vector b = (Vec(3) << 2.0, 5.0, 6.0); + Vector a = (Vector(3) << 10., 20., 30.); + Vector b = (Vector(3) << 2.0, 5.0, 6.0); DOUBLES_EQUAL(20+100+180,dot(a,b),1e-9); } /* ************************************************************************* */ TEST( TestVector, axpy ) { - Vector x = (Vec(3) << 10., 20., 30.); - Vector y0 = (Vec(3) << 2.0, 5.0, 6.0); + Vector x = (Vector(3) << 10., 20., 30.); + Vector y0 = (Vector(3) << 2.0, 5.0, 6.0); Vector y1 = y0, y2 = y0; axpy(0.1,x,y1); axpy(0.1,x,y2.head(3)); - Vector expected = (Vec(3) << 3.0, 7.0, 9.0); + Vector expected = (Vector(3) << 3.0, 7.0, 9.0); EXPECT(assert_equal(expected,y1)); EXPECT(assert_equal(expected,Vector(y2))); } @@ -297,8 +297,8 @@ TEST( TestVector, axpy ) /* ************************************************************************* */ TEST( TestVector, equals ) { - Vector v1 = (Vec(1) << 0.0/std::numeric_limits::quiet_NaN()); //testing nan - Vector v2 = (Vec(1) << 1.0); + Vector v1 = (Vector(1) << 0.0/std::numeric_limits::quiet_NaN()); //testing nan + Vector v2 = (Vector(1) << 1.0); double tol = 1.; EXPECT(!equal_with_abs_tol(v1, v2, tol)); } @@ -306,7 +306,7 @@ TEST( TestVector, equals ) /* ************************************************************************* */ TEST( TestVector, greater_than ) { - Vector v1 = (Vec(3) << 1.0, 2.0, 3.0), + Vector v1 = (Vector(3) << 1.0, 2.0, 3.0), v2 = zero(3); EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than EXPECT(greaterThanOrEqual(v1, v2)); // test equals @@ -315,31 +315,31 @@ TEST( TestVector, greater_than ) /* ************************************************************************* */ TEST( TestVector, reciprocal ) { - Vector v = (Vec(3) << 1.0, 2.0, 4.0); - EXPECT(assert_equal((Vec(3) << 1.0, 0.5, 0.25),reciprocal(v))); + Vector v = (Vector(3) << 1.0, 2.0, 4.0); + EXPECT(assert_equal((Vector(3) << 1.0, 0.5, 0.25),reciprocal(v))); } /* ************************************************************************* */ TEST( TestVector, linear_dependent ) { - Vector v1 = (Vec(3) << 1.0, 2.0, 3.0); - Vector v2 = (Vec(3) << -2.0, -4.0, -6.0); + Vector v1 = (Vector(3) << 1.0, 2.0, 3.0); + Vector v2 = (Vector(3) << -2.0, -4.0, -6.0); EXPECT(linear_dependent(v1, v2)); } /* ************************************************************************* */ TEST( TestVector, linear_dependent2 ) { - Vector v1 = (Vec(3) << 0.0, 2.0, 0.0); - Vector v2 = (Vec(3) << 0.0, -4.0, 0.0); + Vector v1 = (Vector(3) << 0.0, 2.0, 0.0); + Vector v2 = (Vector(3) << 0.0, -4.0, 0.0); EXPECT(linear_dependent(v1, v2)); } /* ************************************************************************* */ TEST( TestVector, linear_dependent3 ) { - Vector v1 = (Vec(3) << 0.0, 2.0, 0.0); - Vector v2 = (Vec(3) << 0.1, -4.1, 0.0); + Vector v1 = (Vector(3) << 0.0, 2.0, 0.0); + Vector v2 = (Vector(3) << 0.1, -4.1, 0.0); EXPECT(!linear_dependent(v1, v2)); } diff --git a/gtsam/discrete/tests/testDiscreteMarginals.cpp b/gtsam/discrete/tests/testDiscreteMarginals.cpp index e4c836c5d..c088bcb4e 100644 --- a/gtsam/discrete/tests/testDiscreteMarginals.cpp +++ b/gtsam/discrete/tests/testDiscreteMarginals.cpp @@ -53,10 +53,10 @@ TEST_UNSAFE( DiscreteMarginals, UGM_small ) { EXPECT_DOUBLES_EQUAL( 0.359631, (*actualC)(values), 1e-6); Vector actualCvector = marginals.marginalProbabilities(Cathy); - EXPECT(assert_equal((Vec(2) << 0.359631, 0.640369), actualCvector, 1e-6)); + EXPECT(assert_equal((Vector(2) << 0.359631, 0.640369), actualCvector, 1e-6)); actualCvector = marginals.marginalProbabilities(Mark); - EXPECT(assert_equal((Vec(2) << 0.48628, 0.51372), actualCvector, 1e-6)); + EXPECT(assert_equal((Vector(2) << 0.48628, 0.51372), actualCvector, 1e-6)); } /* ************************************************************************* */ @@ -186,7 +186,7 @@ TEST_UNSAFE( DiscreteMarginals, truss2 ) { // // solver // Vector actualV = solver.marginalProbabilities(key[j]); -// EXPECT(assert_equal((Vec(2) << F[j], T[j]), actualV)); +// EXPECT(assert_equal((Vector(2) << F[j], T[j]), actualV)); // Marginals vector table; diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 927a90ac2..8b7771410 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -42,17 +42,17 @@ Matrix Cal3Bundler::K() const { /* ************************************************************************* */ Vector Cal3Bundler::k() const { - return (Vec(4) << k1_, k2_, 0, 0); + return (Vector(4) << k1_, k2_, 0, 0); } /* ************************************************************************* */ Vector Cal3Bundler::vector() const { - return (Vec(3) << f_, k1_, k2_); + return (Vector(3) << f_, k1_, k2_); } /* ************************************************************************* */ void Cal3Bundler::print(const std::string& s) const { - gtsam::print((Vector)(Vec(5) << f_, k1_, k2_, u0_, v0_), s + ".K"); + gtsam::print((Vector)(Vector(5) << f_, k1_, k2_, u0_, v0_), s + ".K"); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index c286db17d..0d263b625 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -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 (Mat(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); } +Matrix Cal3DS2::K() const { return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); } /* ************************************************************************* */ -Vector Cal3DS2::vector() const { return (Vec(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); } +Vector Cal3DS2::vector() const { return (Vector(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 = (Mat(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr, + *H1 = (Matrix(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 = (Mat(2, 2) << fx, s_, 0.0, fy); - Matrix DR = (Mat(2, 2) << g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy, + 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, 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 = (Mat(2, 2) << fx_, s_, 0.0, fy_); - Matrix DR = (Mat(2, 2) << g + x*dg_dx + dDx_dx, x*dg_dy + dDx_dy, + 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, 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 (Mat(2, 9) << + return (Matrix(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) ); } diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index 6f009fceb..1fb7f4069 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -76,9 +76,9 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, boost::optional Dp) const { const double x = p.x(), y = p.y(); if (Dcal) - *Dcal = (Mat(2, 5) << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0); + *Dcal = (Matrix(2, 5) << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0); if (Dp) - *Dp = (Mat(2, 2) << fx_, s_, 0.000, fy_); + *Dp = (Matrix(2, 2) << fx_, s_, 0.000, fy_); return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 6adc35d0f..bb3327afc 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -126,7 +126,7 @@ public: /// return calibration matrix K Matrix K() const { - return (Mat(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); + return (Matrix(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 (Mat(3, 3) << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, + return (Matrix(3, 3) << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0); } diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index e3838f50f..35079827b 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -36,7 +36,7 @@ Point2 CalibratedCamera::project_to_camera(const Point3& P, boost::optional H1) { if (H1) { double d = 1.0 / P.z(), d2 = d * d; - *H1 = (Mat(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); + *H1 = (Matrix(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 = (Mat(2, 6) << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), + *Dpose = (Matrix(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 - * (Mat(2, 3) << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), + * (Matrix(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)); } diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 44d7b6e32..c162171c0 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -273,7 +273,7 @@ public: boost::optional Dpoint = boost::none) { if (Dpoint) { double d = 1.0 / P.z(), d2 = d * d; - *Dpoint = (Mat(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); + *Dpoint = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); } return Point2(P.x() / P.z(), P.y() / P.z()); } diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 71956854f..6fc9330ad 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -27,7 +27,7 @@ namespace gtsam { /** Explicit instantiation of base class to export members */ INSTANTIATE_LIE(Point2); -static const Matrix oneOne = (Mat(1, 2) << 1.0, 1.0); +static const Matrix oneOne = (Matrix(1, 2) << 1.0, 1.0); /* ************************************************************************* */ void Point2::print(const string& s) const { @@ -45,7 +45,7 @@ double Point2::norm(boost::optional H) const { if (H) { Matrix D_result_d; if (fabs(r) > 1e-10) - D_result_d = (Mat(1, 2) << x_ / r, y_ / r); + D_result_d = (Matrix(1, 2) << x_ / r, y_ / r); else D_result_d = oneOne; // TODO: really infinity, why 1 here?? *H = D_result_d; diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index a4a5e8769..30a4434fe 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -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 = (Mat(3, 1) << t_.x(), t_.y(), 1.0); + Matrix T = (Matrix(3, 1) << t_.x(), t_.y(), 1.0); return collect(2, &R, &T); } @@ -75,13 +75,13 @@ Vector Pose2::Logmap(const Pose2& p) { const Point2& t = p.t(); double w = R.theta(); if (std::abs(w) < 1e-10) - return (Vec(3) << t.x(), t.y(), w); + return (Vector(3) << t.x(), t.y(), w); else { double c_1 = R.c()-1.0, s = R.s(); double det = c_1*c_1 + s*s; Point2 p = R_PI_2 * (R.unrotate(t) - t); Point2 v = (w/det) * p; - return (Vec(3) << v.x(), v.y(), w); + return (Vector(3) << v.x(), v.y(), w); } } @@ -101,7 +101,7 @@ Vector Pose2::localCoordinates(const Pose2& p2) const { return Logmap(between(p2)); #else Pose2 r = between(p2); - return (Vec(3) << r.x(), r.y(), r.theta()); + return (Vector(3) << r.x(), r.y(), r.theta()); #endif } @@ -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 (Mat(3, 3) << + return (Matrix(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 = (Mat(2, 3) << + if (H1) *H1 = (Matrix(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 = (Mat(2, 1) << -q.y(), q.x()); + const Matrix Drotate1 = (Matrix(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 H1, if (H1) { double dt1 = -s2 * x + c2 * y; double dt2 = -c2 * x - s2 * y; - *H1 = (Mat(3, 3) << + *H1 = (Matrix(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 * (Mat(2, 3) << + if (H1) *H1 = H * (Matrix(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 * (Mat(2, 3) << + if (H1) *H1 = H * (Matrix(2, 3) << -r_.c(), r_.s(), 0.0, -r_.s(), -r_.c(), 0.0); - if (H2) *H2 = H * (Mat(2, 3) << + if (H2) *H2 = H * (Matrix(2, 3) << pose2.r_.c(), -pose2.r_.s(), 0.0, pose2.r_.s(), pose2.r_.c(), 0.0); return r; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 037dc1cf1..26244877b 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -172,7 +172,7 @@ public: * @return xihat, 3*3 element of Lie algebra that can be exponentiated */ static inline Matrix wedge(double vx, double vy, double w) { - return (Mat(3,3) << + return (Matrix(3,3) << 0.,-w, vx, w, 0., vy, 0., 0., 0.); diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 90fcd4726..adbe763b3 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -306,7 +306,7 @@ double Pose3::range(const Point3& point, boost::optional H1, Point3 d = transform_to(point, H1, H2); double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt( d2); - Matrix D_result_d = (Mat(1, 3) << x / n, y / n, z / n); + Matrix D_result_d = (Matrix(1, 3) << x / n, y / n, z / n); if (H1) *H1 = D_result_d * (*H1); if (H2) diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 4850d53b6..c4ed4a0cb 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -218,7 +218,7 @@ public: * @return xihat, 4*4 element of Lie algebra that can be exponentiated */ static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) { - return (Mat(4,4) << + return (Matrix(4,4) << 0.,-wz, wy, vx, wz, 0.,-wx, vy, -wy, wx, 0., vz, diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 2e66c5e6d..27f6f9cd8 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -65,12 +65,12 @@ Rot2& Rot2::normalize() { /* ************************************************************************* */ Matrix Rot2::matrix() const { - return (Mat(2, 2) << c_, -s_, s_, c_); + return (Matrix(2, 2) << c_, -s_, s_, c_); } /* ************************************************************************* */ Matrix Rot2::transpose() const { - return (Mat(2, 2) << c_, s_, -s_, c_); + return (Matrix(2, 2) << c_, s_, -s_, c_); } /* ************************************************************************* */ @@ -78,7 +78,7 @@ Matrix Rot2::transpose() const { Point2 Rot2::rotate(const Point2& p, boost::optional H1, boost::optional H2) const { const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y()); - if (H1) *H1 = (Mat(2, 1) << -q.y(), q.x()); + if (H1) *H1 = (Matrix(2, 1) << -q.y(), q.x()); if (H2) *H2 = matrix(); return q; } @@ -88,7 +88,7 @@ Point2 Rot2::rotate(const Point2& p, boost::optional H1, Point2 Rot2::unrotate(const Point2& p, boost::optional H1, boost::optional H2) const { const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y()); - if (H1) *H1 = (Mat(2, 1) << q.y(), -q.x()); // R_{pi/2}q + if (H1) *H1 = (Matrix(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 H) { double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); if(fabs(n) > 1e-5) { - if (H) *H = (Mat(1, 2) << -y / d2, x / d2); + if (H) *H = (Matrix(1, 2) << -y / d2, x / d2); return Rot2::fromCosSin(x / n, y / n); } else { - if (H) *H = (Mat(1, 2) << 0.0, 0.0); + if (H) *H = (Matrix(1, 2) << 0.0, 0.0); return Rot2(); } } diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 55b93c399..ed531a2bd 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -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 = (Mat(3, 6) << + *H1 = (Matrix(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 * (Mat(3, 3) << + *H2 = d * (Matrix(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 (Mat(3, 3) << + return (Matrix(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() diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index d112889af..de5ca1ed1 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -27,7 +27,7 @@ using namespace gtsam; GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera) -static const Pose3 pose1((Matrix)(Mat(3,3) << +static const Pose3 pose1((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 8b69316c4..594db159b 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -29,7 +29,7 @@ using namespace gtsam; static const Cal3_S2 K(625, 625, 0, 0, 0); -static const Pose3 pose1((Matrix)(Mat(3,3) << +static const Pose3 pose1((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 39042c800..66ee5a387 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -46,8 +46,8 @@ TEST(Point2, Lie) { EXPECT(assert_equal(-eye(2), H1)); EXPECT(assert_equal(eye(2), H2)); - EXPECT(assert_equal(Point2(5,7), p1.retract((Vec(2) << 4., 5.)))); - EXPECT(assert_equal((Vec(2) << 3.,3.), p1.localCoordinates(p2))); + EXPECT(assert_equal(Point2(5,7), p1.retract((Vector(2) << 4., 5.)))); + EXPECT(assert_equal((Vector(2) << 3.,3.), p1.localCoordinates(p2))); } /* ************************************************************************* */ @@ -101,7 +101,7 @@ TEST( Point2, norm ) { // exception, for (0,0) derivative is [Inf,Inf] but we return [1,1] actual = x1.norm(actualH); EXPECT_DOUBLES_EQUAL(0, actual, 1e-9); - expectedH = (Mat(1, 2) << 1.0, 1.0); + expectedH = (Matrix(1, 2) << 1.0, 1.0); EXPECT(assert_equal(expectedH,actualH)); actual = x2.norm(actualH); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 60335aeab..ef4a3894c 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -39,7 +39,7 @@ TEST(Point3, Lie) { EXPECT(assert_equal(-eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(5,7,9), p1.retract((Vec(3) << 4., 5., 6.)))); + 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))); } diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 3d26303cd..a5646f647 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -68,7 +68,7 @@ TEST(Pose2, retract) { #else Pose2 expected(M_PI/2.0+0.99, Point2(1.015, 2.01)); #endif - Pose2 actual = pose.retract((Vec(3) << 0.01, -0.015, 0.99)); + Pose2 actual = pose.retract((Vector(3) << 0.01, -0.015, 0.99)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -76,7 +76,7 @@ TEST(Pose2, retract) { TEST(Pose2, expmap) { Pose2 pose(M_PI/2.0, Point2(1, 2)); Pose2 expected(1.00811, 2.01528, 2.5608); - Pose2 actual = expmap_default(pose, (Vec(3) << 0.01, -0.015, 0.99)); + Pose2 actual = expmap_default(pose, (Vector(3) << 0.01, -0.015, 0.99)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -84,7 +84,7 @@ TEST(Pose2, expmap) { TEST(Pose2, expmap2) { Pose2 pose(M_PI/2.0, Point2(1, 2)); Pose2 expected(1.00811, 2.01528, 2.5608); - Pose2 actual = expmap_default(pose, (Vec(3) << 0.01, -0.015, 0.99)); + Pose2 actual = expmap_default(pose, (Vector(3) << 0.01, -0.015, 0.99)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -92,14 +92,14 @@ 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 = (Mat(3,3) << + Matrix A = (Matrix(3,3) << 0.0, -0.99, 0.01, 0.99, 0.0, -0.015, 0.0, 0.0, 0.0); Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0; Matrix expected = eye(3) + A + A2 + A3 + A4; - Vector v = (Vec(3) << 0.01, -0.015, 0.99); + Vector v = (Vector(3) << 0.01, -0.015, 0.99); Pose2 pose = Pose2::Expmap(v); Pose2 pose2(v); EXPECT(assert_equal(pose, pose2)); @@ -110,7 +110,7 @@ TEST(Pose2, expmap3) { /* ************************************************************************* */ TEST(Pose2, expmap0a) { Pose2 expected(0.0101345, -0.0149092, 0.018); - Pose2 actual = Pose2::Expmap((Vec(3) << 0.01, -0.015, 0.018)); + Pose2 actual = Pose2::Expmap((Vector(3) << 0.01, -0.015, 0.018)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -118,7 +118,7 @@ TEST(Pose2, expmap0a) { TEST(Pose2, expmap0b) { // a quarter turn Pose2 expected(1.0, 1.0, M_PI/2); - Pose2 actual = Pose2::Expmap((Vec(3) << M_PI/2, 0.0, M_PI/2)); + Pose2 actual = Pose2::Expmap((Vector(3) << M_PI/2, 0.0, M_PI/2)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -126,7 +126,7 @@ TEST(Pose2, expmap0b) { TEST(Pose2, expmap0c) { // a half turn Pose2 expected(0.0, 2.0, M_PI); - Pose2 actual = Pose2::Expmap((Vec(3) << M_PI, 0.0, M_PI)); + Pose2 actual = Pose2::Expmap((Vector(3) << M_PI, 0.0, M_PI)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -134,7 +134,7 @@ TEST(Pose2, expmap0c) { TEST(Pose2, expmap0d) { // a full turn Pose2 expected(0, 0, 0); - Pose2 actual = Pose2::Expmap((Vec(3) << 2*M_PI, 0.0, 2*M_PI)); + Pose2 actual = Pose2::Expmap((Vector(3) << 2*M_PI, 0.0, 2*M_PI)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -143,7 +143,7 @@ TEST(Pose2, expmap0d) { // test case for screw motion in the plane namespace screw { double w=0.3; - Vector xi = (Vec(3) << 0.0, w, w); + Vector xi = (Vector(3) << 0.0, w, w); Rot2 expectedR = Rot2::fromAngle(w); Point2 expectedT(-0.0446635, 0.29552); Pose2 expected(expectedR, expectedT); @@ -161,7 +161,7 @@ TEST(Pose3, expmap_c) TEST(Pose2, expmap_c_full) { double w=0.3; - Vector xi = (Vec(3) << 0.0, w, w); + Vector xi = (Vector(3) << 0.0, w, w); Rot2 expectedR = Rot2::fromAngle(w); Point2 expectedT(-0.0446635, 0.29552); Pose2 expected(expectedR, expectedT); @@ -175,9 +175,9 @@ TEST(Pose2, logmap) { Pose2 pose0(M_PI/2.0, Point2(1, 2)); Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01)); #ifdef SLOW_BUT_CORRECT_EXPMAP - Vector expected = (Vec(3) << 0.00986473, -0.0150896, 0.018); + Vector expected = (Vector(3) << 0.00986473, -0.0150896, 0.018); #else - Vector expected = (Vec(3) << 0.01, -0.015, 0.018); + Vector expected = (Vector(3) << 0.01, -0.015, 0.018); #endif Vector actual = pose0.localCoordinates(pose); EXPECT(assert_equal(expected, actual, 1e-5)); @@ -187,7 +187,7 @@ TEST(Pose2, logmap) { TEST(Pose2, logmap_full) { Pose2 pose0(M_PI/2.0, Point2(1, 2)); Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01)); - Vector expected = (Vec(3) << 0.00986473, -0.0150896, 0.018); + Vector expected = (Vector(3) << 0.00986473, -0.0150896, 0.018); Vector actual = logmap_default(pose0, pose); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -204,8 +204,8 @@ TEST( Pose2, transform_to ) // expected Point2 expected(2,2); - 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); + 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); // actual Matrix actualH1, actualH2; @@ -236,8 +236,8 @@ TEST (Pose2, transform_from) Point2 expected(0., 2.); EXPECT(assert_equal(expected, actual)); - Matrix H1_expected = (Mat(2, 3) << 0., -1., -2., 1., 0., -1.); - Matrix H2_expected = (Mat(2, 2) << 0., -1., 1., 0.); + Matrix H1_expected = (Matrix(2, 3) << 0., -1., -2., 1., 0., -1.); + Matrix H2_expected = (Matrix(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 = (Mat(3,3) << + Matrix expectedH1 = (Matrix(3,3) << 0.0, 1.0, 0.0, -1.0, 0.0, 2.0, 0.0, 0.0, 1.0 @@ -348,14 +348,14 @@ TEST(Pose2, inverse ) namespace { /* ************************************************************************* */ Vector homogeneous(const Point2& p) { - return (Vec(3) << p.x(), p.y(), 1.0); + return (Vector(3) << p.x(), p.y(), 1.0); } /* ************************************************************************* */ Matrix matrix(const Pose2& gTl) { Matrix gRl = gTl.r().matrix(); Point2 gt = gTl.t(); - return (Mat(3, 3) << + return (Matrix(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((Mat(3,3) << + EXPECT(assert_equal((Matrix(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((Mat(2,2) << + EXPECT(assert_equal((Matrix(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((Mat(3,3) << + EXPECT(assert_equal((Matrix(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 = (Mat(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 @@ -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 = (Mat(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 diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 9d10e2865..175a11ff1 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -104,7 +104,7 @@ TEST( Pose3, expmap_a_full2) TEST(Pose3, expmap_b) { Pose3 p1(Rot3(), Point3(100, 0, 0)); - Pose3 p2 = p1.retract((Vec(6) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0)); + Pose3 p2 = p1.retract((Vector(6) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0)); Pose3 expected(Rot3::rodriguez(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0)); EXPECT(assert_equal(expected, p2,1e-2)); } @@ -113,7 +113,7 @@ TEST(Pose3, expmap_b) // test case for screw motion in the plane namespace screw { double a=0.3, c=cos(a), s=sin(a), w=0.3; - Vector xi = (Vec(6) << 0.0, 0.0, w, w, 0.0, 1.0); + Vector xi = (Vector(6) << 0.0, 0.0, w, w, 0.0, 1.0); Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1); Point3 expectedT(0.29552, 0.0446635, 1); Pose3 expected(expectedR, expectedT); @@ -163,13 +163,13 @@ Pose3 Agrawal06iros(const Vector& xi) { TEST(Pose3, expmaps_galore_full) { Vector xi; Pose3 actual; - xi = (Vec(6) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6); + xi = (Vector(6) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6); actual = Pose3::Expmap(xi); EXPECT(assert_equal(expm(xi), actual,1e-6)); EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6)); - xi = (Vec(6) << 0.1, -0.2, 0.3, -0.4, 0.5, -0.6); + xi = (Vector(6) << 0.1, -0.2, 0.3, -0.4, 0.5, -0.6); for (double theta=1.0;0.3*theta<=M_PI;theta*=2) { Vector txi = xi*theta; actual = Pose3::Expmap(txi); @@ -181,7 +181,7 @@ TEST(Pose3, expmaps_galore_full) } // Works with large v as well, but expm needs 10 iterations! - xi = (Vec(6) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0); + xi = (Vector(6) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0); actual = Pose3::Expmap(xi); EXPECT(assert_equal(expm(xi,10), actual,1e-5)); EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); @@ -194,7 +194,7 @@ TEST(Pose3, Adjoint_compose_full) // To debug derivatives of compose, assert that // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 const Pose3& T1 = T; - Vector x = (Vec(6) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8); + Vector x = (Vector(6) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8); Pose3 expected = T1 * Pose3::Expmap(x) * T2; Vector y = T2.inverse().Adjoint(x); Pose3 actual = T1 * T2 * Pose3::Expmap(y); @@ -510,7 +510,7 @@ TEST(Pose3, subgroups) { // Frank - Below only works for correct "Agrawal06iros style expmap // lines in canonical coordinates correspond to Abelian subgroups in SE(3) - Vector d = (Vec(6) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6); + Vector d = (Vector(6) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6); // exp(-d)=inverse(exp(d)) EXPECT(assert_equal(Pose3::Expmap(-d),Pose3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) @@ -675,7 +675,7 @@ TEST(Pose3, align_2) { /// exp(xi) exp(y) = exp(xi + x) /// Hence, y = log (exp(-xi)*exp(xi+x)) -Vector xi = (Vec(6) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0); +Vector xi = (Vector(6) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0); Vector testDerivExpmapInv(const LieVector& dxi) { Vector y = Pose3::Logmap(Pose3::Expmap(-xi)*Pose3::Expmap(xi+dxi)); @@ -723,8 +723,8 @@ Vector testDerivAdjointTranspose(const LieVector& xi, const LieVector& v) { } TEST( Pose3, adjointTranspose) { - Vector xi = (Vec(6) << 0.01, 0.02, 0.03, 1.0, 2.0, 3.0); - Vector v = (Vec(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0); + Vector xi = (Vector(6) << 0.01, 0.02, 0.03, 1.0, 2.0, 3.0); + Vector v = (Vector(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0); Vector expected = testDerivAdjointTranspose(xi, v); Matrix actualH; diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 38c8a58b0..7b80e8ee9 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -57,7 +57,7 @@ TEST( Rot3, constructor) /* ************************************************************************* */ TEST( Rot3, constructor2) { - Matrix R = (Mat(3, 3) << 11., 12., 13., 21., 22., 23., 31., 32., 33.); + Matrix R = (Matrix(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)); @@ -101,7 +101,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) { TEST( Rot3, rodriguez) { Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0); - Vector w = (Vec(3) << epsilon, 0., 0.); + Vector w = (Vector(3) << epsilon, 0., 0.); Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); } @@ -109,7 +109,7 @@ TEST( Rot3, rodriguez) /* ************************************************************************* */ TEST( Rot3, rodriguez2) { - Vector axis = (Vec(3) << 0., 1., 0.); // rotation around Y + Vector axis = (Vector(3) << 0., 1., 0.); // rotation around Y double angle = 3.14 / 4.0; Rot3 actual = Rot3::rodriguez(axis, angle); Rot3 expected(0.707388, 0, 0.706825, @@ -121,7 +121,7 @@ TEST( Rot3, rodriguez2) /* ************************************************************************* */ TEST( Rot3, rodriguez3) { - Vector w = (Vec(3) << 0.1, 0.2, 0.3); + Vector w = (Vector(3) << 0.1, 0.2, 0.3); Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w)); Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); @@ -130,7 +130,7 @@ TEST( Rot3, rodriguez3) /* ************************************************************************* */ TEST( Rot3, rodriguez4) { - Vector axis = (Vec(3) << 0., 0., 1.); // rotation around Z + Vector axis = (Vector(3) << 0., 0., 1.); // rotation around Z double angle = M_PI/2.0; Rot3 actual = Rot3::rodriguez(axis, angle); double c=cos(angle),s=sin(angle); @@ -156,7 +156,7 @@ TEST(Rot3, log) Rot3 R; #define CHECK_OMEGA(X,Y,Z) \ - w = (Vec(3) << (double)X, (double)Y, double(Z)); \ + w = (Vector(3) << (double)X, (double)Y, double(Z)); \ R = Rot3::rodriguez(w); \ EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12)); @@ -190,7 +190,7 @@ TEST(Rot3, log) // Check 360 degree rotations #define CHECK_OMEGA_ZERO(X,Y,Z) \ - w = (Vec(3) << (double)X, (double)Y, double(Z)); \ + w = (Vector(3) << (double)X, (double)Y, double(Z)); \ R = Rot3::rodriguez(w); \ EXPECT(assert_equal(zero(3), Rot3::Logmap(R))); @@ -217,7 +217,7 @@ TEST(Rot3, manifold_caley) CHECK(assert_equal(d12,-d21)); // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = (Vec(3) << 0.1, 0.2, 0.3); + Vector d = (Vector(3) << 0.1, 0.2, 0.3); // exp(-d)=inverse(exp(d)) CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) @@ -245,7 +245,7 @@ TEST(Rot3, manifold_slow_caley) CHECK(assert_equal(d12,-d21)); // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = (Vec(3) << 0.1, 0.2, 0.3); + Vector d = (Vector(3) << 0.1, 0.2, 0.3); // exp(-d)=inverse(exp(d)) CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) @@ -277,7 +277,7 @@ TEST(Rot3, manifold_expmap) CHECK(assert_equal(d12,-d21)); // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = (Vec(3) << 0.1, 0.2, 0.3); + Vector d = (Vector(3) << 0.1, 0.2, 0.3); // exp(-d)=inverse(exp(d)) CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) @@ -403,7 +403,7 @@ TEST( Rot3, between ) } /* ************************************************************************* */ -Vector w = (Vec(3) << 0.1, 0.27, -0.2); +Vector w = (Vector(3) << 0.1, 0.27, -0.2); // Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes? // We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 @@ -473,7 +473,7 @@ TEST( Rot3, yaw_pitch_roll ) Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3); CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3))); - CHECK(assert_equal((Vector)(Vec(3) << 0.1, 0.2, 0.3),expected.ypr())); + CHECK(assert_equal((Vector)(Vector(3) << 0.1, 0.2, 0.3),expected.ypr())); } /* ************************************************************************* */ @@ -483,25 +483,25 @@ TEST( Rot3, RQ) Matrix actualK; Vector actual; boost::tie(actualK, actual) = RQ(R.matrix()); - Vector expected = (Vec(3) << 0.14715, 0.385821, 0.231671); + Vector expected = (Vector(3) << 0.14715, 0.385821, 0.231671); CHECK(assert_equal(I3,actualK)); CHECK(assert_equal(expected,actual,1e-6)); // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] CHECK(assert_equal(expected,R.xyz(),1e-6)); - CHECK(assert_equal((Vector)(Vec(3) << 0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); + CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r] - CHECK(assert_equal((Vector)(Vec(3) << 0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); - CHECK(assert_equal((Vector)(Vec(3) << 0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); + CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); + CHECK(assert_equal((Vector)(Vector(3) << 0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); // Try ypr for pure yaw-pitch-roll matrices - CHECK(assert_equal((Vector)(Vec(3) << 0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); - CHECK(assert_equal((Vector)(Vec(3) << 0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); - CHECK(assert_equal((Vector)(Vec(3) << 0.0,0.0,0.1),Rot3::roll (0.1).ypr())); + CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); + CHECK(assert_equal((Vector)(Vector(3) << 0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); + CHECK(assert_equal((Vector)(Vector(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 = (Mat(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0); + Matrix K = (Matrix(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)); @@ -510,11 +510,11 @@ TEST( Rot3, RQ) /* ************************************************************************* */ TEST( Rot3, expmapStability ) { - Vector w = (Vec(3) << 78e-9, 5e-8, 97e-7); + Vector w = (Vector(3) << 78e-9, 5e-8, 97e-7); double theta = w.norm(); double theta2 = theta*theta; Rot3 actualR = Rot3::Expmap(w); - Matrix W = (Mat(3, 3) << 0.0, -w(2), w(1), + Matrix W = (Matrix(3, 3) << 0.0, -w(2), w(1), w(2), 0.0, -w(0), -w(1), w(0), 0.0 ); Matrix W2 = W*W; @@ -526,7 +526,7 @@ TEST( Rot3, expmapStability ) { /* ************************************************************************* */ TEST( Rot3, logmapStability ) { - Vector w = (Vec(3) << 1e-8, 0.0, 0.0); + Vector w = (Vector(3) << 1e-8, 0.0, 0.0); Rot3 R = Rot3::Expmap(w); // double tr = R.r1().x()+R.r2().y()+R.r3().z(); // std::cout.precision(5000); @@ -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)(Mat(3, 3) << + Rot3 R1 = Rot3((Matrix)(Matrix(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)(Mat(3, 3) << + Rot3 R2 = Rot3((Matrix)(Matrix(3, 3) << -0.207341903877828, 0.250149415542075, 0.945745528564780, 0.881304914479026, -0.371869043667957, 0.291573424846290, 0.424630407073532, 0.893945571198514, -0.143353873763946)); diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp index 1d9f10dc5..990be345f 100644 --- a/gtsam/geometry/tests/testRot3Q.cpp +++ b/gtsam/geometry/tests/testRot3Q.cpp @@ -52,7 +52,7 @@ TEST( Rot3, constructor) /* ************************************************************************* */ TEST( Rot3, constructor2) { - Matrix R = (Mat(3, 3) << 11., 12., 13., 21., 22., 23., 31., 32., 33.); + Matrix R = (Matrix(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)); @@ -88,7 +88,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) { TEST( Rot3, rodriguez) { Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0); - Vector w = (Vec(3) << epsilon, 0., 0.); + Vector w = (Vector(3) << epsilon, 0., 0.); Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); } @@ -96,7 +96,7 @@ TEST( Rot3, rodriguez) /* ************************************************************************* */ TEST( Rot3, rodriguez2) { - Vector axis = (Vec(3) << 0.,1.,0.); // rotation around Y + Vector axis = (Vector(3) << 0.,1.,0.); // rotation around Y double angle = 3.14 / 4.0; Rot3 actual = Rot3::rodriguez(axis, angle); Rot3 expected(0.707388, 0, 0.706825, @@ -108,7 +108,7 @@ TEST( Rot3, rodriguez2) /* ************************************************************************* */ TEST( Rot3, rodriguez3) { - Vector w = (Vec(3) << 0.1, 0.2, 0.3); + Vector w = (Vector(3) << 0.1, 0.2, 0.3); Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w)); Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); @@ -117,7 +117,7 @@ TEST( Rot3, rodriguez3) /* ************************************************************************* */ TEST( Rot3, rodriguez4) { - Vector axis = (Vec(3) << 0., 0., 1.); // rotation around Z + Vector axis = (Vector(3) << 0., 0., 1.); // rotation around Z double angle = M_PI_2; Rot3 actual = Rot3::rodriguez(axis, angle); double c=cos(angle),s=sin(angle); @@ -138,35 +138,35 @@ TEST( Rot3, expmap) /* ************************************************************************* */ TEST(Rot3, log) { - Vector w1 = (Vec(3) << 0.1, 0.0, 0.0); + Vector w1 = (Vector(3) << 0.1, 0.0, 0.0); Rot3 R1 = Rot3::rodriguez(w1); CHECK(assert_equal(w1, Rot3::Logmap(R1))); - Vector w2 = (Vec(3) << 0.0, 0.1, 0.0); + Vector w2 = (Vector(3) << 0.0, 0.1, 0.0); Rot3 R2 = Rot3::rodriguez(w2); CHECK(assert_equal(w2, Rot3::Logmap(R2))); - Vector w3 = (Vec(3) << 0.0, 0.0, 0.1); + Vector w3 = (Vector(3) << 0.0, 0.0, 0.1); Rot3 R3 = Rot3::rodriguez(w3); CHECK(assert_equal(w3, Rot3::Logmap(R3))); - Vector w = (Vec(3) << 0.1, 0.4, 0.2); + Vector w = (Vector(3) << 0.1, 0.4, 0.2); Rot3 R = Rot3::rodriguez(w); CHECK(assert_equal(w, Rot3::Logmap(R))); - Vector w5 = (Vec(3) << 0.0, 0.0, 0.0); + Vector w5 = (Vector(3) << 0.0, 0.0, 0.0); Rot3 R5 = Rot3::rodriguez(w5); CHECK(assert_equal(w5, Rot3::Logmap(R5))); - Vector w6 = (Vec(3) << boost::math::constants::pi(), 0.0, 0.0); + Vector w6 = (Vector(3) << boost::math::constants::pi(), 0.0, 0.0); Rot3 R6 = Rot3::rodriguez(w6); CHECK(assert_equal(w6, Rot3::Logmap(R6))); - Vector w7 = (Vec(3) << 0.0, boost::math::constants::pi(), 0.0); + Vector w7 = (Vector(3) << 0.0, boost::math::constants::pi(), 0.0); Rot3 R7 = Rot3::rodriguez(w7); CHECK(assert_equal(w7, Rot3::Logmap(R7))); - Vector w8 = (Vec(3) << 0.0, 0.0, boost::math::constants::pi()); + Vector w8 = (Vector(3) << 0.0, 0.0, boost::math::constants::pi()); Rot3 R8 = Rot3::rodriguez(w8); CHECK(assert_equal(w8, Rot3::Logmap(R8))); } @@ -190,7 +190,7 @@ TEST(Rot3, manifold) CHECK(assert_equal(d12,-d21)); // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = (Vec(3) << 0.1, 0.2, 0.3); + Vector d = (Vector(3) << 0.1, 0.2, 0.3); // exp(-d)=inverse(exp(d)) CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) @@ -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 = (Mat(3, 3) << + Matrix expectedr1 = (Matrix(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); @@ -381,25 +381,25 @@ TEST( Rot3, RQ) Matrix actualK; Vector actual; boost::tie(actualK, actual) = RQ(R.matrix()); - Vector expected = (Vec(3) << 0.14715, 0.385821, 0.231671); + Vector expected = (Vector(3) << 0.14715, 0.385821, 0.231671); CHECK(assert_equal(eye(3),actualK)); CHECK(assert_equal(expected,actual,1e-6)); // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] CHECK(assert_equal(expected,R.xyz(),1e-6)); - CHECK(assert_equal((Vector)(Vec(3) <<0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); + CHECK(assert_equal((Vector)(Vector(3) <<0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r] - CHECK(assert_equal((Vector)(Vec(3) <<0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); - CHECK(assert_equal((Vector)(Vec(3) <<0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); + CHECK(assert_equal((Vector)(Vector(3) <<0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); + CHECK(assert_equal((Vector)(Vector(3) <<0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); // Try ypr for pure yaw-pitch-roll matrices - CHECK(assert_equal((Vector)(Vec(3) <<0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); - CHECK(assert_equal((Vector)(Vec(3) <<0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); - CHECK(assert_equal((Vector)(Vec(3) <<0.0,0.0,0.1),Rot3::roll (0.1).ypr())); + CHECK(assert_equal((Vector)(Vector(3) <<0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); + CHECK(assert_equal((Vector)(Vector(3) <<0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); + CHECK(assert_equal((Vector)(Vector(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 = (Mat(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0); + Matrix K = (Matrix(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)); @@ -408,11 +408,11 @@ TEST( Rot3, RQ) /* ************************************************************************* */ TEST( Rot3, expmapStability ) { - Vector w = (Vec(3) << 78e-9, 5e-8, 97e-7); + Vector w = (Vector(3) << 78e-9, 5e-8, 97e-7); double theta = w.norm(); double theta2 = theta*theta; Rot3 actualR = Rot3::Expmap(w); - Matrix W = (Mat(3, 3) << 0.0, -w(2), w(1), + Matrix W = (Matrix(3, 3) << 0.0, -w(2), w(1), w(2), 0.0, -w(0), -w(1), w(0), 0.0 ); Matrix W2 = W*W; @@ -425,7 +425,7 @@ TEST( Rot3, expmapStability ) { // Does not work with Quaternions ///* ************************************************************************* */ //TEST( Rot3, logmapStability ) { -// Vector w = (Vec(3) << 1e-8, 0.0, 0.0); +// Vector w = (Vector(3) << 1e-8, 0.0, 0.0); // Rot3 R = Rot3::Expmap(w); //// double tr = R.r1().x()+R.r2().y()+R.r3().z(); //// std::cout.precision(5000); @@ -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((Mat(3, 3) << + Rot3 R1 = Rot3((Matrix(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((Mat(3, 3) << + Rot3 R2 = Rot3((Matrix(3, 3) << -0.207341903877828, 0.250149415542075, 0.945745528564780, 0.881304914479026, -0.371869043667957, 0.291573424846290, 0.424630407073532, 0.893945571198514, -0.143353873763946)); diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 1f35846f8..9ced28dca 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -28,7 +28,7 @@ using namespace gtsam; static const Cal3_S2 K(625, 625, 0, 0, 0); -static const Pose3 pose1((Matrix)(Mat(3,3) << +static const Pose3 pose1((Matrix)(Matrix(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 = (Mat(3,4) << + Matrix P = (Matrix(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); diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index f1947e658..a7a82c9de 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -74,7 +74,7 @@ TEST( StereoCamera, project) /* ************************************************************************* */ -static Pose3 camera1((Matrix)(Mat(3,3) << +static Pose3 camera1((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. diff --git a/gtsam/geometry/tests/testStereoPoint2.cpp b/gtsam/geometry/tests/testStereoPoint2.cpp index 96a950ba5..598febcc8 100644 --- a/gtsam/geometry/tests/testStereoPoint2.cpp +++ b/gtsam/geometry/tests/testStereoPoint2.cpp @@ -44,8 +44,8 @@ TEST(StereoPoint2, Lie) { EXPECT(assert_equal(StereoPoint2(3,3,3), p1.between(p2))); - EXPECT(assert_equal(StereoPoint2(5,7,9), p1.retract((Vec(3) << 4., 5., 6.)))); - EXPECT(assert_equal((Vec(3) << 3., 3., 3.), p1.localCoordinates(p2))); + EXPECT(assert_equal(StereoPoint2(5,7,9), p1.retract((Vector(3) << 4., 5., 6.)))); + EXPECT(assert_equal((Vector(3) << 3., 3., 3.), p1.localCoordinates(p2))); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testErrors.cpp b/gtsam/linear/tests/testErrors.cpp index a627dfe6a..c78382316 100644 --- a/gtsam/linear/tests/testErrors.cpp +++ b/gtsam/linear/tests/testErrors.cpp @@ -29,12 +29,12 @@ using namespace gtsam; TEST( Errors, arithmetic ) { Errors e; - e += (Vec(2) << 1.0,2.0), (Vec(3) << 3.0,4.0,5.0); + e += (Vector(2) << 1.0,2.0), (Vector(3) << 3.0,4.0,5.0); DOUBLES_EQUAL(1+4+9+16+25,dot(e,e),1e-9); axpy(2.0,e,e); Errors expected; - expected += (Vec(2) << 3.0,6.0), (Vec(3) << 9.0,12.0,15.0); + expected += (Vector(2) << 3.0,6.0), (Vector(3) << 9.0,12.0,15.0); CHECK(assert_equal(expected,e)); } diff --git a/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp b/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp index 8764878ee..d87007d0b 100644 --- a/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp +++ b/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp @@ -38,8 +38,8 @@ using namespace gtsam; static const Key _x_=0, _y_=1, _z_=2; static GaussianBayesNet smallBayesNet = list_of - (GaussianConditional(_x_, (Vec(1) << 9.0), (Mat(1, 1) << 1.0), _y_, (Mat(1, 1) << 1.0))) - (GaussianConditional(_y_, (Vec(1) << 5.0), (Mat(1, 1) << 1.0))); + (GaussianConditional(_x_, (Vector(1) << 9.0), (Matrix(1, 1) << 1.0), _y_, (Matrix(1, 1) << 1.0))) + (GaussianConditional(_y_, (Vector(1) << 5.0), (Matrix(1, 1) << 1.0))); /* ************************************************************************* */ TEST( GaussianBayesNet, matrix ) @@ -47,11 +47,11 @@ TEST( GaussianBayesNet, matrix ) Matrix R; Vector d; boost::tie(R,d) = smallBayesNet.matrix(); // find matrix and RHS - Matrix R1 = (Mat(2, 2) << + Matrix R1 = (Matrix(2, 2) << 1.0, 1.0, 0.0, 1.0 ); - Vector d1 = (Vec(2) << 9.0, 5.0); + Vector d1 = (Vector(2) << 9.0, 5.0); EXPECT(assert_equal(R,R1)); EXPECT(assert_equal(d,d1)); @@ -63,8 +63,8 @@ TEST( GaussianBayesNet, optimize ) VectorValues actual = smallBayesNet.optimize(); VectorValues expected = map_list_of - (_x_, (Vec(1) << 4.0)) - (_y_, (Vec(1) << 5.0)); + (_x_, (Vector(1) << 4.0)) + (_y_, (Vector(1) << 5.0)); EXPECT(assert_equal(expected,actual)); } @@ -78,13 +78,13 @@ TEST( GaussianBayesNet, optimize3 ) // NOTE: we are supplying a new RHS here VectorValues expected = map_list_of - (_x_, (Vec(1) << -1.0)) - (_y_, (Vec(1) << 5.0)); + (_x_, (Vector(1) << -1.0)) + (_y_, (Vector(1) << 5.0)); // Test different RHS version VectorValues gx = map_list_of - (_x_, (Vec(1) << 4.0)) - (_y_, (Vec(1) << 5.0)); + (_x_, (Vector(1) << 4.0)) + (_y_, (Vector(1) << 5.0)); VectorValues actual = smallBayesNet.backSubstitute(gx); EXPECT(assert_equal(expected, actual)); } @@ -97,11 +97,11 @@ TEST( GaussianBayesNet, backSubstituteTranspose ) // 5 1 1 3 VectorValues x = map_list_of - (_x_, (Vec(1) << 2.0)) - (_y_, (Vec(1) << 5.0)), + (_x_, (Vector(1) << 2.0)) + (_y_, (Vector(1) << 5.0)), expected = map_list_of - (_x_, (Vec(1) << 2.0)) - (_y_, (Vec(1) << 3.0)); + (_x_, (Vector(1) << 2.0)) + (_y_, (Vector(1) << 3.0)); VectorValues actual = smallBayesNet.backSubstituteTranspose(x); EXPECT(assert_equal(expected, actual)); @@ -113,15 +113,15 @@ TEST( GaussianBayesNet, DeterminantTest ) { GaussianBayesNet cbn; cbn += GaussianConditional( - 0, (Vec(2) << 3.0, 4.0 ), (Mat(2, 2) << 1.0, 3.0, 0.0, 4.0 ), - 1, (Mat(2, 2) << 2.0, 1.0, 2.0, 3.0), noiseModel::Isotropic::Sigma(2, 2.0)); + 0, (Vector(2) << 3.0, 4.0 ), (Matrix(2, 2) << 1.0, 3.0, 0.0, 4.0 ), + 1, (Matrix(2, 2) << 2.0, 1.0, 2.0, 3.0), noiseModel::Isotropic::Sigma(2, 2.0)); cbn += GaussianConditional( - 1, (Vec(2) << 5.0, 6.0 ), (Mat(2, 2) << 1.0, 1.0, 0.0, 3.0 ), - 2, (Mat(2, 2) << 1.0, 0.0, 5.0, 2.0), noiseModel::Isotropic::Sigma(2, 2.0)); + 1, (Vector(2) << 5.0, 6.0 ), (Matrix(2, 2) << 1.0, 1.0, 0.0, 3.0 ), + 2, (Matrix(2, 2) << 1.0, 0.0, 5.0, 2.0), noiseModel::Isotropic::Sigma(2, 2.0)); cbn += GaussianConditional( - 3, (Vec(2) << 7.0, 8.0 ), (Mat(2, 2) << 1.0, 1.0, 0.0, 5.0 ), noiseModel::Isotropic::Sigma(2, 2.0)); + 3, (Vector(2) << 7.0, 8.0 ), (Matrix(2, 2) << 1.0, 1.0, 0.0, 5.0 ), noiseModel::Isotropic::Sigma(2, 2.0)); double expectedDeterminant = 60.0 / 64.0; double actualDeterminant = cbn.determinant(); @@ -144,21 +144,21 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 0, (Vec(2) << 1.0,2.0), (Mat(2, 2) << 3.0,4.0,0.0,6.0), - 3, (Mat(2, 2) << 7.0,8.0,9.0,10.0), - 4, (Mat(2, 2) << 11.0,12.0,13.0,14.0))); + 0, (Vector(2) << 1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0), + 3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0), + 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 1, (Vec(2) << 15.0,16.0), (Mat(2, 2) << 17.0,18.0,0.0,20.0), - 2, (Mat(2, 2) << 21.0,22.0,23.0,24.0), - 4, (Mat(2, 2) << 25.0,26.0,27.0,28.0))); + 1, (Vector(2) << 15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0), + 2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0), + 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 2, (Vec(2) << 29.0,30.0), (Mat(2, 2) << 31.0,32.0,0.0,34.0), - 3, (Mat(2, 2) << 35.0,36.0,37.0,38.0))); + 2, (Vector(2) << 29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0), + 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 3, (Vec(2) << 39.0,40.0), (Mat(2, 2) << 41.0,42.0,0.0,44.0), - 4, (Mat(2, 2) << 45.0,46.0,47.0,48.0))); + 3, (Vector(2) << 39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0), + 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, (Vec(2) << 49.0,50.0), (Mat(2, 2) << 51.0,52.0,0.0,54.0))); + 4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0))); // Compute the Hessian numerically Matrix hessian = numericalHessian( diff --git a/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp b/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp index d7d521ce1..1ac1fbf46 100644 --- a/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp +++ b/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp @@ -38,10 +38,10 @@ namespace { const Key x1=1, x2=2, x3=3, x4=4; const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5); const GaussianFactorGraph chain = list_of - (JacobianFactor(x2, (Mat(1, 1) << 1.), x1, (Mat(1, 1) << 1.), (Vec(1) << 1.), chainNoise)) - (JacobianFactor(x2, (Mat(1, 1) << 1.), x3, (Mat(1, 1) << 1.), (Vec(1) << 1.), chainNoise)) - (JacobianFactor(x3, (Mat(1, 1) << 1.), x4, (Mat(1, 1) << 1.), (Vec(1) << 1.), chainNoise)) - (JacobianFactor(x4, (Mat(1, 1) << 1.), (Vec(1) << 1.), chainNoise)); + (JacobianFactor(x2, (Matrix(1, 1) << 1.), x1, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)) + (JacobianFactor(x2, (Matrix(1, 1) << 1.), x3, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)) + (JacobianFactor(x3, (Matrix(1, 1) << 1.), x4, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)) + (JacobianFactor(x4, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)); const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4)); /* ************************************************************************* */ @@ -84,13 +84,13 @@ TEST( GaussianBayesTree, eliminate ) { GaussianBayesTree bt = *chain.eliminateMultifrontal(chainOrdering); - Matrix two = (Mat(1, 1) << 2.); - Matrix one = (Mat(1, 1) << 1.); + Matrix two = (Matrix(1, 1) << 2.); + Matrix one = (Matrix(1, 1) << 1.); GaussianBayesTree bayesTree_expected; bayesTree_expected.insertRoot( - MakeClique(GaussianConditional(pair_list_of(x3, (Mat(2, 1) << 2., 0.)) (x4, (Mat(2, 1) << 2., 2.)), 2, (Vec(2) << 2., 2.)), list_of - (MakeClique(GaussianConditional(pair_list_of(x2, (Mat(2, 1) << -2.*sqrt(2.), 0.)) (x1, (Mat(2, 1) << -sqrt(2.), -sqrt(2.))) (x3, (Mat(2, 1) << -sqrt(2.), sqrt(2.))), 2, (Vec(2) << -2.*sqrt(2.), 0.)))))); + MakeClique(GaussianConditional(pair_list_of(x3, (Matrix(2, 1) << 2., 0.)) (x4, (Matrix(2, 1) << 2., 2.)), 2, (Vector(2) << 2., 2.)), list_of + (MakeClique(GaussianConditional(pair_list_of(x2, (Matrix(2, 1) << -2.*sqrt(2.), 0.)) (x1, (Matrix(2, 1) << -sqrt(2.), -sqrt(2.))) (x3, (Matrix(2, 1) << -sqrt(2.), sqrt(2.))), 2, (Vector(2) << -2.*sqrt(2.), 0.)))))); EXPECT(assert_equal(bayesTree_expected, bt)); } @@ -99,10 +99,10 @@ TEST( GaussianBayesTree, eliminate ) TEST( GaussianBayesTree, optimizeMultiFrontal ) { VectorValues expected = pair_list_of - (x1, (Vec(1) << 0.)) - (x2, (Vec(1) << 1.)) - (x3, (Vec(1) << 0.)) - (x4, (Vec(1) << 1.)); + (x1, (Vector(1) << 0.)) + (x2, (Vector(1) << 1.)) + (x3, (Vector(1) << 0.)) + (x4, (Vector(1) << 1.)); VectorValues actual = chain.eliminateMultifrontal(chainOrdering)->optimize(); EXPECT(assert_equal(expected,actual)); @@ -191,56 +191,56 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { GaussianBayesTree bt; bt.insertRoot(MakeClique(GaussianConditional( pair_list_of - (2, (Mat(6, 2) << + (2, (Matrix(6, 2) << 31.0,32.0, 0.0,34.0, 0.0,0.0, 0.0,0.0, 0.0,0.0, 0.0,0.0)) - (3, (Mat(6, 2) << + (3, (Matrix(6, 2) << 35.0,36.0, 37.0,38.0, 41.0,42.0, 0.0,44.0, 0.0,0.0, 0.0,0.0)) - (4, (Mat(6, 2) << + (4, (Matrix(6, 2) << 0.0,0.0, 0.0,0.0, 45.0,46.0, 47.0,48.0, 51.0,52.0, 0.0,54.0)), - 3, (Vec(6) << 29.0,30.0,39.0,40.0,49.0,50.0)), list_of + 3, (Vector(6) << 29.0,30.0,39.0,40.0,49.0,50.0)), list_of (MakeClique(GaussianConditional( pair_list_of - (0, (Mat(4, 2) << + (0, (Matrix(4, 2) << 3.0,4.0, 0.0,6.0, 0.0,0.0, 0.0,0.0)) - (1, (Mat(4, 2) << + (1, (Matrix(4, 2) << 0.0,0.0, 0.0,0.0, 17.0,18.0, 0.0,20.0)) - (2, (Mat(4, 2) << + (2, (Matrix(4, 2) << 0.0,0.0, 0.0,0.0, 21.0,22.0, 23.0,24.0)) - (3, (Mat(4, 2) << + (3, (Matrix(4, 2) << 7.0,8.0, 9.0,10.0, 0.0,0.0, 0.0,0.0)) - (4, (Mat(4, 2) << + (4, (Matrix(4, 2) << 11.0,12.0, 13.0,14.0, 25.0,26.0, 27.0,28.0)), - 2, (Vec(4) << 1.0,2.0,15.0,16.0)))))); + 2, (Vector(4) << 1.0,2.0,15.0,16.0)))))); // Compute the Hessian numerically Matrix hessian = numericalHessian( @@ -264,11 +264,11 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { // Known steepest descent point from Bayes' net version VectorValues expectedFromBN = pair_list_of - (0, (Vec(2) << 0.000129034, 0.000688183)) - (1, (Vec(2) << 0.0109679, 0.0253767)) - (2, (Vec(2) << 0.0680441, 0.114496)) - (3, (Vec(2) << 0.16125, 0.241294)) - (4, (Vec(2) << 0.300134, 0.423233)); + (0, (Vector(2) << 0.000129034, 0.000688183)) + (1, (Vector(2) << 0.0109679, 0.0253767)) + (2, (Vector(2) << 0.0680441, 0.114496)) + (3, (Vector(2) << 0.16125, 0.241294)) + (4, (Vector(2) << 0.300134, 0.423233)); // Compute the steepest descent point with the dogleg function VectorValues actual = bt.optimizeGradientSearch(); diff --git a/gtsam/linear/tests/testGaussianConditionalUnordered.cpp b/gtsam/linear/tests/testGaussianConditionalUnordered.cpp index 85bf9595c..6bb8a05e1 100644 --- a/gtsam/linear/tests/testGaussianConditionalUnordered.cpp +++ b/gtsam/linear/tests/testGaussianConditionalUnordered.cpp @@ -39,25 +39,25 @@ using namespace boost::assign; static const double tol = 1e-5; -static Matrix R = (Mat(2, 2) << +static Matrix R = (Matrix(2, 2) << -12.1244, -5.1962, 0., 4.6904); /* ************************************************************************* */ TEST(GaussianConditional, constructor) { - Matrix S1 = (Mat(2, 2) << + Matrix S1 = (Matrix(2, 2) << -5.2786, -8.6603, 5.0254, 5.5432); - Matrix S2 = (Mat(2, 2) << + Matrix S2 = (Matrix(2, 2) << -10.5573, -5.9385, 5.5737, 3.0153); - Matrix S3 = (Mat(2, 2) << + Matrix S3 = (Matrix(2, 2) << -11.3820, -7.2581, -3.0153, -3.5635); - Vector d = (Vec(2) << 1.0, 2.0); - SharedDiagonal s = noiseModel::Diagonal::Sigmas((Vec(2) << 3.0, 4.0)); + Vector d = (Vector(2) << 1.0, 2.0); + SharedDiagonal s = noiseModel::Diagonal::Sigmas((Vector(2) << 3.0, 4.0)); vector > terms = pair_list_of (1, R) @@ -114,9 +114,9 @@ TEST( GaussianConditional, equals ) R(0,0) = 0.1 ; R(1,0) = 0.3; R(0,1) = 0.0 ; R(1,1) = 0.34; - SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vec(2) << 1.0, 0.34)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 0.34)); - Vector d = (Vec(2) << 0.2, 0.5); + Vector d = (Vector(2) << 0.2, 0.5); GaussianConditional expected(1, d, R, 2, A1, 10, A2, model), @@ -133,13 +133,13 @@ TEST( GaussianConditional, solve ) expectedX(0) = 20-3-11 ; expectedX(1) = 40-7-15; // create a conditional Gaussian node - Matrix R = (Mat(2, 2) << 1., 0., + Matrix R = (Matrix(2, 2) << 1., 0., 0., 1.); - Matrix A1 = (Mat(2, 2) << 1., 2., + Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.); - Matrix A2 = (Mat(2, 2) << 5., 6., + Matrix A2 = (Matrix(2, 2) << 5., 6., 7., 8.); Vector d(2); d << 20.0, 40.0; @@ -177,7 +177,7 @@ TEST( GaussianConditional, solve_simple ) GaussianConditional cg(list_of(1)(2), 1, blockMatrix); // partial solution - Vector sx1 = (Vec(2) << 9.0, 10.0); + Vector sx1 = (Vector(2) << 9.0, 10.0); // elimination order: 1, 2 VectorValues actual = map_list_of @@ -185,7 +185,7 @@ TEST( GaussianConditional, solve_simple ) VectorValues expected = map_list_of (2, sx1) - (1, (Vec(4) << -3.1,-3.4,-11.9,-13.2)); + (1, (Vector(4) << -3.1,-3.4,-11.9,-13.2)); // verify indices/size EXPECT_LONGS_EQUAL(2, (long)cg.size()); @@ -213,15 +213,15 @@ TEST( GaussianConditional, solve_multifrontal ) EXPECT(assert_equal(Vector(blockMatrix.full().rightCols(1)), cg.get_d())); // partial solution - Vector sl1 = (Vec(2) << 9.0, 10.0); + Vector sl1 = (Vector(2) << 9.0, 10.0); // elimination order; _x_, _x1_, _l1_ VectorValues actual = map_list_of (10, sl1); // parent VectorValues expected = map_list_of - (1, (Vector)(Vec(2) << -3.1,-3.4)) - (2, (Vector)(Vec(2) << -11.9,-13.2)) + (1, (Vector)(Vector(2) << -3.1,-3.4)) + (2, (Vector)(Vector(2) << -11.9,-13.2)) (10, sl1); // verify indices/size @@ -241,8 +241,8 @@ TEST( GaussianConditional, solveTranspose ) { * 1 1 9 * 1 5 */ - Matrix R11 = (Mat(1, 1) << 1.0), S12 = (Mat(1, 1) << 1.0); - Matrix R22 = (Mat(1, 1) << 1.0); + Matrix R11 = (Matrix(1, 1) << 1.0), S12 = (Matrix(1, 1) << 1.0); + Matrix R22 = (Matrix(1, 1) << 1.0); Vector d1(1), d2(1); d1(0) = 9; d2(0) = 5; @@ -258,11 +258,11 @@ TEST( GaussianConditional, solveTranspose ) { VectorValues x = map_list_of - (1, (Vec(1) << 2.)) - (2, (Vec(1) << 5.)), + (1, (Vector(1) << 2.)) + (2, (Vector(1) << 5.)), y = map_list_of - (1, (Vec(1) << 2.)) - (2, (Vec(1) << 3.)); + (1, (Vector(1) << 2.)) + (2, (Vector(1) << 3.)); // test functional version VectorValues actual = cbn.backSubstituteTranspose(x); diff --git a/gtsam/linear/tests/testGaussianDensity.cpp b/gtsam/linear/tests/testGaussianDensity.cpp index f1dbacfdd..f87862399 100644 --- a/gtsam/linear/tests/testGaussianDensity.cpp +++ b/gtsam/linear/tests/testGaussianDensity.cpp @@ -25,11 +25,11 @@ using namespace std; /* ************************************************************************* */ TEST(GaussianDensity, constructor) { - Matrix R = (Mat(2,2) << + Matrix R = (Matrix(2,2) << -12.1244, -5.1962, 0., 4.6904); - Vector d = (Vec(2) << 1.0, 2.0), s = (Vec(2) << 3.0, 4.0); + Vector d = (Vector(2) << 1.0, 2.0), s = (Vector(2) << 3.0, 4.0); GaussianConditional conditional(1, d, R, noiseModel::Diagonal::Sigmas(s)); GaussianDensity copied(conditional); diff --git a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp index 5247a80b6..6123b0071 100644 --- a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp @@ -46,15 +46,15 @@ TEST(GaussianFactorGraph, initialization) { fg += JacobianFactor(0, 10*eye(2), -1.0*ones(2), unit2), - JacobianFactor(0, -10*eye(2),1, 10*eye(2), (Vec(2) << 2.0, -1.0), unit2), - JacobianFactor(0, -5*eye(2), 2, 5*eye(2), (Vec(2) << 0.0, 1.0), unit2), - JacobianFactor(1, -5*eye(2), 2, 5*eye(2), (Vec(2) << -1.0, 1.5), unit2); + JacobianFactor(0, -10*eye(2),1, 10*eye(2), (Vector(2) << 2.0, -1.0), unit2), + JacobianFactor(0, -5*eye(2), 2, 5*eye(2), (Vector(2) << 0.0, 1.0), unit2), + JacobianFactor(1, -5*eye(2), 2, 5*eye(2), (Vector(2) << -1.0, 1.5), unit2); EXPECT_LONGS_EQUAL(4, (long)fg.size()); // Test sparse, which takes a vector and returns a matrix, used in MATLAB // Note that this the augmented vector and the RHS is in column 7 - Matrix expectedIJS = (Mat(3, 22) << + Matrix expectedIJS = (Matrix(3, 22) << 1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 5., 6., 7., 8., 7., 8., 7., 8., 1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 7., 3., 4., 5., 6., 7., 7., 10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5., 0., 1., -5., -5., 5., 5., -1., 1.5 @@ -73,7 +73,7 @@ TEST(GaussianFactorGraph, sparseJacobian) { // 0 0 0 14 15 16 // Expected - NOTE that we transpose this! - Matrix expectedT = (Mat(16, 3) << + Matrix expectedT = (Matrix(16, 3) << 1., 1., 2., 1., 2., 4., 1., 3., 6., @@ -95,8 +95,8 @@ TEST(GaussianFactorGraph, sparseJacobian) { GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5); - gfg.add(0, (Mat(2, 3) << 1., 2., 3., 5., 6., 7.), (Vec(2) << 4., 8.), model); - gfg.add(0, (Mat(2, 3) << 9.,10., 0., 0., 0., 0.), 1, (Mat(2, 2) << 11., 12., 14., 15.), (Vec(2) << 13.,16.), model); + gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.), (Vector(2) << 4., 8.), model); + gfg.add(0, (Matrix(2, 3) << 9.,10., 0., 0., 0., 0.), 1, (Matrix(2, 2) << 11., 12., 14., 15.), (Vector(2) << 13.,16.), model); Matrix actual = gfg.sparseJacobian_(); @@ -114,8 +114,8 @@ TEST(GaussianFactorGraph, matrices) { GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Unit::Create(2); - gfg.add(0, (Mat(2, 3) << 1., 2., 3., 5., 6., 7.), (Vec(2) << 4., 8.), model); - gfg.add(0, (Mat(2, 3) << 9.,10., 0., 0., 0., 0.), 1, (Mat(2, 2) << 11., 12., 14., 15.), (Vec(2) << 13.,16.), model); + gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.), (Vector(2) << 4., 8.), model); + gfg.add(0, (Matrix(2, 3) << 9.,10., 0., 0., 0., 0.), 1, (Matrix(2, 2) << 11., 12., 14., 15.), (Vector(2) << 13.,16.), model); Matrix jacobian(4,6); jacobian << @@ -151,11 +151,11 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), (Vec(2) << 2.0, -1.0), unit2); + fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), (Vector(2) << 2.0, -1.0), unit2); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), (Vec(2) << 0.0, 1.0), unit2); + fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), (Vector(2) << 0.0, 1.0), unit2); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vec(2) << -1.0, 1.5), unit2); + fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vector(2) << -1.0, 1.5), unit2); return fg; } @@ -168,9 +168,9 @@ TEST( GaussianFactorGraph, gradient ) // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2 // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5] VectorValues expected = map_list_of - (1, (Vec(2) << 5.0, -12.5)) - (2, (Vec(2) << 30.0, 5.0)) - (0, (Vec(2) << -25.0, 17.5)); + (1, (Vector(2) << 5.0, -12.5)) + (2, (Vector(2) << 30.0, 5.0)) + (0, (Vector(2) << -25.0, 17.5)); // Check the gradient at delta=0 VectorValues zero = VectorValues::Zero(expected); @@ -190,15 +190,15 @@ TEST( GaussianFactorGraph, transposeMultiplication ) GaussianFactorGraph A = createSimpleGaussianFactorGraph(); Errors e; e += - (Vec(2) << 0.0, 0.0), - (Vec(2) << 15.0, 0.0), - (Vec(2) << 0.0,-5.0), - (Vec(2) << -7.5,-5.0); + (Vector(2) << 0.0, 0.0), + (Vector(2) << 15.0, 0.0), + (Vector(2) << 0.0,-5.0), + (Vector(2) << -7.5,-5.0); VectorValues expected; - expected.insert(1, (Vec(2) << -37.5,-50.0)); - expected.insert(2, (Vec(2) << -150.0, 25.0)); - expected.insert(0, (Vec(2) << 187.5, 25.0)); + expected.insert(1, (Vector(2) << -37.5,-50.0)); + expected.insert(2, (Vector(2) << -150.0, 25.0)); + expected.insert(0, (Vector(2) << 187.5, 25.0)); VectorValues actual = A.transposeMultiply(e); EXPECT(assert_equal(expected, actual)); @@ -242,14 +242,14 @@ TEST( GaussianFactorGraph, multiplyHessianAdd ) GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); VectorValues x = map_list_of - (0, (Vec(2) << 1,2)) - (1, (Vec(2) << 3,4)) - (2, (Vec(2) << 5,6)); + (0, (Vector(2) << 1,2)) + (1, (Vector(2) << 3,4)) + (2, (Vector(2) << 5,6)); VectorValues expected; - expected.insert(0, (Vec(2) << -450, -450)); - expected.insert(1, (Vec(2) << 0, 0)); - expected.insert(2, (Vec(2) << 950, 1050)); + expected.insert(0, (Vector(2) << -450, -450)); + expected.insert(1, (Vector(2) << 0, 0)); + expected.insert(2, (Vector(2) << 950, 1050)); VectorValues actual; gfg.multiplyHessianAdd(1.0, x, actual); @@ -263,8 +263,8 @@ TEST( GaussianFactorGraph, multiplyHessianAdd ) /* ************************************************************************* */ static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() { GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); - gfg += HessianFactor(1, 2, 100*eye(2,2), zeros(2,2), (Vec(2) << 0.0, 1.0), - 400*eye(2,2), (Vec(2) << 1.0, 1.0), 3.0); + gfg += HessianFactor(1, 2, 100*eye(2,2), zeros(2,2), (Vector(2) << 0.0, 1.0), + 400*eye(2,2), (Vector(2) << 1.0, 1.0), 3.0); return gfg; } @@ -280,14 +280,14 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 ) EXPECT(assert_equal(Y,AtA*X)); VectorValues x = map_list_of - (0, (Vec(2) << 1,2)) - (1, (Vec(2) << 3,4)) - (2, (Vec(2) << 5,6)); + (0, (Vector(2) << 1,2)) + (1, (Vector(2) << 3,4)) + (2, (Vector(2) << 5,6)); VectorValues expected; - expected.insert(0, (Vec(2) << -450, -450)); - expected.insert(1, (Vec(2) << 300, 400)); - expected.insert(2, (Vec(2) << 2950, 3450)); + expected.insert(0, (Vector(2) << -450, -450)); + expected.insert(1, (Vector(2) << 300, 400)); + expected.insert(2, (Vector(2) << 2950, 3450)); VectorValues actual; gfg.multiplyHessianAdd(1.0, x, actual); @@ -306,7 +306,7 @@ TEST( GaussianFactorGraph, matricesMixed ) Matrix A; Vector b; boost::tie(A,b) = gfg.jacobian(); // incorrect ! Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian(); // correct EXPECT(assert_equal(A.transpose()*A, AtA)); - Vector expected = - (Vec(6) << -25, 17.5, 5, -13.5, 29, 4); + Vector expected = - (Vector(6) << -25, 17.5, 5, -13.5, 29, 4); EXPECT(assert_equal(expected, eta)); EXPECT(assert_equal(A.transpose()*b, eta)); } @@ -318,9 +318,9 @@ TEST( GaussianFactorGraph, gradientAtZero ) GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); VectorValues expected; VectorValues actual = gfg.gradientAtZero(); - expected.insert(0, (Vec(2) << -25, 17.5)); - expected.insert(1, (Vec(2) << 5, -13.5)); - expected.insert(2, (Vec(2) << 29, 4)); + expected.insert(0, (Vector(2) << -25, 17.5)); + expected.insert(1, (Vector(2) << 5, -13.5)); + expected.insert(2, (Vector(2) << 29, 4)); EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam/linear/tests/testHessianFactorUnordered.cpp b/gtsam/linear/tests/testHessianFactorUnordered.cpp index 5e6f49235..383604c76 100644 --- a/gtsam/linear/tests/testHessianFactorUnordered.cpp +++ b/gtsam/linear/tests/testHessianFactorUnordered.cpp @@ -51,7 +51,7 @@ TEST(HessianFactor, emptyConstructor) TEST(HessianFactor, ConversionConstructor) { HessianFactor expected(list_of(0)(1), - SymmetricBlockMatrix(list_of(2)(4)(1), (Mat(7,7) << + SymmetricBlockMatrix(list_of(2)(4)(1), (Matrix(7,7) << 125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000, 0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000, -25.0000, 0.0, 25.0000, 0.0, 0.0, 0.0, -5.0000, @@ -61,22 +61,22 @@ TEST(HessianFactor, ConversionConstructor) 25.0000, -17.5000, -5.0000, 7.5000, -20.0000, 10.0000, 8.2500))); JacobianFactor jacobian( - 0, (Mat(4,2) << -1., 0., + 0, (Matrix(4,2) << -1., 0., +0.,-1., 1., 0., +0.,1.), - 1, (Mat(4,4) << 1., 0., 0.00, 0., // f4 + 1, (Matrix(4,4) << 1., 0., 0.00, 0., // f4 0., 1., 0.00, 0., // f4 0., 0., -1., 0., // f2 0., 0., 0.00, -1.), // f2 - (Vec(4) << -0.2, 0.3, 0.2, -0.1), - noiseModel::Diagonal::Sigmas((Vec(4) << 0.2, 0.2, 0.1, 0.1))); + (Vector(4) << -0.2, 0.3, 0.2, -0.1), + noiseModel::Diagonal::Sigmas((Vector(4) << 0.2, 0.2, 0.1, 0.1))); HessianFactor actual(jacobian); VectorValues values = pair_list_of - (0, (Vec(2) << 1.0, 2.0)) - (1, (Vec(4) << 3.0, 4.0, 5.0, 6.0)); + (0, (Vector(2) << 1.0, 2.0)) + (1, (Vector(4) << 3.0, 4.0, 5.0, 6.0)); EXPECT_LONGS_EQUAL(2, (long)actual.size()); EXPECT(assert_equal(expected, actual, 1e-9)); @@ -86,8 +86,8 @@ TEST(HessianFactor, ConversionConstructor) /* ************************************************************************* */ TEST(HessianFactor, Constructor1) { - Matrix G = (Mat(2,2) << 3.0, 5.0, 5.0, 6.0); - Vector g = (Vec(2) << -8.0, -9.0); + Matrix G = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0); + Vector g = (Vector(2) << -8.0, -9.0); double f = 10.0; HessianFactor factor(0, G, g, f); @@ -97,7 +97,7 @@ TEST(HessianFactor, Constructor1) EXPECT(assert_equal(g, Vector(factor.linearTerm()))); EXPECT_LONGS_EQUAL(1, (long)factor.size()); - VectorValues dx = pair_list_of(0, (Vec(2) << 1.5, 2.5)); + VectorValues dx = pair_list_of(0, (Vector(2) << 1.5, 2.5)); // error 0.5*(f - 2*x'*g + x'*G*x) double expected = 80.375; @@ -111,7 +111,7 @@ TEST(HessianFactor, Constructor1) /* ************************************************************************* */ TEST(HessianFactor, Constructor1b) { - Vector mu = (Vec(2) << 1.0,2.0); + Vector mu = (Vector(2) << 1.0,2.0); Matrix Sigma = eye(2,2); HessianFactor factor(0, mu, Sigma); @@ -130,15 +130,15 @@ TEST(HessianFactor, Constructor1b) /* ************************************************************************* */ TEST(HessianFactor, Constructor2) { - Matrix G11 = (Mat(1,1) << 1.0); - Matrix G12 = (Mat(1,2) << 2.0, 4.0); - Matrix G22 = (Mat(2,2) << 3.0, 5.0, 5.0, 6.0); - Vector g1 = (Vec(1) << -7.0); - Vector g2 = (Vec(2) << -8.0, -9.0); + Matrix G11 = (Matrix(1,1) << 1.0); + Matrix G12 = (Matrix(1,2) << 2.0, 4.0); + Matrix G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0); + Vector g1 = (Vector(1) << -7.0); + Vector g2 = (Vector(2) << -8.0, -9.0); double f = 10.0; - Vector dx0 = (Vec(1) << 0.5); - Vector dx1 = (Vec(2) << 1.5, 2.5); + Vector dx0 = (Vector(1) << 0.5); + Vector dx1 = (Vector(2) << 1.5, 2.5); VectorValues dx = pair_list_of (0, dx0) @@ -164,31 +164,31 @@ TEST(HessianFactor, Constructor2) VectorValues dxLarge = pair_list_of (0, dx0) (1, dx1) - (2, (Vec(2) << 0.1, 0.2)); + (2, (Vector(2) << 0.1, 0.2)); EXPECT_DOUBLES_EQUAL(expected, factor.error(dxLarge), 1e-10); } /* ************************************************************************* */ TEST(HessianFactor, Constructor3) { - Matrix G11 = (Mat(1,1) << 1.0); - Matrix G12 = (Mat(1,2) << 2.0, 4.0); - Matrix G13 = (Mat(1,3) << 3.0, 6.0, 9.0); + Matrix G11 = (Matrix(1,1) << 1.0); + Matrix G12 = (Matrix(1,2) << 2.0, 4.0); + Matrix G13 = (Matrix(1,3) << 3.0, 6.0, 9.0); - Matrix G22 = (Mat(2,2) << 3.0, 5.0, 5.0, 6.0); - Matrix G23 = (Mat(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0); + Matrix G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0); + Matrix G23 = (Matrix(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0); - Matrix G33 = (Mat(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0); + Matrix G33 = (Matrix(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0); - Vector g1 = (Vec(1) << -7.0); - Vector g2 = (Vec(2) << -8.0, -9.0); - Vector g3 = (Vec(3) << 1.0, 2.0, 3.0); + Vector g1 = (Vector(1) << -7.0); + Vector g2 = (Vector(2) << -8.0, -9.0); + Vector g3 = (Vector(3) << 1.0, 2.0, 3.0); double f = 10.0; - Vector dx0 = (Vec(1) << 0.5); - Vector dx1 = (Vec(2) << 1.5, 2.5); - Vector dx2 = (Vec(3) << 1.5, 2.5, 3.5); + Vector dx0 = (Vector(1) << 0.5); + Vector dx1 = (Vector(2) << 1.5, 2.5); + Vector dx2 = (Vector(3) << 1.5, 2.5, 3.5); VectorValues dx = pair_list_of (0, dx0) @@ -218,24 +218,24 @@ TEST(HessianFactor, Constructor3) /* ************************************************************************* */ TEST(HessianFactor, ConstructorNWay) { - Matrix G11 = (Mat(1,1) << 1.0); - Matrix G12 = (Mat(1,2) << 2.0, 4.0); - Matrix G13 = (Mat(1,3) << 3.0, 6.0, 9.0); + Matrix G11 = (Matrix(1,1) << 1.0); + Matrix G12 = (Matrix(1,2) << 2.0, 4.0); + Matrix G13 = (Matrix(1,3) << 3.0, 6.0, 9.0); - Matrix G22 = (Mat(2,2) << 3.0, 5.0, 5.0, 6.0); - Matrix G23 = (Mat(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0); + Matrix G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0); + Matrix G23 = (Matrix(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0); - Matrix G33 = (Mat(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0); + Matrix G33 = (Matrix(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0); - Vector g1 = (Vec(1) << -7.0); - Vector g2 = (Vec(2) << -8.0, -9.0); - Vector g3 = (Vec(3) << 1.0, 2.0, 3.0); + Vector g1 = (Vector(1) << -7.0); + Vector g2 = (Vector(2) << -8.0, -9.0); + Vector g3 = (Vector(3) << 1.0, 2.0, 3.0); double f = 10.0; - Vector dx0 = (Vec(1) << 0.5); - Vector dx1 = (Vec(2) << 1.5, 2.5); - Vector dx2 = (Vec(3) << 1.5, 2.5, 3.5); + Vector dx0 = (Vector(1) << 0.5); + Vector dx1 = (Vector(2) << 1.5, 2.5); + Vector dx2 = (Vector(3) << 1.5, 2.5, 3.5); VectorValues dx = pair_list_of (0, dx0) @@ -271,30 +271,30 @@ TEST(HessianFactor, ConstructorNWay) /* ************************************************************************* */ TEST(HessianFactor, CombineAndEliminate) { - Matrix A01 = (Mat(3,3) << + Matrix A01 = (Matrix(3,3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); - Vector b0 = (Vec(3) << 1.5, 1.5, 1.5); - Vector s0 = (Vec(3) << 1.6, 1.6, 1.6); + Vector b0 = (Vector(3) << 1.5, 1.5, 1.5); + Vector s0 = (Vector(3) << 1.6, 1.6, 1.6); - Matrix A10 = (Mat(3,3) << + Matrix A10 = (Matrix(3,3) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0); - Matrix A11 = (Mat(3,3) << + Matrix A11 = (Matrix(3,3) << -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, -2.0); - Vector b1 = (Vec(3) << 2.5, 2.5, 2.5); - Vector s1 = (Vec(3) << 2.6, 2.6, 2.6); + Vector b1 = (Vector(3) << 2.5, 2.5, 2.5); + Vector s1 = (Vector(3) << 2.6, 2.6, 2.6); - Matrix A21 = (Mat(3,3) << + Matrix A21 = (Matrix(3,3) << 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 3.0); - Vector b2 = (Vec(3) << 3.5, 3.5, 3.5); - Vector s2 = (Vec(3) << 3.6, 3.6, 3.6); + Vector b2 = (Vector(3) << 3.5, 3.5, 3.5); + Vector s2 = (Vector(3) << 3.6, 3.6, 3.6); GaussianFactorGraph gfg; gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); @@ -330,10 +330,10 @@ TEST(HessianFactor, eliminate2 ) // sigmas double sigma1 = 0.2; double sigma2 = 0.1; - Vector sigmas = (Vec(4) << sigma1, sigma1, sigma2, sigma2); + Vector sigmas = (Vector(4) << sigma1, sigma1, sigma2, sigma2); // the combined linear factor - Matrix Ax2 = (Mat(4,2) << + Matrix Ax2 = (Matrix(4,2) << // x2 -1., 0., +0.,-1., @@ -341,7 +341,7 @@ TEST(HessianFactor, eliminate2 ) +0.,1. ); - Matrix Al1x1 = (Mat(4,4) << + Matrix Al1x1 = (Matrix(4,4) << // l1 x1 1., 0., 0.00, 0., // f4 0., 1., 0.00, 0., // f4 @@ -370,26 +370,26 @@ TEST(HessianFactor, eliminate2 ) // create expected Conditional Gaussian double oldSigma = 0.0894427; // from when R was made unit - Matrix R11 = (Mat(2,2) << + Matrix R11 = (Matrix(2,2) << 1.00, 0.00, 0.00, 1.00 )/oldSigma; - Matrix S12 = (Mat(2,4) << + Matrix S12 = (Matrix(2,4) << -0.20, 0.00,-0.80, 0.00, +0.00,-0.20,+0.00,-0.80 )/oldSigma; - Vector d = (Vec(2) << 0.2,-0.14)/oldSigma; + Vector d = (Vector(2) << 0.2,-0.14)/oldSigma; GaussianConditional expectedCG(0, d, R11, 1, S12); EXPECT(assert_equal(expectedCG, *actual_Chol.first, 1e-4)); // the expected linear factor double sigma = 0.2236; - Matrix Bl1x1 = (Mat(2,4) << + Matrix Bl1x1 = (Matrix(2,4) << // l1 x1 1.00, 0.00, -1.00, 0.00, 0.00, 1.00, +0.00, -1.00 )/sigma; - Vector b1 = (Vec(2) << 0.0,0.894427); + Vector b1 = (Vector(2) << 0.0,0.894427); JacobianFactor expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0)); EXPECT(assert_equal(HessianFactor(expectedLF), *actual_Chol.second, 1.5e-3)); } @@ -398,16 +398,16 @@ TEST(HessianFactor, eliminate2 ) TEST(HessianFactor, combine) { // update the information matrix with a single jacobian factor - Matrix A0 = (Mat(2, 2) << + Matrix A0 = (Matrix(2, 2) << 11.1803399, 0.0, 0.0, 11.1803399); - Matrix A1 = (Mat(2, 2) << + Matrix A1 = (Matrix(2, 2) << -2.23606798, 0.0, 0.0, -2.23606798); - Matrix A2 = (Mat(2, 2) << + Matrix A2 = (Matrix(2, 2) << -8.94427191, 0.0, 0.0, -8.94427191); - Vector b = (Vec(2) << 2.23606798,-1.56524758); + Vector b = (Vector(2) << 2.23606798,-1.56524758); SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2)); GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model)); GaussianFactorGraph factors = list_of(f); @@ -415,7 +415,7 @@ TEST(HessianFactor, combine) { // Form Ab' * Ab HessianFactor actual(factors); - Matrix expected = (Mat(7, 7) << + Matrix expected = (Matrix(7, 7) << 125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000, 0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000, -25.0000, 0.0, 5.0000, 0.0, 20.0000, 0.0, -5.0000, @@ -430,11 +430,11 @@ TEST(HessianFactor, combine) { /* ************************************************************************* */ TEST(HessianFactor, gradientAtZero) { - Matrix G11 = (Mat(1, 1) << 1.0); - Matrix G12 = (Mat(1, 2) << 0.0, 0.0); - Matrix G22 = (Mat(2, 2) << 1.0, 0.0, 0.0, 1.0); - Vector g1 = (Vec(1) << -7.0); - Vector g2 = (Vec(2) << -8.0, -9.0); + Matrix G11 = (Matrix(1, 1) << 1.0); + Matrix G12 = (Matrix(1, 2) << 0.0, 0.0); + Matrix G22 = (Matrix(2, 2) << 1.0, 0.0, 0.0, 1.0); + Vector g1 = (Vector(1) << -7.0); + Vector g2 = (Vector(2) << -8.0, -9.0); double f = 194; HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); diff --git a/gtsam/linear/tests/testJacobianFactorUnordered.cpp b/gtsam/linear/tests/testJacobianFactorUnordered.cpp index 396454f8a..72afa3ae9 100644 --- a/gtsam/linear/tests/testJacobianFactorUnordered.cpp +++ b/gtsam/linear/tests/testJacobianFactorUnordered.cpp @@ -42,8 +42,8 @@ namespace { (make_pair(15, 3*Matrix3::Identity())); // RHS and sigmas - const Vector b = (Vec(3) << 1., 2., 3.); - const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.5)); + const Vector b = (Vector(3) << 1., 2., 3.); + const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.5)); } } @@ -240,22 +240,22 @@ TEST(JacobianFactor, operators ) SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); Matrix I = eye(2); - Vector b = (Vec(2) << 0.2,-0.1); + Vector b = (Vector(2) << 0.2,-0.1); JacobianFactor lf(1, -I, 2, I, b, sigma0_1); VectorValues c; - c.insert(1, (Vec(2) << 10.,20.)); - c.insert(2, (Vec(2) << 30.,60.)); + c.insert(1, (Vector(2) << 10.,20.)); + c.insert(2, (Vector(2) << 30.,60.)); // test A*x - Vector expectedE = (Vec(2) << 200.,400.); + Vector expectedE = (Vector(2) << 200.,400.); Vector actualE = lf * c; EXPECT(assert_equal(expectedE, actualE)); // test A^e VectorValues expectedX; - expectedX.insert(1, (Vec(2) << -2000.,-4000.)); - expectedX.insert(2, (Vec(2) << 2000., 4000.)); + expectedX.insert(1, (Vector(2) << -2000.,-4000.)); + expectedX.insert(2, (Vector(2) << 2000., 4000.)); VectorValues actualX = VectorValues::Zero(expectedX); lf.transposeMultiplyAdd(1.0, actualE, actualX); EXPECT(assert_equal(expectedX, actualX)); @@ -263,8 +263,8 @@ TEST(JacobianFactor, operators ) // test gradient at zero Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian(); VectorValues expectedG; - expectedG.insert(1, (Vec(2) << 20,-10)); - expectedG.insert(2, (Vec(2) << -20, 10)); + expectedG.insert(1, (Vector(2) << 20,-10)); + expectedG.insert(2, (Vector(2) << -20, 10)); FastVector keys; keys += 1,2; EXPECT(assert_equal(-A.transpose()*b2, expectedG.vector(keys))); VectorValues actualG = lf.gradientAtZero(); @@ -290,30 +290,30 @@ TEST(JacobianFactor, empty ) /* ************************************************************************* */ TEST(JacobianFactor, eliminate) { - Matrix A01 = (Mat(3, 3) << + Matrix A01 = (Matrix(3, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); - Vector b0 = (Vec(3) << 1.5, 1.5, 1.5); - Vector s0 = (Vec(3) << 1.6, 1.6, 1.6); + Vector b0 = (Vector(3) << 1.5, 1.5, 1.5); + Vector s0 = (Vector(3) << 1.6, 1.6, 1.6); - Matrix A10 = (Mat(3, 3) << + Matrix A10 = (Matrix(3, 3) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0); - Matrix A11 = (Mat(3, 3) << + Matrix A11 = (Matrix(3, 3) << -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, -2.0); - Vector b1 = (Vec(3) << 2.5, 2.5, 2.5); - Vector s1 = (Vec(3) << 2.6, 2.6, 2.6); + Vector b1 = (Vector(3) << 2.5, 2.5, 2.5); + Vector s1 = (Vector(3) << 2.6, 2.6, 2.6); - Matrix A21 = (Mat(3, 3) << + Matrix A21 = (Matrix(3, 3) << 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 3.0); - Vector b2 = (Vec(3) << 3.5, 3.5, 3.5); - Vector s2 = (Vec(3) << 3.6, 3.6, 3.6); + Vector b2 = (Vector(3) << 3.5, 3.5, 3.5); + Vector s2 = (Vector(3) << 3.6, 3.6, 3.6); GaussianFactorGraph gfg; gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); @@ -345,10 +345,10 @@ TEST(JacobianFactor, eliminate2 ) // sigmas double sigma1 = 0.2; double sigma2 = 0.1; - Vector sigmas = (Vec(4) << sigma1, sigma1, sigma2, sigma2); + Vector sigmas = (Vector(4) << sigma1, sigma1, sigma2, sigma2); // the combined linear factor - Matrix Ax2 = (Mat(4, 2) << + Matrix Ax2 = (Matrix(4, 2) << // x2 -1., 0., +0.,-1., @@ -356,7 +356,7 @@ TEST(JacobianFactor, eliminate2 ) +0.,1. ); - Matrix Al1x1 = (Mat(4, 4) << + Matrix Al1x1 = (Matrix(4, 4) << // l1 x1 1., 0., 0.00, 0., // f4 0., 1., 0.00, 0., // f4 @@ -382,27 +382,27 @@ TEST(JacobianFactor, eliminate2 ) // create expected Conditional Gaussian double oldSigma = 0.0894427; // from when R was made unit - Matrix R11 = (Mat(2, 2) << + Matrix R11 = (Matrix(2, 2) << 1.00, 0.00, 0.00, 1.00 )/oldSigma; - Matrix S12 = (Mat(2, 4) << + Matrix S12 = (Matrix(2, 4) << -0.20, 0.00,-0.80, 0.00, +0.00,-0.20,+0.00,-0.80 )/oldSigma; - Vector d = (Vec(2) << 0.2,-0.14)/oldSigma; + Vector d = (Vector(2) << 0.2,-0.14)/oldSigma; GaussianConditional expectedCG(2, d, R11, 11, S12); EXPECT(assert_equal(expectedCG, *actual.first, 1e-4)); // the expected linear factor double sigma = 0.2236; - Matrix Bl1x1 = (Mat(2, 4) << + Matrix Bl1x1 = (Matrix(2, 4) << // l1 x1 1.00, 0.00, -1.00, 0.00, 0.00, 1.00, +0.00, -1.00 )/sigma; - Vector b1 = (Vec(2) << 0.0, 0.894427); + Vector b1 = (Vector(2) << 0.0, 0.894427); JacobianFactor expectedLF(11, Bl1x1, b1); EXPECT(assert_equal(expectedLF, *actual.second,1e-3)); } @@ -411,7 +411,7 @@ TEST(JacobianFactor, eliminate2 ) TEST(JacobianFactor, EliminateQR) { // Augmented Ab test case for whole factor graph - Matrix Ab = (Mat(14, 11) << + Matrix Ab = (Matrix(14, 11) << 4., 0., 1., 4., 1., 0., 3., 6., 8., 8., 1., 9., 2., 0., 1., 6., 3., 9., 6., 6., 9., 4., 5., 3., 7., 9., 5., 5., 9., 1., 3., 7., 0., @@ -441,7 +441,7 @@ TEST(JacobianFactor, EliminateQR) EXPECT(assert_equal(2.0 * Ab, actualDense)); // Expected augmented matrix, both GaussianConditional (first 6 rows) and remaining factor (next 4 rows) - Matrix R = 2.0 * (Mat(11, 11) << + Matrix R = 2.0 * (Matrix(11, 11) << -12.1244, -5.1962, -5.2786, -8.6603, -10.5573, -5.9385, -11.3820, -7.2581, -8.7427, -13.4440, -5.3611, 0., 4.6904, 5.0254, 5.5432, 5.5737, 3.0153, -3.0153, -3.5635, -3.9290, -2.7412, 2.1625, 0., 0., -13.8160, -8.7166, -10.2245, -8.8666, -8.7632, -5.2544, -6.9192, -10.5537, -9.3250, @@ -486,7 +486,7 @@ TEST ( JacobianFactor, constraint_eliminate1 ) EXPECT(actual.second->size() == 0); // verify conditional Gaussian - Vector sigmas = (Vec(2) << 0.0, 0.0); + Vector sigmas = (Vector(2) << 0.0, 0.0); GaussianConditional expCG(1, v, eye(2), noiseModel::Diagonal::Sigmas(sigmas)); EXPECT(assert_equal(expCG, *actual.first)); } @@ -523,14 +523,14 @@ TEST ( JacobianFactor, constraint_eliminate2 ) EXPECT(assert_equal(expectedLF, *actual.second)); // verify CG - Matrix R = (Mat(2, 2) << + Matrix R = (Matrix(2, 2) << 1.0, 2.0, 0.0, 1.0); - Matrix S = (Mat(2, 2) << + Matrix S = (Matrix(2, 2) << 1.0, 2.0, 0.0, 0.0); - Vector d = (Vec(2) << 3.0, 0.6666); - Vector sigmas = (Vec(2) << 0.0, 0.0); + Vector d = (Vector(2) << 3.0, 0.6666); + Vector sigmas = (Vector(2) << 0.0, 0.0); GaussianConditional expectedCG(1, d, R, 2, S, noiseModel::Diagonal::Sigmas(sigmas)); EXPECT(assert_equal(expectedCG, *actual.first, 1e-4)); } diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index 0a9a6fef5..87ff419c8 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -30,7 +30,7 @@ using namespace gtsam; /** Small 2D point class implemented as a Vector */ struct State: Vector { State(double x, double y) : - Vector((Vec(2) << x, y)) { + Vector((Vector(2) << x, y)) { } }; @@ -49,7 +49,7 @@ TEST( KalmanFilter, constructor ) { // Assert it has the correct mean, covariance and information EXPECT(assert_equal(x_initial, p1->mean())); - Matrix Sigma = (Mat(2, 2) << 0.01, 0.0, 0.0, 0.01); + Matrix Sigma = (Matrix(2, 2) << 0.01, 0.0, 0.0, 0.01); EXPECT(assert_equal(Sigma, p1->covariance())); EXPECT(assert_equal(inverse(Sigma), p1->information())); @@ -67,7 +67,7 @@ TEST( KalmanFilter, linear1 ) { // Create the controls and measurement properties for our example Matrix F = eye(2, 2); Matrix B = eye(2, 2); - Vector u = (Vec(2) << 1.0, 0.0); + Vector u = (Vector(2) << 1.0, 0.0); SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1); Matrix Q = 0.01*eye(2, 2); Matrix H = eye(2, 2); @@ -135,10 +135,10 @@ TEST( KalmanFilter, linear1 ) { TEST( KalmanFilter, predict ) { // Create dynamics model - Matrix F = (Mat(2, 2) << 1.0, 0.1, 0.2, 1.1); - Matrix B = (Mat(2, 3) << 1.0, 0.1, 0.2, 1.1, 1.2, 0.8); - Vector u = (Vec(3) << 1.0, 0.0, 2.0); - Matrix R = (Mat(2, 2) << 1.0, 0.5, 0.0, 3.0); + Matrix F = (Matrix(2, 2) << 1.0, 0.1, 0.2, 1.1); + Matrix B = (Matrix(2, 3) << 1.0, 0.1, 0.2, 1.1, 1.2, 0.8); + Vector u = (Vector(3) << 1.0, 0.0, 2.0); + Matrix R = (Matrix(2, 2) << 1.0, 0.5, 0.0, 3.0); Matrix M = trans(R)*R; Matrix Q = inverse(M); @@ -168,7 +168,7 @@ TEST( KalmanFilter, predict ) { TEST( KalmanFilter, QRvsCholesky ) { Vector mean = ones(9); - Matrix covariance = 1e-6 * (Mat(9, 9) << + Matrix covariance = 1e-6 * (Matrix(9, 9) << 15.0, -6.2, 0.0, 0.0, 0.0, 0.0, 0.0, 63.8, -0.6, -6.2, 21.9, -0.0, 0.0, 0.0, 0.0, -63.8, -0.0, -0.1, 0.0, -0.0, 100.0, 0.0, 0.0, 0.0, 0.0, 0.1, -0.0, @@ -187,7 +187,7 @@ TEST( KalmanFilter, QRvsCholesky ) { KalmanFilter::State p0b = kfb.init(mean, covariance); // Set up dynamics update - Matrix Psi_k = 1e-6 * (Mat(9, 9) << + Matrix Psi_k = 1e-6 * (Matrix(9, 9) << 1000000.0, 0.0, 0.0, -19200.0, 600.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 600.0, 19200.0, 200.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, -0.0, -200.0, 19200.0, 0.0, 0.0, 0.0, @@ -199,7 +199,7 @@ TEST( KalmanFilter, QRvsCholesky ) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0); Matrix B = zeros(9, 1); Vector u = zero(1); - Matrix dt_Q_k = 1e-6 * (Mat(9, 9) << + Matrix dt_Q_k = 1e-6 * (Matrix(9, 9) << 33.7, 3.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.1, 126.4, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.0, -0.3, 88.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, @@ -219,10 +219,10 @@ TEST( KalmanFilter, QRvsCholesky ) { EXPECT(assert_equal(pa->information(), pb->information(), 1e-7)); // and in addition attain the correct covariance - Vector expectedMean = (Vec(9) << 0.9814, 1.0200, 1.0190, 1., 1., 1., 1., 1., 1.); + Vector expectedMean = (Vector(9) << 0.9814, 1.0200, 1.0190, 1., 1., 1., 1., 1., 1.); EXPECT(assert_equal(expectedMean, pa->mean(), 1e-7)); EXPECT(assert_equal(expectedMean, pb->mean(), 1e-7)); - Matrix expected = 1e-6 * (Mat(9, 9) << + Matrix expected = 1e-6 * (Matrix(9, 9) << 48.8, -3.1, -0.0, -0.4, -0.4, 0.0, 0.0, 63.8, -0.6, -3.1, 148.4, -0.3, 0.5, 1.7, 0.2, -63.8, 0.0, -0.1, -0.0, -0.3, 188.0, -0.0, 0.2, 1.2, 0.0, 0.1, 0.0, @@ -236,12 +236,12 @@ TEST( KalmanFilter, QRvsCholesky ) { EXPECT(assert_equal(expected, pb->covariance(), 1e-7)); // prepare update - Matrix H = 1e-3 * (Mat(3, 9) << + Matrix H = 1e-3 * (Matrix(3, 9) << 0.0, 9795.9, 83.6, 0.0, 0.0, 0.0, 1000.0, 0.0, 0.0, -9795.9, 0.0, -5.2, 0.0, 0.0, 0.0, 0.0, 1000.0, 0.0, -83.6, 5.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.); - Vector z = (Vec(3) << 0.2599 , 1.3327 , 0.2007); - Vector sigmas = (Vec(3) << 0.3323 , 0.2470 , 0.1904); + Vector z = (Vector(3) << 0.2599 , 1.3327 , 0.2007); + Vector sigmas = (Vector(3) << 0.3323 , 0.2470 , 0.1904); SharedDiagonal modelR = noiseModel::Diagonal::Sigmas(sigmas); // do update @@ -253,10 +253,10 @@ TEST( KalmanFilter, QRvsCholesky ) { EXPECT(assert_equal(pa2->information(), pb2->information(), 1e-7)); // and in addition attain the correct mean and covariance - Vector expectedMean2 = (Vec(9) << 0.9207, 0.9030, 1.0178, 1.0002, 0.9992, 0.9998, 0.9981, 1.0035, 0.9882); + Vector expectedMean2 = (Vector(9) << 0.9207, 0.9030, 1.0178, 1.0002, 0.9992, 0.9998, 0.9981, 1.0035, 0.9882); EXPECT(assert_equal(expectedMean2, pa2->mean(), 1e-4));// not happy with tolerance here ! EXPECT(assert_equal(expectedMean2, pb2->mean(), 1e-4));// is something still amiss? - Matrix expected2 = 1e-6 * (Mat(9, 9) << + Matrix expected2 = 1e-6 * (Matrix(9, 9) << 46.1, -2.6, -0.0, -0.4, -0.4, 0.0, 0.0, 63.9, -0.5, -2.6, 132.8, -0.5, 0.4, 1.5, 0.2, -64.0, -0.0, -0.1, -0.0, -0.5, 188.0, -0.0, 0.2, 1.2, -0.0, 0.1, 0.0, diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 54fcac6e0..99c301dbb 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -31,11 +31,11 @@ using namespace gtsam; using namespace noiseModel; static double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var; -static Matrix R = (Mat(3, 3) << +static Matrix R = (Matrix(3, 3) << s_1, 0.0, 0.0, 0.0, s_1, 0.0, 0.0, 0.0, s_1); -static Matrix Sigma = (Mat(3, 3) << +static Matrix Sigma = (Matrix(3, 3) << var, 0.0, 0.0, 0.0, var, 0.0, 0.0, 0.0, var); @@ -45,17 +45,17 @@ static Matrix Sigma = (Mat(3, 3) << /* ************************************************************************* */ TEST(NoiseModel, constructors) { - Vector whitened = (Vec(3) << 5.0,10.0,15.0); - Vector unwhitened = (Vec(3) << 10.0,20.0,30.0); + Vector whitened = (Vector(3) << 5.0,10.0,15.0); + Vector unwhitened = (Vector(3) << 10.0,20.0,30.0); // Construct noise models vector m; m.push_back(Gaussian::SqrtInformation(R)); m.push_back(Gaussian::Covariance(Sigma)); //m.push_back(Gaussian::Information(Q)); - m.push_back(Diagonal::Sigmas((Vec(3) << sigma, sigma, sigma))); - m.push_back(Diagonal::Variances((Vec(3) << var, var, var))); - m.push_back(Diagonal::Precisions((Vec(3) << prc, prc, prc))); + m.push_back(Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma))); + m.push_back(Diagonal::Variances((Vector(3) << var, var, var))); + m.push_back(Diagonal::Precisions((Vector(3) << prc, prc, prc))); m.push_back(Isotropic::Sigma(3, sigma)); m.push_back(Isotropic::Variance(3, var)); m.push_back(Isotropic::Precision(3, prc)); @@ -74,7 +74,7 @@ TEST(NoiseModel, constructors) DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9); // test R matrix - Matrix expectedR((Mat(3, 3) << + Matrix expectedR((Matrix(3, 3) << s_1, 0.0, 0.0, 0.0, s_1, 0.0, 0.0, 0.0, s_1)); @@ -83,12 +83,12 @@ TEST(NoiseModel, constructors) EXPECT(assert_equal(expectedR,mi->R())); // test Whiten operator - Matrix H((Mat(3, 4) << + Matrix H((Matrix(3, 4) << 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 0.0, 1.0)); - Matrix expected((Mat(3, 4) << + Matrix expected((Matrix(3, 4) << 0.0, 0.0, s_1, s_1, 0.0, s_1, 0.0, s_1, s_1, 0.0, 0.0, s_1)); @@ -104,7 +104,7 @@ TEST(NoiseModel, constructors) /* ************************************************************************* */ TEST(NoiseModel, Unit) { - Vector v = (Vec(3) << 5.0,10.0,15.0); + Vector v = (Vector(3) << 5.0,10.0,15.0); Gaussian::shared_ptr u(Unit::Create(3)); EXPECT(assert_equal(v,u->whiten(v))); } @@ -114,8 +114,8 @@ TEST(NoiseModel, equals) { Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R), g2 = Gaussian::SqrtInformation(eye(3,3)); - Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vec(3) << sigma, sigma, sigma)), - d2 = Diagonal::Sigmas((Vec(3) << 0.1, 0.2, 0.3)); + Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma)), + d2 = Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3)); Isotropic::shared_ptr i1 = Isotropic::Sigma(3, sigma), i2 = Isotropic::Sigma(3, 0.7); @@ -133,7 +133,7 @@ TEST(NoiseModel, equals) ///* ************************************************************************* */ //TEST(NoiseModel, ConstrainedSmart ) //{ -// Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vec(3) << sigma, 0.0, sigma), true); +// Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vector(3) << sigma, 0.0, sigma), true); // Diagonal::shared_ptr n1 = boost::dynamic_pointer_cast(nonconstrained); // Constrained::shared_ptr n2 = boost::dynamic_pointer_cast(nonconstrained); // EXPECT(n1); @@ -152,8 +152,8 @@ TEST(NoiseModel, ConstrainedConstructors ) Constrained::shared_ptr actual; size_t d = 3; double m = 100.0; - Vector sigmas = (Vec(3) << sigma, 0.0, 0.0); - Vector mu = (Vec(3) << 200.0, 300.0, 400.0); + Vector sigmas = (Vector(3) << sigma, 0.0, 0.0); + Vector mu = (Vector(3) << 200.0, 300.0, 400.0); actual = Constrained::All(d); EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu())); @@ -173,12 +173,12 @@ TEST(NoiseModel, ConstrainedConstructors ) /* ************************************************************************* */ TEST(NoiseModel, ConstrainedMixed ) { - Vector feasible = (Vec(3) << 1.0, 0.0, 1.0), - infeasible = (Vec(3) << 1.0, 1.0, 1.0); - Diagonal::shared_ptr d = Constrained::MixedSigmas((Vec(3) << sigma, 0.0, sigma)); + Vector feasible = (Vector(3) << 1.0, 0.0, 1.0), + infeasible = (Vector(3) << 1.0, 1.0, 1.0); + Diagonal::shared_ptr d = Constrained::MixedSigmas((Vector(3) << sigma, 0.0, sigma)); // NOTE: we catch constrained variables elsewhere, so whitening does nothing - EXPECT(assert_equal((Vec(3) << 0.5, 1.0, 0.5),d->whiten(infeasible))); - EXPECT(assert_equal((Vec(3) << 0.5, 0.0, 0.5),d->whiten(feasible))); + EXPECT(assert_equal((Vector(3) << 0.5, 1.0, 0.5),d->whiten(infeasible))); + EXPECT(assert_equal((Vector(3) << 0.5, 0.0, 0.5),d->whiten(feasible))); DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9); DOUBLES_EQUAL(0.5,d->distance(feasible),1e-9); @@ -187,13 +187,13 @@ TEST(NoiseModel, ConstrainedMixed ) /* ************************************************************************* */ TEST(NoiseModel, ConstrainedAll ) { - Vector feasible = (Vec(3) << 0.0, 0.0, 0.0), - infeasible = (Vec(3) << 1.0, 1.0, 1.0); + Vector feasible = (Vector(3) << 0.0, 0.0, 0.0), + infeasible = (Vector(3) << 1.0, 1.0, 1.0); Constrained::shared_ptr i = Constrained::All(3); // NOTE: we catch constrained variables elsewhere, so whitening does nothing - EXPECT(assert_equal((Vec(3) << 1.0, 1.0, 1.0),i->whiten(infeasible))); - EXPECT(assert_equal((Vec(3) << 0.0, 0.0, 0.0),i->whiten(feasible))); + EXPECT(assert_equal((Vector(3) << 1.0, 1.0, 1.0),i->whiten(infeasible))); + EXPECT(assert_equal((Vector(3) << 0.0, 0.0, 0.0),i->whiten(feasible))); DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9); DOUBLES_EQUAL(0.0,i->distance(feasible),1e-9); @@ -202,15 +202,15 @@ TEST(NoiseModel, ConstrainedAll ) /* ************************************************************************* */ namespace exampleQR { // create a matrix to eliminate - Matrix Ab = (Mat(4, 7) << + Matrix Ab = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., -0.1); - Vector sigmas = (Vec(4) << 0.2, 0.2, 0.1, 0.1); + Vector sigmas = (Vector(4) << 0.2, 0.2, 0.1, 0.1); // the matrix AB yields the following factorized version: - Matrix Rd = (Mat(4, 7) << + Matrix Rd = (Matrix(4, 7) << 11.1803, 0.0, -2.23607, 0.0, -8.94427, 0.0, 2.23607, 0.0, 11.1803, 0.0, -2.23607, 0.0, -8.94427,-1.56525, 0.0, 0.0, 4.47214, 0.0, -4.47214, 0.0, 0.0, @@ -225,7 +225,7 @@ TEST( NoiseModel, QR ) Matrix Ab2 = exampleQR::Ab; // otherwise overwritten ! // Expected result - Vector expectedSigmas = (Vec(4) << 0.0894427, 0.0894427, 0.223607, 0.223607); + Vector expectedSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607); SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas); // Call Gaussian version @@ -238,7 +238,7 @@ TEST( NoiseModel, QR ) SharedDiagonal actual2 = constrained->QR(Ab2); SharedDiagonal expectedModel2 = noiseModel::Diagonal::Sigmas(expectedSigmas); EXPECT(assert_equal(*expectedModel2,*actual2,1e-6)); - Matrix expectedRd2 = (Mat(4, 7) << + Matrix expectedRd2 = (Matrix(4, 7) << 1., 0., -0.2, 0., -0.8, 0., 0.2, 0., 1., 0.,-0.2, 0., -0.8,-0.14, 0., 0., 1., 0., -1., 0., 0.0, @@ -250,10 +250,10 @@ TEST( NoiseModel, QR ) TEST(NoiseModel, QRNan ) { SharedDiagonal constrained = noiseModel::Constrained::All(2); - Matrix Ab = (Mat(2, 5) << 1., 2., 1., 2., 3., 2., 1., 2., 4., 4.); + Matrix Ab = (Matrix(2, 5) << 1., 2., 1., 2., 3., 2., 1., 2., 4., 4.); SharedDiagonal expected = noiseModel::Constrained::All(2); - Matrix expectedAb = (Mat(2, 5) << 1., 2., 1., 2., 3., 0., 1., 0., 0., 2.0/3); + Matrix expectedAb = (Matrix(2, 5) << 1., 2., 1., 2., 3., 0., 1., 0., 0., 2.0/3); SharedDiagonal actual = constrained->QR(Ab); EXPECT(assert_equal(*expected,*actual)); @@ -281,7 +281,7 @@ TEST(NoiseModel, ScalarOrVector ) /* ************************************************************************* */ TEST(NoiseModel, WhitenInPlace) { - Vector sigmas = (Vec(3) << 0.1, 0.1, 0.1); + Vector sigmas = (Vector(3) << 0.1, 0.1, 0.1); SharedDiagonal model = Diagonal::Sigmas(sigmas); Matrix A = eye(3); model->WhitenInPlace(A); @@ -304,8 +304,8 @@ TEST(NoiseModel, robustFunction) TEST(NoiseModel, robustNoise) { const double k = 10.0, error1 = 1.0, error2 = 100.0; - Matrix A = (Mat(2, 2) << 1.0, 10.0, 100.0, 1000.0); - Vector b = (Vec(2) << error1, error2); + Matrix A = (Matrix(2, 2) << 1.0, 10.0, 100.0, 1000.0); + Vector b = (Vector(2) << error1, error2); const Robust::shared_ptr robust = Robust::Create( mEstimator::Huber::Create(k, mEstimator::Huber::Scalar), Unit::Create(2)); diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index 7c688b285..970cdfe24 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -49,10 +49,10 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); /* ************************************************************************* */ // example noise models -static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.2, 0.3)); +static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3)); static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3)); static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2); -static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas((Vec(3) << 0.0, 0.0, 0.1)); +static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas((Vector(3) << 0.0, 0.0, 0.1)); static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); /* ************************************************************************* */ @@ -135,9 +135,9 @@ BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional /* ************************************************************************* */ TEST (Serialization, linear_factors) { VectorValues values; - values.insert(0, (Vec(1) << 1.0)); - values.insert(1, (Vec(2) << 2.0,3.0)); - values.insert(2, (Vec(2) << 4.0,5.0)); + values.insert(0, (Vector(1) << 1.0)); + values.insert(1, (Vector(2) << 2.0,3.0)); + values.insert(2, (Vector(2) << 4.0,5.0)); EXPECT(equalsObj(values)); EXPECT(equalsXML(values)); EXPECT(equalsBinary(values)); @@ -145,7 +145,7 @@ TEST (Serialization, linear_factors) { Key i1 = 4, i2 = 7; Matrix A1 = eye(3), A2 = -1.0 * eye(3); Vector b = ones(3); - SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vec(3) << 1.0, 2.0, 3.0)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1.0, 2.0, 3.0)); JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); EXPECT(equalsObj(jacobianfactor)); EXPECT(equalsXML(jacobianfactor)); @@ -159,9 +159,9 @@ TEST (Serialization, linear_factors) { /* ************************************************************************* */ TEST (Serialization, gaussian_conditional) { - Matrix A1 = (Mat(2, 2) << 1., 2., 3., 4.); - Matrix A2 = (Mat(2, 2) << 6., 0.2, 8., 0.4); - Matrix R = (Mat(2, 2) << 0.1, 0.3, 0.0, 0.34); + Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.); + Matrix A2 = (Matrix(2, 2) << 6., 0.2, 8., 0.4); + Matrix R = (Matrix(2, 2) << 0.1, 0.3, 0.0, 0.34); Vector d(2); d << 0.2, 0.5; GaussianConditional cg(0, d, R, 1, A1, 2, A2); @@ -174,9 +174,9 @@ TEST (Serialization, gaussian_conditional) { TEST (Serialization, gaussian_factor_graph) { GaussianFactorGraph graph; { - Matrix A1 = (Mat(2, 2) << 1., 2., 3., 4.); - Matrix A2 = (Mat(2, 2) << 6., 0.2, 8., 0.4); - Matrix R = (Mat(2, 2) << 0.1, 0.3, 0.0, 0.34); + Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.); + Matrix A2 = (Matrix(2, 2) << 6., 0.2, 8., 0.4); + Matrix R = (Matrix(2, 2) << 0.1, 0.3, 0.0, 0.34); Vector d(2); d << 0.2, 0.5; GaussianConditional cg(0, d, R, 1, A1, 2, A2); graph.push_back(cg); @@ -186,7 +186,7 @@ TEST (Serialization, gaussian_factor_graph) { Key i1 = 4, i2 = 7; Matrix A1 = eye(3), A2 = -1.0 * eye(3); Vector b = ones(3); - SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vec(3) << 1.0, 2.0, 3.0)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1.0, 2.0, 3.0)); JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); HessianFactor hessianfactor(jacobianfactor); graph.push_back(jacobianfactor); @@ -203,10 +203,10 @@ TEST (Serialization, gaussian_bayes_tree) { const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4)); const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5); const GaussianFactorGraph chain = list_of - (JacobianFactor(x2, (Mat(1, 1) << 1.), x1, (Mat(1, 1) << 1.), (Vec(1) << 1.), chainNoise)) - (JacobianFactor(x2, (Mat(1, 1) << 1.), x3, (Mat(1, 1) << 1.), (Vec(1) << 1.), chainNoise)) - (JacobianFactor(x3, (Mat(1, 1) << 1.), x4, (Mat(1, 1) << 1.), (Vec(1) << 1.), chainNoise)) - (JacobianFactor(x4, (Mat(1, 1) << 1.), (Vec(1) << 1.), chainNoise)); + (JacobianFactor(x2, (Matrix(1, 1) << 1.), x1, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)) + (JacobianFactor(x2, (Matrix(1, 1) << 1.), x3, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)) + (JacobianFactor(x3, (Matrix(1, 1) << 1.), x4, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)) + (JacobianFactor(x4, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise)); GaussianBayesTree init = *chain.eliminateMultifrontal(chainOrdering); GaussianBayesTree expected = *chain.eliminateMultifrontal(chainOrdering); diff --git a/gtsam/linear/tests/testVectorValuesUnordered.cpp b/gtsam/linear/tests/testVectorValuesUnordered.cpp index 74e0b2ca2..75114921b 100644 --- a/gtsam/linear/tests/testVectorValuesUnordered.cpp +++ b/gtsam/linear/tests/testVectorValuesUnordered.cpp @@ -36,10 +36,10 @@ TEST(VectorValues, basics) // insert VectorValues actual; - actual.insert(0, (Vec(1) << 1)); - actual.insert(1, (Vec(2) << 2, 3)); - actual.insert(5, (Vec(2) << 6, 7)); - actual.insert(2, (Vec(2) << 4, 5)); + actual.insert(0, (Vector(1) << 1)); + actual.insert(1, (Vector(2) << 2, 3)); + actual.insert(5, (Vector(2) << 6, 7)); + actual.insert(2, (Vector(2) << 4, 5)); // Check dimensions LONGS_EQUAL(4, actual.size()); @@ -58,12 +58,12 @@ TEST(VectorValues, basics) EXPECT(!actual.exists(6)); // Check values - EXPECT(assert_equal((Vec(1) << 1), actual[0])); - EXPECT(assert_equal((Vec(2) << 2, 3), actual[1])); - EXPECT(assert_equal((Vec(2) << 4, 5), actual[2])); - EXPECT(assert_equal((Vec(2) << 6, 7), actual[5])); + EXPECT(assert_equal((Vector(1) << 1), actual[0])); + EXPECT(assert_equal((Vector(2) << 2, 3), actual[1])); + EXPECT(assert_equal((Vector(2) << 4, 5), actual[2])); + EXPECT(assert_equal((Vector(2) << 6, 7), actual[5])); FastVector keys = list_of(0)(1)(2)(5); - EXPECT(assert_equal((Vec(7) << 1, 2, 3, 4, 5, 6, 7), actual.vector(keys))); + EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7), actual.vector(keys))); // Check exceptions CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument); @@ -74,18 +74,18 @@ TEST(VectorValues, basics) TEST(VectorValues, combine) { VectorValues expected; - expected.insert(0, (Vec(1) << 1)); - expected.insert(1, (Vec(2) << 2, 3)); - expected.insert(5, (Vec(2) << 6, 7)); - expected.insert(2, (Vec(2) << 4, 5)); + expected.insert(0, (Vector(1) << 1)); + expected.insert(1, (Vector(2) << 2, 3)); + expected.insert(5, (Vector(2) << 6, 7)); + expected.insert(2, (Vector(2) << 4, 5)); VectorValues first; - first.insert(0, (Vec(1) << 1)); - first.insert(1, (Vec(2) << 2, 3)); + first.insert(0, (Vector(1) << 1)); + first.insert(1, (Vector(2) << 2, 3)); VectorValues second; - second.insert(5, (Vec(2) << 6, 7)); - second.insert(2, (Vec(2) << 4, 5)); + second.insert(5, (Vector(2) << 6, 7)); + second.insert(2, (Vector(2) << 4, 5)); VectorValues actual(first, second); @@ -96,14 +96,14 @@ TEST(VectorValues, combine) TEST(VectorValues, subvector) { VectorValues init; - init.insert(10, (Vec(1) << 1)); - init.insert(11, (Vec(2) << 2, 3)); - init.insert(12, (Vec(2) << 4, 5)); - init.insert(13, (Vec(2) << 6, 7)); + init.insert(10, (Vector(1) << 1)); + init.insert(11, (Vector(2) << 2, 3)); + init.insert(12, (Vector(2) << 4, 5)); + init.insert(13, (Vector(2) << 6, 7)); std::vector keys; keys += 10, 12, 13; - Vector expSubVector = (Vec(5) << 1, 4, 5, 6, 7); + Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7); EXPECT(assert_equal(expSubVector, init.vector(keys))); } @@ -111,16 +111,16 @@ TEST(VectorValues, subvector) TEST(VectorValues, LinearAlgebra) { VectorValues test1; - test1.insert(0, (Vec(1) << 1)); - test1.insert(1, (Vec(2) << 2, 3)); - test1.insert(5, (Vec(2) << 6, 7)); - test1.insert(2, (Vec(2) << 4, 5)); + test1.insert(0, (Vector(1) << 1)); + test1.insert(1, (Vector(2) << 2, 3)); + test1.insert(5, (Vector(2) << 6, 7)); + test1.insert(2, (Vector(2) << 4, 5)); VectorValues test2; - test2.insert(0, (Vec(1) << 6)); - test2.insert(1, (Vec(2) << 1, 6)); - test2.insert(5, (Vec(2) << 4, 3)); - test2.insert(2, (Vec(2) << 1, 8)); + test2.insert(0, (Vector(1) << 6)); + test2.insert(1, (Vector(2) << 1, 6)); + test2.insert(5, (Vector(2) << 4, 3)); + test2.insert(2, (Vector(2) << 1, 8)); // Dot product double dotExpected = test1.vector().dot(test2.vector()); @@ -175,10 +175,10 @@ TEST(VectorValues, convert) x << 1, 2, 3, 4, 5, 6, 7; VectorValues expected; - expected.insert(0, (Vec(1) << 1)); - expected.insert(1, (Vec(2) << 2, 3)); - expected.insert(2, (Vec(2) << 4, 5)); - expected.insert(5, (Vec(2) << 6, 7)); + expected.insert(0, (Vector(1) << 1)); + expected.insert(1, (Vector(2) << 2, 3)); + expected.insert(2, (Vector(2) << 4, 5)); + expected.insert(5, (Vector(2) << 6, 7)); std::map dims; dims.insert(make_pair(0,1)); @@ -200,11 +200,11 @@ TEST(VectorValues, convert) TEST(VectorValues, vector_sub) { VectorValues vv; - vv.insert(0, (Vec(1) << 1)); - vv.insert(1, (Vec(2) << 2, 3)); - vv.insert(2, (Vec(2) << 4, 5)); - vv.insert(5, (Vec(2) << 6, 7)); - vv.insert(7, (Vec(2) << 8, 9)); + vv.insert(0, (Vector(1) << 1)); + vv.insert(1, (Vector(2) << 2, 3)); + vv.insert(2, (Vector(2) << 4, 5)); + vv.insert(5, (Vector(2) << 6, 7)); + vv.insert(7, (Vector(2) << 8, 9)); std::map dims; dims.insert(make_pair(0,1)); diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index af1403d06..f5b5fd234 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -25,8 +25,8 @@ using namespace gtsam; /* ************************************************************************* */ TEST( ImuBias, Constructor) { - Vector bias_acc((Vec(3) << 0.1,0.2,0.4)); - Vector bias_gyro((Vec(3) << -0.2, 0.5, 0.03)); + Vector bias_acc((Vector(3) << 0.1,0.2,0.4)); + Vector bias_gyro((Vector(3) << -0.2, 0.5, 0.03)); // Default Constructor gtsam::imuBias::ConstantBias bias1; diff --git a/gtsam/nonlinear/WhiteNoiseFactor.h b/gtsam/nonlinear/WhiteNoiseFactor.h index 3d8e5b804..852ad147c 100644 --- a/gtsam/nonlinear/WhiteNoiseFactor.h +++ b/gtsam/nonlinear/WhiteNoiseFactor.h @@ -75,11 +75,11 @@ namespace gtsam { Key j1, Key j2) { double e = u - z, e2 = e * e; double c = 2 * logSqrt2PI - log(p) + e2 * p; - Vector g1 = (Vec(1) << -e * p); - Vector g2 = (Vec(1) << 0.5 / p - 0.5 * e2); - Matrix G11 = (Mat(1, 1) << p); - Matrix G12 = (Mat(1, 1) << e); - Matrix G22 = (Mat(1, 1) << 0.5 / (p * p)); + Vector g1 = (Vector(1) << -e * p); + Vector g2 = (Vector(1) << 0.5 / p - 0.5 * e2); + Matrix G11 = (Matrix(1, 1) << p); + Matrix G12 = (Matrix(1, 1) << e); + Matrix G22 = (Matrix(1, 1) << 0.5 / (p * p)); return HessianFactor::shared_ptr( new HessianFactor(j1, j2, G11, G12, g1, G22, g2, c)); } @@ -137,7 +137,7 @@ namespace gtsam { * TODO: Where is this used? should disappear. */ virtual Vector unwhitenedError(const Values& x) const { - return (Vec(1) << std::sqrt(2 * error(x))); + return (Vector(1) << std::sqrt(2 * error(x))); } /** diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index e239b4ba5..bf75f134b 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -30,13 +30,13 @@ Pose2 poseA1(0.0, 0.0, 0.0), poseA2(2.0, 0.0, 0.0); /* ************************************************************************* */ TEST( testLinearContainerFactor, generic_jacobian_factor ) { - Matrix A1 = (Mat(2, 2) << + Matrix A1 = (Matrix(2, 2) << 2.74222, -0.0067457, 0.0, 2.63624); - Matrix A2 = (Mat(2, 2) << + Matrix A2 = (Matrix(2, 2) << -0.0455167, -0.0443573, -0.0222154, -0.102489); - Vector b = (Vec(2) << 0.0277052, + Vector b = (Vector(2) << 0.0277052, -0.0533393); JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2); @@ -64,13 +64,13 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) { /* ************************************************************************* */ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { - Matrix A1 = (Mat(2, 2) << + Matrix A1 = (Matrix(2, 2) << 2.74222, -0.0067457, 0.0, 2.63624); - Matrix A2 = (Mat(2, 2) << + Matrix A2 = (Matrix(2, 2) << -0.0455167, -0.0443573, -0.0222154, -0.102489); - Vector b = (Vec(2) << 0.0277052, + Vector b = (Vector(2) << 0.0277052, -0.0533393); JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2); @@ -97,8 +97,8 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { EXPECT(assert_equal(expLinPoint, *actFactor.linearizationPoint())); // Check error evaluation - Vector delta_l1 = (Vec(2) << 1.0, 2.0); - Vector delta_l2 = (Vec(2) << 3.0, 4.0); + Vector delta_l1 = (Vector(2) << 1.0, 2.0); + Vector delta_l2 = (Vector(2) << 3.0, 4.0); VectorValues delta = values.zeroVectors(); delta.at(l1) = delta_l1; @@ -117,22 +117,22 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { /* ************************************************************************* */ TEST( testLinearContainerFactor, generic_hessian_factor ) { - Matrix G11 = (Mat(1, 1) << 1.0); - Matrix G12 = (Mat(1, 2) << 2.0, 4.0); - Matrix G13 = (Mat(1, 3) << 3.0, 6.0, 9.0); + Matrix G11 = (Matrix(1, 1) << 1.0); + Matrix G12 = (Matrix(1, 2) << 2.0, 4.0); + Matrix G13 = (Matrix(1, 3) << 3.0, 6.0, 9.0); - Matrix G22 = (Mat(2, 2) << 3.0, 5.0, + Matrix G22 = (Matrix(2, 2) << 3.0, 5.0, 0.0, 6.0); - Matrix G23 = (Mat(2, 3) << 4.0, 6.0, 8.0, + Matrix G23 = (Matrix(2, 3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0); - Matrix G33 = (Mat(3, 3) << 1.0, 2.0, 3.0, + Matrix G33 = (Matrix(3, 3) << 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0); - Vector g1 = (Vec(1) << -7.0); - Vector g2 = (Vec(2) << -8.0, -9.0); - Vector g3 = (Vec(3) << 1.0, 2.0, 3.0); + Vector g1 = (Vector(1) << -7.0); + Vector g2 = (Vector(2) << -8.0, -9.0); + Vector g3 = (Vector(3) << 1.0, 2.0, 3.0); double f = 10.0; @@ -158,21 +158,21 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { // 2 variable example, one pose, one landmark (planar) // Initial ordering: x1, l1 - Matrix G11 = (Mat(3, 3) << + Matrix G11 = (Matrix(3, 3) << 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0); - Matrix G12 = (Mat(3, 2) << + Matrix G12 = (Matrix(3, 2) << 1.0, 2.0, 3.0, 5.0, 4.0, 6.0); - Vector g1 = (Vec(3) << 1.0, 2.0, 3.0); + Vector g1 = (Vector(3) << 1.0, 2.0, 3.0); - Matrix G22 = (Mat(2, 2) << + Matrix G22 = (Matrix(2, 2) << 0.5, 0.2, 0.0, 0.6); - Vector g2 = (Vec(2) << -8.0, -9.0); + Vector g2 = (Vector(2) << -8.0, -9.0); double f = 10.0; @@ -197,16 +197,16 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { EXPECT(assert_equal(expLinPoints, actLinPoint)); // Create delta - Vector delta_l1 = (Vec(2) << 1.0, 2.0); - Vector delta_x1 = (Vec(3) << 3.0, 4.0, 0.5); - Vector delta_x2 = (Vec(3) << 6.0, 7.0, 0.3); + Vector delta_l1 = (Vector(2) << 1.0, 2.0); + Vector delta_x1 = (Vector(3) << 3.0, 4.0, 0.5); + Vector delta_x2 = (Vector(3) << 6.0, 7.0, 0.3); // Check error calculation VectorValues delta = linearizationPoint.zeroVectors(); delta.at(l1) = delta_l1; delta.at(x1) = delta_x1; delta.at(x2) = delta_x2; - EXPECT(assert_equal((Vec(5) << 3.0, 4.0, 0.5, 1.0, 2.0), delta.vector(initFactor.keys()))); + EXPECT(assert_equal((Vector(5) << 3.0, 4.0, 0.5, 1.0, 2.0), delta.vector(initFactor.keys()))); Values noisyValues = linearizationPoint.retract(delta); double expError = initFactor.error(delta); @@ -214,7 +214,7 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { EXPECT_DOUBLES_EQUAL(initFactor.error(linearizationPoint.zeroVectors()), actFactor.error(linearizationPoint), tol); // Compute updated versions - Vector dv = (Vec(5) << 3.0, 4.0, 0.5, 1.0, 2.0); + Vector dv = (Vector(5) << 3.0, 4.0, 0.5, 1.0, 2.0); Vector g(5); g << g1, g2; Vector g_prime = g - G.selfadjointView() * dv; diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 55faf61ca..636789652 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -171,8 +171,8 @@ TEST(Values, expmap_a) config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); VectorValues increment = pair_list_of - (key1, (Vec(3) << 1.0, 1.1, 1.2)) - (key2, (Vec(3) << 1.3, 1.4, 1.5)); + (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)); @@ -189,7 +189,7 @@ TEST(Values, expmap_b) config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); VectorValues increment = pair_list_of - (key2, (Vec(3) << 1.3, 1.4, 1.5)); + (key2, (Vector(3) << 1.3, 1.4, 1.5)); Values expected; expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); @@ -242,8 +242,8 @@ TEST(Values, localCoordinates) valuesA.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); VectorValues expDelta = pair_list_of - (key1, (Vec(3) << 0.1, 0.2, 0.3)) - (key2, (Vec(3) << 0.4, 0.5, 0.6)); + (key1, (Vector(3) << 0.1, 0.2, 0.3)) + (key2, (Vector(3) << 0.4, 0.5, 0.6)); Values valuesB = valuesA.retract(expDelta); @@ -275,11 +275,11 @@ TEST(Values, extract_keys) TEST(Values, exists_) { Values config0; - config0.insert(key1, LieVector((Vec(1) << 1.))); - config0.insert(key2, LieVector((Vec(1) << 2.))); + config0.insert(key1, LieVector((Vector(1) << 1.))); + config0.insert(key2, LieVector((Vector(1) << 2.))); boost::optional v = config0.exists(key1); - CHECK(assert_equal((Vec(1) << 1.),*v)); + CHECK(assert_equal((Vector(1) << 1.),*v)); } /* ************************************************************************* */ diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index a1c263099..78b8abd1f 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -74,9 +74,9 @@ struct BoundingConstraint1: public NoiseModelFactor1 { } if (isGreaterThan_) - return (Vec(1) << error); + return (Vector(1) << error); else - return -1.0 * (Vec(1) << error); + return -1.0 * (Vector(1) << error); } private: @@ -147,9 +147,9 @@ struct BoundingConstraint2: public NoiseModelFactor2 { } if (isGreaterThan_) - return (Vec(1) << error); + return (Vector(1) << error); else - return -1.0 * (Vec(1) << error); + return -1.0 * (Vector(1) << error); } private: diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 4762373e9..43927757e 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -151,7 +151,7 @@ pair load2D( // SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart); if (!model) { - Vector variances = (Vec(3) << m(0, 0), m(1, 1), m(2, 2)); + Vector variances = (Vector(3) << m(0, 0), m(1, 1), m(2, 2)); model = noiseModel::Diagonal::Variances(variances, smart); } @@ -179,7 +179,7 @@ pair load2D( continue; noiseModel::Diagonal::shared_ptr measurementNoise = - noiseModel::Diagonal::Sigmas((Vec(2) << bearing_std, range_std)); + noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); *graph += BearingRangeFactor(id1, id2, bearing, range, measurementNoise); // Insert poses or points if they do not exist yet @@ -211,7 +211,7 @@ pair load2D( { double rangeVar = v1; double bearingVar = v1 / 10.0; - measurementNoise = noiseModel::Diagonal::Sigmas((Vec(2) << bearingVar, rangeVar)); + measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << bearingVar, rangeVar)); } else { @@ -386,7 +386,7 @@ pair load2D_robust( continue; noiseModel::Diagonal::shared_ptr measurementNoise = - noiseModel::Diagonal::Sigmas((Vec(2) << bearing_std, range_std)); + noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); *graph += BearingRangeFactor(id1, id2, bearing, range, measurementNoise); // Insert poses or points if they do not exist yet diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 89c95a9d9..b37b36546 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -86,7 +86,7 @@ static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); TEST( GeneralSFMFactor, equals ) { // Create two identical factors and make sure they're equal - Vector z = (Vec(2) << 323.,240.); + Vector z = (Vector(2) << 323.,240.); const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr @@ -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((Vec(2) << -3.0, 0.0), factor->unwhitenedError(values))); + EXPECT(assert_equal((Vector(2) << -3.0, 0.0), factor->unwhitenedError(values))); } static const double baseline = 5.0 ; @@ -309,7 +309,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { } else { - Vector delta = (Vec(11) << + Vector delta = (Vector(11) << rot_noise, rot_noise, rot_noise, // rotation trans_noise, trans_noise, trans_noise, // translation focal_noise, focal_noise, // f_x, f_y diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 79a6245d2..a0377f087 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -86,7 +86,7 @@ static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); TEST( GeneralSFMFactor_Cal3Bundler, equals ) { // Create two identical factors and make sure they're equal - Vector z = (Vec(2) << 323.,240.); + Vector z = (Vector(2) << 323.,240.); const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr @@ -111,7 +111,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, error ) { Pose3 x1(R,t1); values.insert(X(1), GeneralCamera(x1)); Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal((Vec(2) << -3.0, 0.0), factor->unwhitenedError(values))); + EXPECT(assert_equal((Vector(2) << -3.0, 0.0), factor->unwhitenedError(values))); } @@ -308,7 +308,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) { } else { - Vector delta = (Vec(9) << + Vector delta = (Vector(9) << rot_noise, rot_noise, rot_noise, // rotation trans_noise, trans_noise, trans_noise, // translation focal_noise, distort_noise, distort_noise // f, k1, k2 diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index ce5e34b06..b40da2e2a 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam/slam/tests/testPoseRotationPrior.cpp @@ -18,8 +18,8 @@ using namespace gtsam; -const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas((Vec(1) << 0.1)); -const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.2, 0.3)); +const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas((Vector(1) << 0.1)); +const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3)); typedef PoseRotationPrior Pose2RotationPrior; typedef PoseRotationPrior Pose3RotationPrior; @@ -30,7 +30,7 @@ const gtsam::Key poseKey = 1; // Pose3 examples const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0); -const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap((Vec(3) << 0.1, 0.2, 0.3)); +const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap((Vector(3) << 0.1, 0.2, 0.3)); // Pose2 examples const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); @@ -62,7 +62,7 @@ TEST( testPoseRotationFactor, level3_error ) { Pose3 pose1(rot3A, point3A); Pose3RotationPrior factor(poseKey, rot3C, model3); Matrix actH1; - EXPECT(assert_equal((Vec(3) << -0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vector(3) << -0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); @@ -84,7 +84,7 @@ TEST( testPoseRotationFactor, level2_error ) { Pose2 pose1(rot2A, point2A); Pose2RotationPrior factor(poseKey, rot2B, model1); Matrix actH1; - EXPECT(assert_equal((Vec(1) << -M_PI_2), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vector(1) << -M_PI_2), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); diff --git a/gtsam/slam/tests/testPoseTranslationPrior.cpp b/gtsam/slam/tests/testPoseTranslationPrior.cpp index 265ab3ef0..36d94c9a3 100644 --- a/gtsam/slam/tests/testPoseTranslationPrior.cpp +++ b/gtsam/slam/tests/testPoseTranslationPrior.cpp @@ -15,8 +15,8 @@ using namespace gtsam; -const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.2)); -const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.2, 0.3)); +const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.2)); +const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3)); typedef PoseTranslationPrior Pose2TranslationPrior; typedef PoseTranslationPrior Pose3TranslationPrior; @@ -59,7 +59,7 @@ TEST( testPoseTranslationFactor, level3_error ) { Pose3 pose1(rot3A, point3A); Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; - EXPECT(assert_equal((Vec(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); @@ -81,7 +81,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) { Pose3 pose1(rot3B, point3A); Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; - EXPECT(assert_equal((Vec(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); @@ -103,7 +103,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) { Pose3 pose1(rot3C, point3A); Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; - EXPECT(assert_equal((Vec(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); @@ -125,7 +125,7 @@ TEST( testPoseTranslationFactor, level2_error ) { Pose2 pose1(rot2A, point2A); Pose2TranslationPrior factor(poseKey, point2B, model2); Matrix actH1; - EXPECT(assert_equal((Vec(2) << -3.0,-4.0), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vector(2) << -3.0,-4.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index 63499900f..b3454269e 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -108,7 +108,7 @@ TEST( ProjectionFactor, Error ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is (-3.0, 0.0) pixels / UnitCovariance - Vector expectedError = (Vec(2) << -3.0, 0.0); + Vector expectedError = (Vector(2) << -3.0, 0.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -131,7 +131,7 @@ TEST( ProjectionFactor, ErrorWithTransform ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is (-3.0, 0.0) pixels / UnitCovariance - Vector expectedError = (Vec(2) << -3.0, 0.0); + Vector expectedError = (Vector(2) << -3.0, 0.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -154,8 +154,8 @@ TEST( ProjectionFactor, Jacobian ) { factor.evaluateError(pose, point, H1Actual, H2Actual); // The expected Jacobians - Matrix H1Expected = (Mat(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.); - Matrix H2Expected = (Mat(2, 3) << 92.376, 0., 0., 0., 92.376, 0.); + Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.); + Matrix H2Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); @@ -180,8 +180,8 @@ TEST( ProjectionFactor, JacobianWithTransform ) { factor.evaluateError(pose, point, H1Actual, H2Actual); // The expected Jacobians - Matrix H1Expected = (Mat(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376); - Matrix H2Expected = (Mat(2, 3) << 0., -92.376, 0., 0., 0., -92.376); + Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376); + Matrix H2Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); diff --git a/gtsam/slam/tests/testRangeFactor.cpp b/gtsam/slam/tests/testRangeFactor.cpp index 98ee0201f..4fa6164a1 100644 --- a/gtsam/slam/tests/testRangeFactor.cpp +++ b/gtsam/slam/tests/testRangeFactor.cpp @@ -118,7 +118,7 @@ TEST( RangeFactor, Error2D ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance - Vector expectedError = (Vec(1) << 0.295630141); + Vector expectedError = (Vector(1) << 0.295630141); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -143,7 +143,7 @@ TEST( RangeFactor, Error2DWithTransform ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance - Vector expectedError = (Vec(1) << 0.295630141); + Vector expectedError = (Vector(1) << 0.295630141); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -165,7 +165,7 @@ TEST( RangeFactor, Error3D ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance - Vector expectedError = (Vec(1) << 0.295630141); + Vector expectedError = (Vector(1) << 0.295630141); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -190,7 +190,7 @@ TEST( RangeFactor, Error3DWithTransform ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance - Vector expectedError = (Vec(1) << 0.295630141); + Vector expectedError = (Vector(1) << 0.295630141); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 64b1beda8..9411e3ccc 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -31,7 +31,7 @@ using namespace std; using namespace gtsam; -static Pose3 camera1((Matrix) (Mat(3, 3) << +static Pose3 camera1((Matrix) (Matrix(3, 3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. @@ -102,7 +102,7 @@ TEST( StereoFactor, Error ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance - Vector expectedError = (Vec(3) << -3.0, +2.0, -1.0); + Vector expectedError = (Vector(3) << -3.0, +2.0, -1.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -123,7 +123,7 @@ TEST( StereoFactor, ErrorWithTransform ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance - Vector expectedError = (Vec(3) << -3.0, +2.0, -1.0); + Vector expectedError = (Vector(3) << -3.0, +2.0, -1.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -144,10 +144,10 @@ TEST( StereoFactor, Jacobian ) { factor.evaluateError(pose, point, H1Actual, H2Actual); // The expected Jacobians - Matrix H1Expected = (Mat(3, 6) << 0.0, -625.0, 0.0, -100.0, 0.0, 0.0, + Matrix H1Expected = (Matrix(3, 6) << 0.0, -625.0, 0.0, -100.0, 0.0, 0.0, 0.0, -625.0, 0.0, -100.0, 0.0, -8.0, 625.0, 0.0, 0.0, 0.0, -100.0, 0.0); - Matrix H2Expected = (Mat(3, 3) << 100.0, 0.0, 0.0, + Matrix H2Expected = (Matrix(3, 3) << 100.0, 0.0, 0.0, 100.0, 0.0, 8.0, 0.0, 100.0, 0.0); @@ -172,10 +172,10 @@ TEST( StereoFactor, JacobianWithTransform ) { factor.evaluateError(pose, point, H1Actual, H2Actual); // The expected Jacobians - Matrix H1Expected = (Mat(3, 6) << -100.0, 0.0, 650.0, 0.0, 100.0, 0.0, + Matrix H1Expected = (Matrix(3, 6) << -100.0, 0.0, 650.0, 0.0, 100.0, 0.0, -100.0, -8.0, 649.2, -8.0, 100.0, 0.0, -10.0, -650.0, 0.0, 0.0, 0.0, 100.0); - Matrix H2Expected = (Mat(3, 3) << 0.0, -100.0, 0.0, + Matrix H2Expected = (Matrix(3, 3) << 0.0, -100.0, 0.0, 8.0, -100.0, 0.0, 0.0, 0.0, -100.0); diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 18666d117..4645af781 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -126,7 +126,7 @@ PoseRTV PoseRTV::planarDynamics(double vel_rate, double heading_rate, const Velocity3& v1 = v(); // Update vehicle heading - Rot3 r2 = r1.retract((Vec(3) << 0.0, 0.0, heading_rate * dt)); + Rot3 r2 = r1.retract((Vector(3) << 0.0, 0.0, heading_rate * dt)); const double yaw2 = r2.ypr()(0); // Update vehicle position @@ -150,7 +150,7 @@ PoseRTV PoseRTV::flyingDynamics( const Velocity3& v1 = v(); // Update vehicle heading (and normalise yaw) - Vector rot_rates = (Vec(3) << 0.0, pitch_rate, heading_rate); + Vector rot_rates = (Vector(3) << 0.0, pitch_rate, heading_rate); Rot3 r2 = r1.retract(rot_rates*dt); // Work out dynamics on platform @@ -165,7 +165,7 @@ PoseRTV PoseRTV::flyingDynamics( Point3 forward(forward_accel, 0.0, 0.0); Vector Acc_n = yaw_correction_bn.rotate(forward).vector() // applies locally forward force in the global frame - - drag * (Vec(3) << v1.x(), v1.y(), 0.0) // drag term dependent on v1 + - drag * (Vector(3) << v1.x(), v1.y(), 0.0) // drag term dependent on v1 + delta(3, 2, loss_lift - lift_control); // falling due to lift lost from pitch // Update Vehicle Position and Velocity diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index 59ef2b9de..ebc430277 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -126,7 +126,7 @@ public: Matrix D_gravityBody_gk; Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk); - Vector f_ext = (Vec(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()); + Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()); Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext; @@ -154,7 +154,7 @@ public: Vector pk_1 = Pose3::dExpInv_exp(-h_*xik_1).transpose()*Inertia_*xik_1; Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_)); - Vector f_ext = (Vec(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()); + Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()); Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext; diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 9807d92c9..be7078d9a 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -56,9 +56,9 @@ TEST( testIMUSystem, optimize_chain ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), (Vector)(Vec(3) << 2.0, 2.0, 0.0)), - pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), (Vector)(Vec(3) << 0.0, 0.0, 0.0)), - pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), (Vector)(Vec(3) << 2.0, 2.0, 0.0)); + pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), (Vector)(Vector(3) << 2.0, 2.0, 0.0)), + pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), (Vector)(Vector(3) << 0.0, 0.0, 0.0)), + pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), (Vector)(Vector(3) << 2.0, 2.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Unit::Create(6); @@ -103,9 +103,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vec(3) << 1.0, 0.0, 0.0)), - pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vec(3) << 1.0, 0.0, 0.0)), - pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vec(3) << 1.0, 0.0, 0.0)); + pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vector(3) << 1.0, 0.0, 0.0)), + pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vector(3) << 1.0, 0.0, 0.0)), + pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vector(3) << 1.0, 0.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0); diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index c6e9c8439..e024ce884 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -47,7 +47,7 @@ TEST( testPoseRTV, constructors ) { EXPECT(assert_equal(Velocity3(), state4.v(), tol)); EXPECT(assert_equal(Pose3(rot, pt), state4.pose(), tol)); - Vector vec_init = (Vec(9) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0, 0.4, 0.5, 0.6); + Vector vec_init = (Vector(9) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0, 0.4, 0.5, 0.6); PoseRTV state5(vec_init); EXPECT(assert_equal(pt, state5.t(), tol)); EXPECT(assert_equal(rot, state5.R(), tol)); @@ -83,7 +83,7 @@ TEST( testPoseRTV, Lie ) { EXPECT(assert_equal(state1, state1.retract(zero(9)), tol)); EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol)); - Vector delta = (Vec(9) << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3); + Vector delta = (Vector(9) << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3); Rot3 rot2 = rot.retract(repeat(3, 0.1)); Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4); Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3); @@ -99,7 +99,7 @@ TEST( testPoseRTV, dynamics_identities ) { PoseRTV x0, x1, x2, x3, x4; const double dt = 0.1; - Vector accel = (Vec(3) << 0.2, 0.0, 0.0), gyro = (Vec(3) << 0.0, 0.0, 0.2); + Vector accel = (Vector(3) << 0.2, 0.0, 0.0), gyro = (Vector(3) << 0.0, 0.0, 0.2); Vector imu01 = zero(6), imu12 = zero(6), imu23 = zero(6), imu34 = zero(6); x1 = x0.generalDynamics(accel, gyro, dt); @@ -181,14 +181,14 @@ TEST( testPoseRTV, transformed_from_2 ) { TEST(testPoseRTV, RRTMbn) { EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(zero(3)), tol)); EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()), tol)); - EXPECT(assert_equal(PoseRTV::RRTMbn((Vec(3) << 0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol)); + EXPECT(assert_equal(PoseRTV::RRTMbn((Vector(3) << 0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol)); } /* ************************************************************************* */ TEST(testPoseRTV, RRTMnb) { EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(zero(3)), tol)); EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()), tol)); - EXPECT(assert_equal(PoseRTV::RRTMnb((Vec(3) << 0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol)); + EXPECT(assert_equal(PoseRTV::RRTMnb((Vector(3) << 0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 53c09e457..5f25811c6 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -18,24 +18,24 @@ const double deg2rad = M_PI/180.0; //Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0)); Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0)); -//LieVector v1((Vec(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0)); -LieVector V1_w((Vec(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0)); +//LieVector v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0)); +LieVector V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0)); LieVector V1_g1 = g1.inverse().Adjoint(V1_w); Pose3 g2(g1.retract(h*V1_g1, Pose3::EXPMAP)); //LieVector v2 = Pose3::Logmap(g1.between(g2)); double mass = 100.0; -Vector gamma2 = (Vec(2) << 0.0, 0.0); // no shape -Vector u2 = (Vec(2) << 0.0, 0.0); // no control at time 2 +Vector gamma2 = (Vector(2) << 0.0, 0.0); // no shape +Vector u2 = (Vector(2) << 0.0, 0.0); // no control at time 2 double distT = 1.0; // distance from the body-centered x axis to the big top motor double distR = 5.0; // distance from the body-centered z axis to the small motor -Matrix Mass = diag((Vec(3) << mass, mass, mass)); -Matrix Inertia = diag((Vec(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass)); +Matrix Mass = diag((Vector(3) << mass, mass, mass)); +Matrix Inertia = diag((Vector(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass)); Vector computeFu(const Vector& gamma, const Vector& control) { double gamma_r = gamma(0), gamma_p = gamma(1); - Matrix F = (Mat(6, 2) << distT*sin(gamma_r), 0.0, + Matrix F = (Matrix(6, 2) << distT*sin(gamma_r), 0.0, distT*sin(gamma_p*cos(gamma_r)), 0.0, 0.0, distR, sin(gamma_p)*cos(gamma_r), 0.0, @@ -100,7 +100,7 @@ TEST( Reconstruction, evaluateError) { /* ************************************************************************* */ // Implement Newton-Euler equation for rigid body dynamics Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) { - Matrix W = Pose3::adjointMap((Vec(6) << Vb(0), Vb(1), Vb(2), 0., 0., 0.)); + Matrix W = Pose3::adjointMap((Vector(6) << Vb(0), Vb(1), Vb(2), 0., 0., 0.)); Vector dV = Inertia.inverse()*(Fb - W*Inertia*Vb); return dV; } diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp index 90b68dc0e..7188463b4 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp @@ -15,9 +15,9 @@ const Key x1 = 1, x2 = 2; const double dt = 1.0; PoseRTV origin, - pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), (Vector)(Vec(3) << 1.0, 0.0, 0.0)), + pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), (Vector)(Vector(3) << 1.0, 0.0, 0.0)), pose1a(Point3(0.5, 0.0, 0.0)), - pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), (Vector)(Vec(3) << 1.0, 0.0, 0.0)); + pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), (Vector)(Vector(3) << 1.0, 0.0, 0.0)); /* ************************************************************************* */ TEST( testVelocityConstraint, trapezoidal ) { diff --git a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp index 02ed2c1b5..3c9b8d129 100644 --- a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp +++ b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp @@ -84,7 +84,7 @@ int main(int argc, char** argv) { // Create a prior on the first pose, placing it at the origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); Key priorKey = 0; newFactors.push_back(PriorFactor(priorKey, priorMean, priorNoise)); newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior @@ -110,11 +110,11 @@ int main(int argc, char** argv) { // Add odometry factors from two different sources with different error stats Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); - noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.1, 0.05)); + noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); - noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.05, 0.05, 0.05)); + noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); // Unlike the fixed-lag versions, the concurrent filter implementation @@ -159,7 +159,7 @@ int main(int argc, char** argv) { Key loopKey1(1000 * (0.0)); Key loopKey2(1000 * (5.0)); Pose2 loopMeasurement = Pose2(9.5, 1.00, 0.00); - noiseModel::Diagonal::shared_ptr loopNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.25)); + noiseModel::Diagonal::shared_ptr loopNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.25)); NonlinearFactor::shared_ptr loopFactor(new BetweenFactor(loopKey1, loopKey2, loopMeasurement, loopNoise)); // This measurement cannot be added directly to the concurrent filter because it connects a filter state to a smoother state @@ -189,11 +189,11 @@ int main(int argc, char** argv) { // Add odometry factors from two different sources with different error stats Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); - noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.1, 0.05)); + noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); - noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.05, 0.05, 0.05)); + noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); // Unlike the fixed-lag versions, the concurrent filter implementation @@ -262,11 +262,11 @@ int main(int argc, char** argv) { // Add odometry factors from two different sources with different error stats Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); - noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.1, 0.05)); + noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); - noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.05, 0.05, 0.05)); + noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); // Unlike the fixed-lag versions, the concurrent filter implementation diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index 64df1dd64..0980630ad 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -78,7 +78,7 @@ int main(int argc, char** argv) { // Create a prior on the first pose, placing it at the origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); Key priorKey = 0; newFactors.push_back(PriorFactor(priorKey, priorMean, priorNoise)); newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior @@ -104,11 +104,11 @@ int main(int argc, char** argv) { // Add odometry factors from two different sources with different error stats Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); - noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.1, 0.05)); + noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); - noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.05, 0.05, 0.05)); + noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); // Update the smoothers with the new factors diff --git a/gtsam_unstable/geometry/BearingS2.cpp b/gtsam_unstable/geometry/BearingS2.cpp index 922309a6c..df9e14a36 100644 --- a/gtsam_unstable/geometry/BearingS2.cpp +++ b/gtsam_unstable/geometry/BearingS2.cpp @@ -29,7 +29,7 @@ BearingS2 BearingS2::fromDownwardsObservation(const Pose3& A, const Point3& B) { Matrix Cnb = A.rotation().matrix().transpose(); // Cbc = [0,0,1;0,1,0;-1,0,0]; - Matrix Cbc = (Mat(3,3) << + Matrix Cbc = (Matrix(3,3) << 0.,0.,1., 0.,1.,0., -1.,0.,0.); diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 9cf92cce5..c33505c54 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -134,7 +134,7 @@ public: double H34 = 0; double H35 = cos_phi/rho; - *H2 = J2 * (Mat(3, 5) << + *H2 = J2 * (Matrix(3, 5) << H11, H12, H13, H14, H15, H21, H22, H23, H24, H25, H31, H32, H33, H34, H35); @@ -143,7 +143,7 @@ public: double H16 = -cos_phi*cos_theta/rho2; double H26 = -cos_phi*sin_theta/rho2; double H36 = -sin_phi/rho2; - *H3 = J2 * (Mat(3, 1) << + *H3 = J2 * (Matrix(3, 1) << H16, H26, H36); diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam_unstable/geometry/triangulation.h index cf33a99ca..0cb9d4838 100644 --- a/gtsam_unstable/geometry/triangulation.h +++ b/gtsam_unstable/geometry/triangulation.h @@ -60,7 +60,7 @@ public: * @param rank_tol SVD rank tolerance * @return Triangulated Point3 */ -Point3 triangulateDLT(const std::vector& projection_matrices, const std::vector& measurements, double rank_tol); +GTSAM_UNSTABLE_EXPORT Point3 triangulateDLT(const std::vector& projection_matrices, const std::vector& measurements, double rank_tol); // Frank says: putting priors on poses and then optimizing is a terrible idea: we turn a 3dof problem into a much more difficult problem // We should have a projectionfactor that knows pose is fixed @@ -135,7 +135,7 @@ std::pair triangulationGraph( * @param landmarkKey to refer to landmark * @return refined Point3 */ -Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Key landmarkKey); +GTSAM_UNSTABLE_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Key landmarkKey); /** * Given an initial estimate , refine a point using measurements in several cameras diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp index e3a5d651e..d1a4b8125 100644 --- a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp @@ -60,8 +60,8 @@ TEST( BatchFixedLagSmoother, Example ) // SETDEBUG("BatchFixedLagSmoother calculateMarginalFactors", true); // Set up parameters - SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1)); - SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1)); + SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); + SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // Create a Fixed-Lag Smoother typedef BatchFixedLagSmoother::KeyTimestampMap Timestamps; diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index 4fd088d55..92332238a 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -39,13 +39,13 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx((Vec(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); -const Pose3 poseError( Rot3::RzRyRx((Vec(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vec(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); -const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vec(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); /* ************************************************************************* */ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) { diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index ee3b2b5da..812c1d143 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -39,13 +39,13 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx((Vec(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); -const Pose3 poseError( Rot3::RzRyRx((Vec(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vec(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); -const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vec(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); /* ************************************************************************* */ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) { diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index f43332c1a..34f768562 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -38,14 +38,14 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx((Vec(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); -//const Pose3 poseError( Rot3::RzRyRx((Vec(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); -const Pose3 poseError( Rot3::RzRyRx((Vec(3) << 0.1, 0.02, -0.1)), Point3(0.5, -0.05, 0.2) ); +const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +//const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.1, 0.02, -0.1)), Point3(0.5, -0.05, 0.2) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vec(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); -const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vec(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); /* ************************************************************************* */ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) { diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp index 46d099738..51c61b8ec 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -39,13 +39,13 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx((Vec(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); -const Pose3 poseError( Rot3::RzRyRx((Vec(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vec(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); -const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vec(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); /* ************************************************************************* */ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) { diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index b21eaa8f0..1e3bc86c6 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -38,13 +38,13 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx((Vec(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); -const Pose3 poseError( Rot3::RzRyRx((Vec(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vec(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); -const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vec(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); /* ************************************************************************* */ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) { diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index a770de6e0..afcd60ae3 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -59,8 +59,8 @@ TEST( IncrementalFixedLagSmoother, Example ) SETDEBUG("IncrementalFixedLagSmoother update", true); // Set up parameters - SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1)); - SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1)); + SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); + SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // Create a Fixed-Lag Smoother typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps; diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index a6e88ae61..848d3ae09 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -60,7 +60,7 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, sigmas_v_a_ = esqrt(T * Pa_.diagonal()); // gravity in nav frame - n_g_ = (Vec(3) << 0.0, 0.0, g_e); + n_g_ = (Vector(3) << 0.0, 0.0, g_e); n_g_cross_ = skewSymmetric(n_g_); // nav frame has Z down !!! } @@ -72,7 +72,7 @@ std::pair AHRS::initialize(double g_e) double sp = sin(mech0_.bRn().inverse().pitch()); double cy = cos(0.0); double sy = sin(0.0); - Matrix Omega_T = (Mat(3, 3) << cy * cp, -sy, 0.0, sy * cp, cy, 0.0, -sp, 0.0, 1.0); + Matrix Omega_T = (Matrix(3, 3) << cy * cp, -sy, 0.0, sy * cp, cy, 0.0, -sp, 0.0, 1.0); // Calculate Jacobian of roll/pitch/yaw wrpt (g1,g2,g3), see doc/ypr.nb Vector b_g = mech0_.b_g(g_e); @@ -82,7 +82,7 @@ std::pair AHRS::initialize(double g_e) double g23 = g2 * g2 + g3 * g3; double g123 = g1 * g1 + g23; double f = 1 / (std::sqrt(g23) * g123); - Matrix H_g = (Mat(3, 3) << + Matrix H_g = (Matrix(3, 3) << 0.0, g3 / g23, -(g2 / g23), // roll std::sqrt(g23) / g123, -f * (g1 * g2), -f * (g1 * g3), // pitch 0.0, 0.0, 0.0); // we don't know anything on yaw @@ -220,8 +220,8 @@ std::pair AHRS::aidGeneral( Matrix b_g = skewSymmetric(increment* f_previous); Matrix H = collect(3, &b_g, &I3, &Z3); // Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_)); -// Matrix R = diag((Vec(3) << 1.0, 0.2, 1.0)); // good for L_twice - Matrix R = diag((Vec(3) << 0.01, 0.0001, 0.01)); +// Matrix R = diag((Vector(3) << 1.0, 0.2, 1.0)); // good for L_twice + Matrix R = diag((Vector(3) << 0.01, 0.0001, 0.01)); // update the Kalman filter KalmanFilter::State updatedState = KF_.updateQ(state, H, z, R); diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 6cccbc1cd..b2174f8a9 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -557,12 +557,12 @@ public: static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) { - Matrix ENU_to_NED = (Mat(3, 3) << + Matrix ENU_to_NED = (Matrix(3, 3) << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0); - Matrix NED_to_ENU = (Mat(3, 3) << + Matrix NED_to_ENU = (Matrix(3, 3) << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0); @@ -613,7 +613,7 @@ public: Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); - Vector omega_earth_ECEF((Vec(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 @@ -627,7 +627,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 = (Vec(3) << 0.0, 0.0, -g_calc); + g_ENU = (Vector(3) << 0.0, 0.0, -g_calc); // Calculate rho @@ -636,7 +636,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 = (Vec(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){ diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 6c5e845b9..5f09ec216 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -481,12 +481,12 @@ public: static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) { - Matrix ENU_to_NED = (Mat(3, 3) << + Matrix ENU_to_NED = (Matrix(3, 3) << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0); - Matrix NED_to_ENU = (Mat(3, 3) << + Matrix NED_to_ENU = (Matrix(3, 3) << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0); diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 5376ff812..5ca736c01 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -290,12 +290,12 @@ public: static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) { - Matrix ENU_to_NED = (Mat(3, 3) << + Matrix ENU_to_NED = (Matrix(3, 3) << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0); - Matrix NED_to_ENU = (Mat(3, 3) << + Matrix NED_to_ENU = (Matrix(3, 3) << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0); @@ -346,7 +346,7 @@ public: Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); - Vector omega_earth_ECEF((Vec(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 @@ -360,7 +360,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 = (Vec(3) << 0.0, 0.0, -g_calc); + g_ENU = (Vector(3) << 0.0, 0.0, -g_calc); // Calculate rho @@ -369,7 +369,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 = (Vec(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){ diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 96d556767..aef15638f 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -99,7 +99,7 @@ public: << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return (Vec(1) << 0.0); + return (Vector(1) << 0.0); } /// Evaluate error h(x)-z and optionally derivatives @@ -218,7 +218,7 @@ public: << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return (Vec(1) << 0.0); + return (Vector(1) << 0.0); } /// Evaluate error h(x)-z and optionally derivatives diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index 882575650..a7464d96f 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -13,8 +13,8 @@ namespace gtsam { /* ************************************************************************* */ Mechanization_bRn2 Mechanization_bRn2::initializeVector(const std::list& U, const std::list& F, const double g_e, bool flat) { - Matrix Umat = (Mat(3, U.size()) << concatVectors(U)); - Matrix Fmat = (Mat(3, F.size()) << concatVectors(F)); + Matrix Umat = (Matrix(3, U.size()) << concatVectors(U)); + Matrix Fmat = (Matrix(3, F.size()) << concatVectors(F)); return initialize(Umat, Fmat, g_e, flat); } @@ -33,14 +33,14 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, if(g_e == 0) { if (flat) // acceleration measured is along the z-axis. - b_g = (Vec(3) << 0.0, 0.0, norm_2(meanF)); + b_g = (Vector(3) << 0.0, 0.0, norm_2(meanF)); else // acceleration measured is the opposite of gravity (10.13) b_g = -meanF; } else { if (flat) // gravity is downward along the z-axis since we are flat on the ground - b_g = (Vec(3) << 0.0,0.0,g_e); + b_g = (Vector(3) << 0.0,0.0,g_e); else // normalize b_g and attribute remainder to biases b_g = - g_e * meanF/meanF.norm(); diff --git a/gtsam_unstable/slam/tests/testAHRS.cpp b/gtsam_unstable/slam/tests/testAHRS.cpp index 1c6c4c474..cc3d693ad 100644 --- a/gtsam_unstable/slam/tests/testAHRS.cpp +++ b/gtsam_unstable/slam/tests/testAHRS.cpp @@ -16,22 +16,22 @@ using namespace std; using namespace gtsam; // stationary interval of gyro U and acc F -Matrix stationaryU = trans((Mat(3, 3) << -0.0004,-0.0002,-0.0014,0.0006,-0.0003,0.0007,0.0006,-0.0002,-0.0003)); -Matrix stationaryF = trans((Mat(3, 3) << 0.1152,-0.0188,9.7419,-0.0163,0.0146,9.7753,-0.0283,-0.0428,9.9021)); +Matrix stationaryU = trans((Matrix(3, 3) << -0.0004,-0.0002,-0.0014,0.0006,-0.0003,0.0007,0.0006,-0.0002,-0.0003)); +Matrix stationaryF = trans((Matrix(3, 3) << 0.1152,-0.0188,9.7419,-0.0163,0.0146,9.7753,-0.0283,-0.0428,9.9021)); double g_e = 9.7963; // Atlanta /* ************************************************************************* */ TEST (AHRS, cov) { // samples stored by row - Matrix A = (Mat(4, 3) << + Matrix A = (Matrix(4, 3) << 1.0, 2.0, 3.0, 5.0, 7.0, 0.0, 9.0, 4.0, 7.0, 6.0, 3.0, 2.0); Matrix actual = cov(trans(A)); - Matrix expected = (Mat(3, 3) << + Matrix expected = (Matrix(3, 3) << 10.9167, 2.3333, 5.0000, 2.3333, 4.6667, -2.6667, 5.0000, -2.6667, 8.6667); @@ -43,7 +43,7 @@ TEST (AHRS, cov) { TEST (AHRS, covU) { Matrix actual = cov(10000*stationaryU); - Matrix expected = (Mat(3, 3) << + Matrix expected = (Matrix(3, 3) << 33.3333333, -1.66666667, 53.3333333, -1.66666667, 0.333333333, -5.16666667, 53.3333333, -5.16666667, 110.333333); @@ -55,7 +55,7 @@ TEST (AHRS, covU) { TEST (AHRS, covF) { Matrix actual = cov(100*stationaryF); - Matrix expected = (Mat(3, 3) << + Matrix expected = (Matrix(3, 3) << 63.3808333, -0.432166667, -48.1706667, -0.432166667, 8.31053333, -16.6792667, -48.1706667, -16.6792667, 71.4297333); diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index 7fb16e4ab..340a3202a 100644 --- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp @@ -59,8 +59,8 @@ TEST( BetweenFactorEM, ConstructorAndEquals) gtsam::Pose2 rel_pose_ideal = p1.between(p2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 5, 5, 1.0))); double prior_outlier = 0.5; double prior_inlier = 0.5; @@ -88,8 +88,8 @@ TEST( BetweenFactorEM, EvaluateError) gtsam::Pose2 rel_pose_ideal = p1.between(p2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 50.0, 50.0, 10.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 50.0, 50.0, 10.0))); gtsam::Values values; values.insert(key1, p1); @@ -103,8 +103,8 @@ TEST( BetweenFactorEM, EvaluateError) Vector actual_err_wh = f.whitenedError(values); - Vector actual_err_wh_inlier = (Vec(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); - Vector actual_err_wh_outlier = (Vec(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); + Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + Vector actual_err_wh_outlier = (Vector(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); // cout << "Inlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "< h_EM(key1, key2, rel_pose_msr, model_inlier, model_outlier, prior_inlier, prior_outlier); actual_err_wh = h_EM.whitenedError(values); - actual_err_wh_inlier = (Vec(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); BetweenFactor h(key1, key2, rel_pose_msr, model_inlier ); Vector actual_err_wh_stnd = h.whitenedError(values); @@ -160,8 +160,8 @@ TEST (BetweenFactorEM, jacobian ) { gtsam::Pose2 rel_pose_ideal = p1.between(p2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 50.0, 50.0, 10.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 50.0, 50.0, 10.0))); gtsam::Values values; values.insert(key1, p1); @@ -182,7 +182,7 @@ TEST (BetweenFactorEM, jacobian ) { // compare to standard between factor BetweenFactor h(key1, key2, rel_pose_msr, model_inlier ); Vector actual_err_wh_stnd = h.whitenedError(values); - Vector actual_err_wh_inlier = (Vec(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); // CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8)); std::vector H_actual_stnd_unwh(2); (void)h.unwhitenedError(values, H_actual_stnd_unwh); @@ -223,8 +223,8 @@ TEST( BetweenFactorEM, CaseStudy) gtsam::Pose2 p2(-0.0491752554, -0.289649075, -0.328993962); gtsam::Pose2 rel_pose_msr(0.0316191379, 0.0247539161, 0.004102182); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 0.4021, 0.286, 0.428))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 4.9821, 4.614, 1.8387))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 0.4021, 0.286, 0.428))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 4.9821, 4.614, 1.8387))); gtsam::Values values; values.insert(key1, p1); @@ -244,8 +244,8 @@ TEST( BetweenFactorEM, CaseStudy) Vector actual_err_unw = f.unwhitenedError(values); Vector actual_err_wh = f.whitenedError(values); - Vector actual_err_wh_inlier = (Vec(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); - Vector actual_err_wh_outlier = (Vec(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); + Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + Vector actual_err_wh_outlier = (Vector(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); if (debug){ cout << "p_inlier_outler: "< f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); @@ -141,17 +141,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel) gtsam::Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // First test: zero angular motion, some acceleration - Vector measurement_acc((Vec(3) <<0.1,0.2,0.3-9.81)); - Vector measurement_gyro((Vec(3) << 0.0, 0.0, 0.0)); + Vector measurement_acc((Vector(3) <<0.1,0.2,0.3-9.81)); + Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); @@ -177,16 +177,16 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot) gtsam::Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // Second test: zero angular motion, some acceleration - Vector measurement_acc((Vec(3) <<0.0,0.0,0.0-9.81)); - Vector measurement_gyro((Vec(3) << 0.1, 0.2, 0.3)); + Vector measurement_acc((Vector(3) <<0.0,0.0,0.0-9.81)); + Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3)); InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); @@ -212,16 +212,16 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) gtsam::Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // Second test: zero angular motion, some acceleration - generated in matlab - Vector measurement_acc((Vec(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); - Vector measurement_gyro((Vec(3) << 0.1, 0.2, 0.3)); + Vector measurement_acc((Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); + Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3)); InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); @@ -254,9 +254,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) //} // //TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) { -// LieVector angles((Vec(3) << 3.001, -1.0004, 2.0005)); +// LieVector angles((Vector(3) << 3.001, -1.0004, 2.0005)); // Rot3 R1(Rot3().RzRyRx(angles)); -// LieVector q((Vec(3) << 5.8, -2.2, 4.105)); +// LieVector q((Vector(3) << 5.8, -2.2, 4.105)); // Rot3 qx(0.0, -q[2], q[1], // q[2], 0.0, -q[0], // -q[1], q[0],0.0); @@ -285,15 +285,15 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) { gtsam::Key BiasKey1(31); double measurement_dt(0.01); - Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - Vector measurement_acc((Vec(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); - Vector measurement_gyro((Vec(3) << 3.14, 3.14/2, -3.14)); + Vector measurement_acc((Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); + Vector measurement_gyro((Vector(3) << 3.14, 3.14/2, -3.14)); InertialNavFactor_GlobalVelocity factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); @@ -374,13 +374,13 @@ TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform) gtsam::Key Vel2(22); gtsam::Key Bias1(31); - Vector measurement_acc((Vec(3) << 0.1, 0.2, 0.4)); - Vector measurement_gyro((Vec(3) << -0.2, 0.5, 0.03)); + Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4)); + Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); double measurement_dt(0.1); - Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -400,13 +400,13 @@ TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform) gtsam::Key Vel2(22); gtsam::Key Bias1(31); - Vector measurement_acc((Vec(3) << 0.1, 0.2, 0.4)); - Vector measurement_gyro((Vec(3) << -0.2, 0.5, 0.03)); + Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4)); + Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); double measurement_dt(0.1); - Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -429,9 +429,9 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) gtsam::Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -440,9 +440,9 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) // First test: zero angular motion, some acceleration - Vector measurement_gyro((Vec(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation + Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = (Vec(3) << 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 + Vector measurement_acc = (Vector(3) << 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 InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); @@ -469,9 +469,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) gtsam::Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -480,9 +480,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) // First test: zero angular motion, some acceleration - Vector measurement_gyro((Vec(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation + Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = (Vec(3) << 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 + Vector measurement_acc = (Vector(3) << 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 InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); @@ -508,9 +508,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) gtsam::Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -519,9 +519,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) // Second test: zero angular motion, some acceleration - Vector measurement_gyro((Vec(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation + Vector measurement_gyro((Vector(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = (Vec(3) << 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 + Vector measurement_acc = (Vector(3) << 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 InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); @@ -547,9 +547,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) gtsam::Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -558,9 +558,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) // Second test: zero angular motion, some acceleration - generated in matlab - Vector measurement_gyro((Vec(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation + Vector measurement_gyro((Vector(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = (Vec(3) << -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 + Vector measurement_acc = (Vector(3) << -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 InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); @@ -575,7 +575,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) -0.636011287, 0.731761397, 0.244979388); Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); Pose3 Pose2(R2, t2); - Vector dv = measurement_dt * (R1.matrix() * body_P_sensor.rotation().matrix() * (Vec(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + world_g); + Vector dv = measurement_dt * (R1.matrix() * body_P_sensor.rotation().matrix() * (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + world_g); LieVector Vel2 = Vel1.compose( dv ); imuBias::ConstantBias Bias1; @@ -596,9 +596,9 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { gtsam::Key BiasKey1(31); double measurement_dt(0.01); - Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -606,9 +606,9 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation - Vector measurement_gyro((Vec(3) << 3.14/2, 3.14, +3.14)); // Measured in ENU orientation + Vector measurement_gyro((Vector(3) << 3.14/2, 3.14, +3.14)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = (Vec(3) << -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 + Vector measurement_acc = (Vector(3) << -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 InertialNavFactor_GlobalVelocity factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 0f092d803..7bbee65ee 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -66,9 +66,9 @@ TEST( InvDepthFactorVariant1, optimize) { // Create a values with slightly incorrect initial conditions Values values; - values.insert(poseKey1, pose1.retract((Vec(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); - values.insert(poseKey2, pose2.retract((Vec(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); - values.insert(landmarkKey, expected.retract((Vec(6) << -0.20, +0.20, -0.35, +0.02, -0.04, +0.05))); + values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); + values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); + values.insert(landmarkKey, expected.retract((Vector(6) << -0.20, +0.20, -0.35, +0.02, -0.04, +0.05))); // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index ffbbcc76b..1512d0fc2 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -64,9 +64,9 @@ TEST( InvDepthFactorVariant2, optimize) { // Create a values with slightly incorrect initial conditions Values values; - values.insert(poseKey1, pose1.retract((Vec(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); - values.insert(poseKey2, pose2.retract((Vec(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); - values.insert(landmarkKey, expected.retract((Vec(3) << +0.02, -0.04, +0.05))); + values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); + values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); + values.insert(landmarkKey, expected.retract((Vector(3) << +0.02, -0.04, +0.05))); // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index e901e6361..799ebdf36 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -64,9 +64,9 @@ TEST( InvDepthFactorVariant3, optimize) { // Create a values with slightly incorrect initial conditions Values values; - values.insert(poseKey1, pose1.retract((Vec(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); - values.insert(poseKey2, pose2.retract((Vec(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); - values.insert(landmarkKey, expected.retract((Vec(3) << +0.02, -0.04, +0.05))); + values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); + values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); + values.insert(landmarkKey, expected.retract((Vector(3) << +0.02, -0.04, +0.05))); // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp index b1326e66f..cbc1022fc 100644 --- a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp @@ -147,7 +147,7 @@ TEST( MultiProjectionFactor, create ){ // Vector actualError(factor.evaluateError(pose, point)); // // // The expected error is (-3.0, 0.0) pixels / UnitCovariance -// Vector expectedError = (Vec(2) << -3.0, 0.0); +// Vector expectedError = (Vector(2) << -3.0, 0.0); // // // Verify we get the expected error // CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -170,7 +170,7 @@ TEST( MultiProjectionFactor, create ){ // Vector actualError(factor.evaluateError(pose, point)); // // // The expected error is (-3.0, 0.0) pixels / UnitCovariance -// Vector expectedError = (Vec(2) << -3.0, 0.0); +// Vector expectedError = (Vector(2) << -3.0, 0.0); // // // Verify we get the expected error // CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -193,8 +193,8 @@ TEST( MultiProjectionFactor, create ){ // factor.evaluateError(pose, point, H1Actual, H2Actual); // // // The expected Jacobians -// Matrix H1Expected = (Mat(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.); -// Matrix H2Expected = (Mat(2, 3) << 92.376, 0., 0., 0., 92.376, 0.); +// Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.); +// Matrix H2Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.); // // // Verify the Jacobians are correct // CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); @@ -219,8 +219,8 @@ TEST( MultiProjectionFactor, create ){ // factor.evaluateError(pose, point, H1Actual, H2Actual); // // // The expected Jacobians -// Matrix H1Expected = (Mat(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376); -// Matrix H2Expected = (Mat(2, 3) << 0., -92.376, 0., 0., 0., -92.376); +// Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376); +// Matrix H2Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376); // // // Verify the Jacobians are correct // CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp index 2c843b0dd..ffc7344cf 100644 --- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp @@ -49,7 +49,7 @@ TEST( testRelativeElevationFactor, level_positive ) { double measured = 0.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; - EXPECT(assert_equal((Vec(1) << 2.0), factor.evaluateError(pose1, point1, actH1, actH2))); + EXPECT(assert_equal((Vector(1) << 2.0), factor.evaluateError(pose1, point1, actH1, actH2))); Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); Matrix expH2 = numericalDerivative22( @@ -64,7 +64,7 @@ TEST( testRelativeElevationFactor, level_negative ) { double measured = -1.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; - EXPECT(assert_equal((Vec(1) << 3.0), factor.evaluateError(pose1, point1, actH1, actH2))); + EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose1, point1, actH1, actH2))); Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); Matrix expH2 = numericalDerivative22( @@ -94,7 +94,7 @@ TEST( testRelativeElevationFactor, rotated_positive ) { double measured = 0.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; - EXPECT(assert_equal((Vec(1) << 2.0), factor.evaluateError(pose2, point1, actH1, actH2))); + EXPECT(assert_equal((Vector(1) << 2.0), factor.evaluateError(pose2, point1, actH1, actH2))); Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); Matrix expH2 = numericalDerivative22( @@ -109,7 +109,7 @@ TEST( testRelativeElevationFactor, rotated_negative1 ) { double measured = -1.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; - EXPECT(assert_equal((Vec(1) << 3.0), factor.evaluateError(pose2, point1, actH1, actH2))); + EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose2, point1, actH1, actH2))); Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); Matrix expH2 = numericalDerivative22( @@ -124,7 +124,7 @@ TEST( testRelativeElevationFactor, rotated_negative2 ) { double measured = -1.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; - EXPECT(assert_equal((Vec(1) << 3.0), factor.evaluateError(pose3, point1, actH1, actH2))); + EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose3, point1, actH1, actH2))); Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); Matrix expH2 = numericalDerivative22( diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index 179cbf13e..f645f5086 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -74,22 +74,22 @@ TEST( SmartRangeFactor, unwhitenedError ) { // Whenever there are two ranges or less, error should be zero Vector actual1 = f.unwhitenedError(values); - EXPECT(assert_equal((Vec(1) << 0.0), actual1)); + EXPECT(assert_equal((Vector(1) << 0.0), actual1)); f.addRange(2, r2); Vector actual2 = f.unwhitenedError(values); - EXPECT(assert_equal((Vec(1) << 0.0), actual2)); + EXPECT(assert_equal((Vector(1) << 0.0), actual2)); f.addRange(3, r3); vector H(3); Vector actual3 = f.unwhitenedError(values); EXPECT_LONGS_EQUAL(3, f.keys().size()); - EXPECT(assert_equal((Vec(1) << 0.0), actual3)); + EXPECT(assert_equal((Vector(1) << 0.0), actual3)); // Check keys and Jacobian Vector actual4 = f.unwhitenedError(values, H); // with H now ! - EXPECT(assert_equal((Vec(1) << 0.0), actual4)); - CHECK(assert_equal((Mat(1, 3) << 0.0,-1.0,0.0), H.front())); - CHECK(assert_equal((Mat(1, 3) << sqrt(2.0)/2,-sqrt(2.0)/2,0.0), H.back())); + EXPECT(assert_equal((Vector(1) << 0.0), actual4)); + CHECK(assert_equal((Matrix(1, 3) << 0.0,-1.0,0.0), H.front())); + CHECK(assert_equal((Matrix(1, 3) << sqrt(2.0)/2,-sqrt(2.0)/2,0.0), H.back())); // Test clone NonlinearFactor::shared_ptr clone = f.clone(); @@ -109,7 +109,7 @@ TEST( SmartRangeFactor, optimization ) { initial.insert(2, pose2); initial.insert(3, Pose2(5, 6, 0)); // does not satisfy range measurement Vector actual5 = f.unwhitenedError(initial); - EXPECT(assert_equal((Vec(1) << sqrt(25.0+16.0)-sqrt(50.0)), actual5)); + EXPECT(assert_equal((Vector(1) << sqrt(25.0+16.0)-sqrt(50.0)), actual5)); // Create Factor graph NonlinearFactorGraph graph; diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp index e4913e6b2..68060997b 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp @@ -60,8 +60,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, ConstructorAndEquals) gtsam::Pose2 rel_pose_ideal = p1.between(p2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vec(3) << 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0))); double prior_outlier = 0.5; double prior_inlier = 0.5; @@ -97,8 +97,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError) gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal; - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vec(3) << 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0))); double prior_outlier = 0.01; double prior_inlier = 0.99; @@ -136,8 +136,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError2) gtsam::Pose2 rel_pose_ideal = orgA_T_currA.between(orgA_T_currB); gtsam::Pose2 rel_pose_msr = rel_pose_ideal; - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vec(3) << 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0))); double prior_outlier = 0.01; double prior_inlier = 0.99; @@ -184,8 +184,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, Optimize) gtsam::Pose2 currA_Tmsr_currB3 = currA_Tmsr_currB1; gtsam::Pose2 currA_Tmsr_currB4 = currA_Tmsr_currB1; - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vec(3) << 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0))); double prior_outlier = 0.01; double prior_inlier = 0.99; @@ -245,8 +245,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian) gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vec(3) << 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0))); double prior_outlier = 0.5; double prior_inlier = 0.5; @@ -284,8 +284,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian) // gtsam::Pose2 rel_pose_ideal = p1.between(p2); // gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); // -// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 0.5, 0.5, 0.05))); -// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 50.0, 50.0, 10.0))); +// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 0.5, 0.5, 0.05))); +// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 50.0, 50.0, 10.0))); // // gtsam::Values values; // values.insert(keyA, p1); @@ -306,7 +306,7 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian) // // compare to standard between factor // BetweenFactor h(keyA, keyB, rel_pose_msr, model_inlier ); // Vector actual_err_wh_stnd = h.whitenedError(values); -// Vector actual_err_wh_inlier = (Vec(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); +// Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); // CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8)); // std::vector H_actual_stnd_unwh(2); // (void)h.unwhitenedError(values, H_actual_stnd_unwh); diff --git a/tests/smallExample.h b/tests/smallExample.h index 024afed65..cf9e382a4 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -223,9 +223,9 @@ Values createValues() { VectorValues createVectorValues() { using namespace impl; VectorValues c = boost::assign::pair_list_of - (_l1_, (Vec(2) << 0.0, -1.0)) - (_x1_, (Vec(2) << 0.0, 0.0)) - (_x2_, (Vec(2) << 1.5, 0.0)); + (_l1_, (Vector(2) << 0.0, -1.0)) + (_x1_, (Vector(2) << 0.0, 0.0)) + (_x2_, (Vector(2) << 1.5, 0.0)); return c; } @@ -250,9 +250,9 @@ VectorValues createCorrectDelta() { using symbol_shorthand::X; using symbol_shorthand::L; VectorValues c; - c.insert(L(1), (Vec(2) << -0.1, 0.1)); - c.insert(X(1), (Vec(2) << -0.1, -0.1)); - c.insert(X(2), (Vec(2) << 0.1, -0.2)); + c.insert(L(1), (Vector(2) << -0.1, 0.1)); + c.insert(X(1), (Vector(2) << -0.1, -0.1)); + c.insert(X(2), (Vector(2) << 0.1, -0.2)); return c; } @@ -278,13 +278,13 @@ GaussianFactorGraph createGaussianFactorGraph() { fg += JacobianFactor(X(1), 10*eye(2), -1.0*ones(2)); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(X(1), -10*eye(2), X(2), 10*eye(2), (Vec(2) << 2.0, -1.0)); + fg += JacobianFactor(X(1), -10*eye(2), X(2), 10*eye(2), (Vector(2) << 2.0, -1.0)); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(X(1), -5*eye(2), L(1), 5*eye(2), (Vec(2) << 0.0, 1.0)); + fg += JacobianFactor(X(1), -5*eye(2), L(1), 5*eye(2), (Vector(2) << 0.0, 1.0)); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(X(2), -5*eye(2), L(1), 5*eye(2), (Vec(2) << -1.0, 1.5)); + fg += JacobianFactor(X(2), -5*eye(2), L(1), 5*eye(2), (Vector(2) << -1.0, 1.5)); return fg; } @@ -297,8 +297,8 @@ GaussianFactorGraph createGaussianFactorGraph() { */ GaussianBayesNet createSmallGaussianBayesNet() { using namespace impl; - Matrix R11 = (Mat(1, 1) << 1.0), S12 = (Mat(1, 1) << 1.0); - Matrix R22 = (Mat(1, 1) << 1.0); + Matrix R11 = (Matrix(1, 1) << 1.0), S12 = (Matrix(1, 1) << 1.0); + Matrix R22 = (Matrix(1, 1) << 1.0); Vector d1(1), d2(1); d1(0) = 9; d2(0) = 5; @@ -323,7 +323,7 @@ Point2 h(const Point2& v) { } Matrix H(const Point2& v) { - return (Mat(2, 2) << + return (Matrix(2, 2) << -sin(v.x()), 0.0, 0.0, cos(v.y())); } @@ -350,7 +350,7 @@ boost::shared_ptr sharedReallyNonlinearFactorGraph() using symbol_shorthand::X; using symbol_shorthand::L; boost::shared_ptr fg(new NonlinearFactorGraph); - Vector z = (Vec(2) << 1.0, 0.0); + Vector z = (Vector(2) << 1.0, 0.0); double sigma = 0.1; boost::shared_ptr factor( new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); @@ -422,7 +422,7 @@ GaussianFactorGraph createSimpleConstraintGraph() { // |0 1||x_2| | 0 -1||y_2| |0| Matrix Ax1 = eye(2); Matrix Ay1 = eye(2) * -1; - Vector b2 = (Vec(2) << 0.0, 0.0); + Vector b2 = (Vector(2) << 0.0, 0.0); JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, constraintModel)); @@ -440,7 +440,7 @@ VectorValues createSimpleConstraintValues() { using symbol_shorthand::X; using symbol_shorthand::L; VectorValues config; - Vector v = (Vec(2) << 1.0, -1.0); + Vector v = (Vector(2) << 1.0, -1.0); config.insert(_x_, v); config.insert(_y_, v); return config; @@ -468,7 +468,7 @@ GaussianFactorGraph createSingleConstraintGraph() { Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0; Matrix Ay1 = eye(2) * 10; - Vector b2 = (Vec(2) << 1.0, 2.0); + Vector b2 = (Vector(2) << 1.0, 2.0); JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, constraintModel)); @@ -484,8 +484,8 @@ GaussianFactorGraph createSingleConstraintGraph() { VectorValues createSingleConstraintValues() { using namespace impl; VectorValues config = boost::assign::pair_list_of - (_x_, (Vec(2) << 1.0, -1.0)) - (_y_, (Vec(2) << 0.2, 0.1)); + (_x_, (Vector(2) << 1.0, -1.0)) + (_y_, (Vector(2) << 0.2, 0.1)); return config; } @@ -494,7 +494,7 @@ GaussianFactorGraph createMultiConstraintGraph() { using namespace impl; // unary factor 1 Matrix A = eye(2); - Vector b = (Vec(2) << -2.0, 2.0); + Vector b = (Vector(2) << -2.0, 2.0); JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1)); // constraint 1 @@ -548,9 +548,9 @@ GaussianFactorGraph createMultiConstraintGraph() { VectorValues createMultiConstraintValues() { using namespace impl; VectorValues config = boost::assign::pair_list_of - (_x_, (Vec(2) << -2.0, 2.0)) - (_y_, (Vec(2) << -0.1, 0.4)) - (_z_, (Vec(2) <<-4.0, 5.0)); + (_x_, (Vector(2) << -2.0, 2.0)) + (_y_, (Vector(2) << -0.1, 0.4)) + (_z_, (Vector(2) <<-4.0, 5.0)); return config; } diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index a4fb72c23..dd16e6a9e 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -129,8 +129,8 @@ TEST( testBoundingConstraint, unary_linearization_active) { config2.insert(key, pt2); GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2); GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2); - JacobianFactor expected1(key, (Mat(1, 2) << 1.0, 0.0), repeat(1, 3.0), hard_model1); - JacobianFactor expected2(key, (Mat(1, 2) << 0.0, 1.0), repeat(1, 5.0), hard_model1); + JacobianFactor expected1(key, (Matrix(1, 2) << 1.0, 0.0), repeat(1, 3.0), hard_model1); + JacobianFactor expected2(key, (Matrix(1, 2) << 0.0, 1.0), repeat(1, 5.0), hard_model1); EXPECT(assert_equal((const GaussianFactor&)expected1, *actual1, tol)); EXPECT(assert_equal((const GaussianFactor&)expected2, *actual2, tol)); } diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index 810086d87..c4bf0480c 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -48,21 +48,21 @@ TEST(DoglegOptimizer, ComputeBlend) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 0, (Vec(2) << 1.0,2.0), (Mat(2, 2) << 3.0,4.0,0.0,6.0), - 3, (Mat(2, 2) << 7.0,8.0,9.0,10.0), - 4, (Mat(2, 2) << 11.0,12.0,13.0,14.0))); + 0, (Vector(2) << 1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0), + 3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0), + 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 1, (Vec(2) << 15.0,16.0), (Mat(2, 2) << 17.0,18.0,0.0,20.0), - 2, (Mat(2, 2) << 21.0,22.0,23.0,24.0), - 4, (Mat(2, 2) << 25.0,26.0,27.0,28.0))); + 1, (Vector(2) << 15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0), + 2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0), + 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 2, (Vec(2) << 29.0,30.0), (Mat(2, 2) << 31.0,32.0,0.0,34.0), - 3, (Mat(2, 2) << 35.0,36.0,37.0,38.0))); + 2, (Vector(2) << 29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0), + 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 3, (Vec(2) << 39.0,40.0), (Mat(2, 2) << 41.0,42.0,0.0,44.0), - 4, (Mat(2, 2) << 45.0,46.0,47.0,48.0))); + 3, (Vector(2) << 39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0), + 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, (Vec(2) << 49.0,50.0), (Mat(2, 2) << 51.0,52.0,0.0,54.0))); + 4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0))); // Compute steepest descent point VectorValues xu = gbn.optimizeGradientSearch(); @@ -84,21 +84,21 @@ TEST(DoglegOptimizer, ComputeDoglegPoint) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 0, (Vec(2) << 1.0,2.0), (Mat(2, 2) << 3.0,4.0,0.0,6.0), - 3, (Mat(2, 2) << 7.0,8.0,9.0,10.0), - 4, (Mat(2, 2) << 11.0,12.0,13.0,14.0))); + 0, (Vector(2) << 1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0), + 3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0), + 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 1, (Vec(2) << 15.0,16.0), (Mat(2, 2) << 17.0,18.0,0.0,20.0), - 2, (Mat(2, 2) << 21.0,22.0,23.0,24.0), - 4, (Mat(2, 2) << 25.0,26.0,27.0,28.0))); + 1, (Vector(2) << 15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0), + 2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0), + 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 2, (Vec(2) << 29.0,30.0), (Mat(2, 2) << 31.0,32.0,0.0,34.0), - 3, (Mat(2, 2) << 35.0,36.0,37.0,38.0))); + 2, (Vector(2) << 29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0), + 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 3, (Vec(2) << 39.0,40.0), (Mat(2, 2) << 41.0,42.0,0.0,44.0), - 4, (Mat(2, 2) << 45.0,46.0,47.0,48.0))); + 3, (Vector(2) << 39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0), + 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, (Vec(2) << 49.0,50.0), (Mat(2, 2) << 51.0,52.0,0.0,54.0))); + 4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0))); // Compute dogleg point for different deltas diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index d9fb91d61..9f22a5840 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -37,20 +37,20 @@ TEST( ExtendedKalmanFilter, linear ) { // Create the Kalman Filter initialization point Point2 x_initial(0.0, 0.0); - SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1)); + SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // Create an ExtendedKalmanFilter object ExtendedKalmanFilter ekf(x0, x_initial, P_initial); // Create the controls and measurement properties for our example double dt = 1.0; - Vector u = (Vec(2) << 1.0, 0.0); + Vector u = (Vector(2) << 1.0, 0.0); Point2 difference(u*dt); - SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1), true); + SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1), true); Point2 z1(1.0, 0.0); Point2 z2(2.0, 0.0); Point2 z3(3.0, 0.0); - SharedDiagonal R = noiseModel::Diagonal::Sigmas((Vec(2) << 0.25, 0.25), true); + SharedDiagonal R = noiseModel::Diagonal::Sigmas((Vector(2) << 0.25, 0.25), true); // Create the set of expected output TestValues Point2 expected1(1.0, 0.0); @@ -107,7 +107,7 @@ public: NonlinearMotionModel(){} NonlinearMotionModel(const Symbol& TestKey1, const Symbol& TestKey2) : - Base(noiseModel::Diagonal::Sigmas((Vec(2) << 1.0, 1.0)), TestKey1, TestKey2), Q_(2,2) { + Base(noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 1.0)), TestKey1, TestKey2), Q_(2,2) { // Initialize motion model parameters: // w is vector of motion noise sigmas. The final covariance is calculated as G*w*w'*G' @@ -405,7 +405,7 @@ TEST( ExtendedKalmanFilter, nonlinear ) { // Create the Kalman Filter initialization point Point2 x_initial(0.90, 1.10); - SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1)); + SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // Create an ExtendedKalmanFilter object ExtendedKalmanFilter ekf(X(0), x_initial, P_initial); @@ -418,7 +418,7 @@ TEST( ExtendedKalmanFilter, nonlinear ) { x_predict = ekf.predict(motionFactor); // Create a measurement factor - NonlinearMeasurementModel measurementFactor(X(i+1), (Vec(1) << z[i])); + NonlinearMeasurementModel measurementFactor(X(i+1), (Vector(1) << z[i])); x_update = ekf.update(measurementFactor); EXPECT(assert_equal(expected_predict[i],x_predict, 1e-6)); diff --git a/tests/testGaussianBayesTree.cpp b/tests/testGaussianBayesTree.cpp index e63d613d1..97bd5bd57 100644 --- a/tests/testGaussianBayesTree.cpp +++ b/tests/testGaussianBayesTree.cpp @@ -77,7 +77,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) // Check the conditional P(C3|Root) double sigma3 = 0.61808; - Matrix A56 = (Mat(2,2) << -0.382022,0.,0.,-0.382022); + Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022); GaussianBayesNet expected3; expected3 += GaussianConditional(X(5), zero(2), eye(2)/sigma3, X(6), A56/sigma3); GaussianBayesTree::sharedClique C3 = bayesTree[X(4)]; @@ -86,7 +86,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) // Check the conditional P(C4|Root) double sigma4 = 0.661968; - Matrix A46 = (Mat(2,2) << -0.146067,0.,0.,-0.146067); + Matrix A46 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067); GaussianBayesNet expected4; expected4 += GaussianConditional(X(4), zero(2), eye(2)/sigma4, X(6), A46/sigma4); GaussianBayesTree::sharedClique C4 = bayesTree[X(3)]; @@ -296,13 +296,13 @@ TEST(GaussianBayesTree, shortcut_overlapping_separator) // f(6,7) GaussianFactorGraph fg; noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); - fg.add(1, (Mat(1, 1) << 1.0), 3, (Mat(1, 1) << 2.0), 5, (Mat(1, 1) << 3.0), (Vec(1) << 4.0), model); - fg.add(1, (Mat(1, 1) << 5.0), (Vec(1) << 6.0), model); - fg.add(2, (Mat(1, 1) << 7.0), 4, (Mat(1, 1) << 8.0), 5, (Mat(1, 1) << 9.0), (Vec(1) << 10.0), model); - fg.add(2, (Mat(1, 1) << 11.0), (Vec(1) << 12.0), model); - fg.add(5, (Mat(1, 1) << 13.0), 6, (Mat(1, 1) << 14.0), (Vec(1) << 15.0), model); - fg.add(6, (Mat(1, 1) << 17.0), 7, (Mat(1, 1) << 18.0), (Vec(1) << 19.0), model); - fg.add(7, (Mat(1, 1) << 20.0), (Vec(1) << 21.0), model); + fg.add(1, (Matrix(1, 1) << 1.0), 3, (Matrix(1, 1) << 2.0), 5, (Matrix(1, 1) << 3.0), (Vector(1) << 4.0), model); + fg.add(1, (Matrix(1, 1) << 5.0), (Vector(1) << 6.0), model); + fg.add(2, (Matrix(1, 1) << 7.0), 4, (Matrix(1, 1) << 8.0), 5, (Matrix(1, 1) << 9.0), (Vector(1) << 10.0), model); + fg.add(2, (Matrix(1, 1) << 11.0), (Vector(1) << 12.0), model); + fg.add(5, (Matrix(1, 1) << 13.0), 6, (Matrix(1, 1) << 14.0), (Vector(1) << 15.0), model); + fg.add(6, (Matrix(1, 1) << 17.0), 7, (Matrix(1, 1) << 18.0), (Vector(1) << 19.0), model); + fg.add(7, (Matrix(1, 1) << 20.0), (Vector(1) << 21.0), model); // Eliminate into BayesTree // c(6,7) diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index f4410d523..962d8b893 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -79,7 +79,7 @@ TEST( GaussianFactorGraph, eliminateOne_x1 ) // create expected Conditional Gaussian Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; - Vector d = (Vec(2) << -0.133333, -0.0222222); + Vector d = (Vector(2) << -0.133333, -0.0222222); GaussianConditional expected(X(1),15*d,R11,L(1),S12,X(2),S13); EXPECT(assert_equal(expected,*conditional,tol)); @@ -97,7 +97,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2 ) // create expected Conditional Gaussian double sig = 0.0894427; Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; - Vector d = (Vec(2) << 0.2, -0.14)/sig, sigma = ones(2); + Vector d = (Vector(2) << 0.2, -0.14)/sig, sigma = ones(2); GaussianConditional expected(ordering[X(2)],d,R11,ordering[L(1)],S12,ordering[X(1)],S13,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -113,7 +113,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1 ) // create expected Conditional Gaussian double sig = sqrt(2.0)/10.; Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; - Vector d = (Vec(2) << -0.1, 0.25)/sig, sigma = ones(2); + Vector d = (Vector(2) << -0.1, 0.25)/sig, sigma = ones(2); GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -130,21 +130,21 @@ TEST( GaussianFactorGraph, eliminateOne_x1_fast ) // create expected Conditional Gaussian Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; - Vector d = (Vec(2) << -0.133333, -0.0222222), sigma = ones(2); + Vector d = (Vector(2) << -0.133333, -0.0222222), sigma = ones(2); GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma); // Create expected remaining new factor - JacobianFactor expectedFactor(1, (Mat(4,2) << + JacobianFactor expectedFactor(1, (Matrix(4,2) << 4.714045207910318, 0., 0., 4.714045207910318, 0., 0., 0., 0.), - 2, (Mat(4,2) << + 2, (Matrix(4,2) << -2.357022603955159, 0., 0., -2.357022603955159, 7.071067811865475, 0., 0., 7.071067811865475), - (Vec(4) << -0.707106781186547, 0.942809041582063, 0.707106781186547, -1.414213562373094), noiseModel::Unit::Create(4)); + (Vector(4) << -0.707106781186547, 0.942809041582063, 0.707106781186547, -1.414213562373094), noiseModel::Unit::Create(4)); EXPECT(assert_equal(expected,*conditional,tol)); EXPECT(assert_equal((const GaussianFactor&)expectedFactor,*remaining.back(),tol)); @@ -160,7 +160,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2_fast ) // create expected Conditional Gaussian double sig = 0.0894427; Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; - Vector d = (Vec(2) << 0.2, -0.14)/sig, sigma = ones(2); + Vector d = (Vector(2) << 0.2, -0.14)/sig, sigma = ones(2); GaussianConditional expected(ordering[X(2)],d,R11,ordering[X(1)],S13,ordering[L(1)],S12,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -176,7 +176,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast ) // create expected Conditional Gaussian double sig = sqrt(2.0)/10.; Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; - Vector d = (Vec(2) << -0.1, 0.25)/sig, sigma = ones(2); + Vector d = (Vector(2) << -0.1, 0.25)/sig, sigma = ones(2); GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -191,15 +191,15 @@ TEST( GaussianFactorGraph, eliminateAll ) Ordering ordering; ordering += X(2),L(1),X(1); - Vector d1 = (Vec(2) << -0.1,-0.1); + Vector d1 = (Vector(2) << -0.1,-0.1); GaussianBayesNet expected = simpleGaussian(ordering[X(1)],d1,0.1); double sig1 = 0.149071; - Vector d2 = (Vec(2) << 0.0, 0.2)/sig1, sigma2 = ones(2); + Vector d2 = (Vector(2) << 0.0, 0.2)/sig1, sigma2 = ones(2); push_front(expected,ordering[L(1)],d2, I/sig1,ordering[X(1)], (-1)*I/sig1,sigma2); double sig2 = 0.0894427; - Vector d3 = (Vec(2) << 0.2, -0.14)/sig2, sigma3 = ones(2); + Vector d3 = (Vector(2) << 0.2, -0.14)/sig2, sigma3 = ones(2); push_front(expected,ordering[X(2)],d3, I/sig2,ordering[L(1)], (-0.2)*I/sig2, ordering[X(1)], (-0.8)*I/sig2, sigma3); // Check one ordering @@ -374,10 +374,10 @@ TEST( GaussianFactorGraph, multiplication ) VectorValues x = createCorrectDelta(ord); Errors actual = A * x; Errors expected; - expected += (Vec(2) << -1.0,-1.0); - expected += (Vec(2) << 2.0,-1.0); - expected += (Vec(2) << 0.0, 1.0); - expected += (Vec(2) << -1.0, 1.5); + expected += (Vector(2) << -1.0,-1.0); + expected += (Vector(2) << 2.0,-1.0); + expected += (Vector(2) << 0.0, 1.0); + expected += (Vector(2) << -1.0, 1.5); EXPECT(assert_equal(expected,actual)); } @@ -390,7 +390,7 @@ TEST( GaussianFactorGraph, elimination ) // Create Gaussian Factor Graph GaussianFactorGraph fg; Matrix Ap = eye(1), An = eye(1) * -1; - Vector b = (Vec(1) << 0.0); + Vector b = (Vector(1) << 0.0); SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,2.0); fg += ord[X(1)], An, ord[X(2)], Ap, b, sigma; fg += ord[X(1)], Ap, b, sigma; @@ -405,10 +405,10 @@ TEST( GaussianFactorGraph, elimination ) // Check matrix Matrix R;Vector d; boost::tie(R,d) = matrix(bayesNet); - Matrix expected = (Mat(2, 2) << + Matrix expected = (Matrix(2, 2) << 0.707107, -0.353553, 0.0, 0.612372); - Matrix expected2 = (Mat(2, 2) << + Matrix expected2 = (Matrix(2, 2) << 0.707107, -0.353553, 0.0, -0.612372); EXPECT(equal_with_abs_tol(expected, R, 1e-6) || equal_with_abs_tol(expected2, R, 1e-6)); @@ -544,7 +544,7 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) { gtsam::Key xC1 = 0, l32 = 1, l41 = 2; // noisemodels at nonlinear level - gtsam::SharedNoiseModel priorModel = noiseModel::Diagonal::Sigmas((Vec(6) << 0.05, 0.05, 3.0, 0.2, 0.2, 0.2)); + gtsam::SharedNoiseModel priorModel = noiseModel::Diagonal::Sigmas((Vector(6) << 0.05, 0.05, 3.0, 0.2, 0.2, 0.2)); gtsam::SharedNoiseModel measModel = noiseModel::Unit::Create(2); gtsam::SharedNoiseModel elevationModel = noiseModel::Isotropic::Sigma(1, 3.0); diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 85c332e6a..4fbbcb1e6 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -42,8 +42,8 @@ const double tol = 1e-4; // SETDEBUG("ISAM2 recalculate", true); // Set up parameters -SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.1, M_PI/100.0)); -SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vec(2) << M_PI/100.0, 0.1)); +SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, M_PI/100.0)); +SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, 0.1)); ISAM2 createSlamlikeISAM2( boost::optional init_values = boost::none, @@ -171,10 +171,10 @@ done: // // Create values where indices 1 and 3 are above the threshold of 0.1 // VectorValues values; // values.reserve(4, 10); -// values.push_back_preallocated((Vec(2) << 0.09, 0.09)); -// values.push_back_preallocated((Vec(3) << 0.11, 0.11, 0.09)); -// values.push_back_preallocated((Vec(3) << 0.09, 0.09, 0.09)); -// values.push_back_preallocated((Vec(2) << 0.11, 0.11)); +// values.push_back_preallocated((Vector(2) << 0.09, 0.09)); +// values.push_back_preallocated((Vector(3) << 0.11, 0.11, 0.09)); +// values.push_back_preallocated((Vector(3) << 0.09, 0.09, 0.09)); +// values.push_back_preallocated((Vector(2) << 0.11, 0.11)); // // // Create a permutation // Permutation permutation(4); diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index 9c8f268dd..ea34afa79 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -105,7 +105,7 @@ TEST( GaussianJunctionTreeB, optimizeMultiFrontal ) // verify VectorValues expected(vector(7,2)); // expected solution - Vector v = (Vec(2) << 0., 0.); + Vector v = (Vector(2) << 0., 0.); for (int i=1; i<=7; i++) expected[ordering[X(i)]] = v; EXPECT(assert_equal(expected,actual)); @@ -134,8 +134,8 @@ TEST(GaussianJunctionTreeB, slamlike) { Values init; NonlinearFactorGraph newfactors; NonlinearFactorGraph fullgraph; - SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.1, M_PI/100.0)); - SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vec(2) << M_PI/100.0, 0.1)); + SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, M_PI/100.0)); + SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, 0.1)); size_t i = 0; diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index a1da302e0..2a84248a3 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -30,18 +30,18 @@ boost::tuple generateProblem() { // 2a. Add Gaussian prior Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1)); + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); graph += PriorFactor(1, priorMean, priorNoise); // 2b. Add odometry factors - SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1)); + SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); graph += BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); graph += BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph += BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph += BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add pose constraint - SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1)); + SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); graph += BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index 923e38fcd..bb50d2afa 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -64,7 +64,7 @@ TEST( Iterative, conjugateGradientDescent ) Vector b; Vector x0 = gtsam::zero(6); boost::tie(A, b) = fg.jacobian(); - Vector expectedX = (Vec(6) << -0.1, 0.1, -0.1, -0.1, 0.1, -0.2); + Vector expectedX = (Vector(6) << -0.1, 0.1, -0.1, -0.1, 0.1, -0.2); // Do conjugate gradient descent, System version System Ab(A, b); @@ -105,7 +105,7 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint ) VectorValues expected; expected.insert(X(1), zero(3)); - expected.insert(X(2), (Vec(3) << -0.5,0.,0.)); + expected.insert(X(2), (Vector(3) << -0.5,0.,0.)); CHECK(assert_equal(expected, actual)); } @@ -132,7 +132,7 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) VectorValues expected; expected.insert(X(1), zero(3)); - expected.insert(X(2), (Vec(3) << -0.5,0.,0.)); + expected.insert(X(2), (Vector(3) << -0.5,0.,0.)); CHECK(assert_equal(expected, actual)); } diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index f037fef71..5478ce38e 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -51,13 +51,13 @@ TEST(Marginals, planarSLAMmarginals) { /* add prior */ // gaussian for prior - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1)); + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin graph += PriorFactor(x1, priorMean, priorNoise); // add the factor to the graph /* add odometry */ // general noisemodel for odometry - SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1)); + SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) // create between factors to represent odometry graph += BetweenFactor(x1, x2, odometry, odometryNoise); @@ -65,7 +65,7 @@ TEST(Marginals, planarSLAMmarginals) { /* add measurements */ // general noisemodel for measurements - SharedDiagonal measurementNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.2)); + SharedDiagonal measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.2)); // create the measurement values - indices are (pose id, landmark id) Rot2 bearing11 = Rot2::fromDegrees(45), diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index b2d8f43cb..b55f6f144 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -166,7 +166,7 @@ TEST ( NonlinearEquality, allow_error_pose ) { // the unwhitened error should provide logmap to the feasible state Pose2 badPoint1(0.0, 2.0, 3.0); Vector actVec = nle.evaluateError(badPoint1); - Vector expVec = (Vec(3) << -0.989992, -0.14112, 0.0); + Vector expVec = (Vector(3) << -0.989992, -0.14112, 0.0); EXPECT(assert_equal(expVec, actVec, 1e-5)); // the actual error should have a gain on it @@ -267,8 +267,8 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) { Point2 ptBad1(2.0, 2.0); config2.insert(key, ptBad1); EXPECT(constraint.active(config2)); - EXPECT(assert_equal((Vec(2) << 1.0, 0.0), constraint.evaluateError(ptBad1), tol)); - EXPECT(assert_equal((Vec(2) << 1.0, 0.0), constraint.unwhitenedError(config2), tol)); + EXPECT(assert_equal((Vector(2) << 1.0, 0.0), constraint.evaluateError(ptBad1), tol)); + EXPECT(assert_equal((Vector(2) << 1.0, 0.0), constraint.unwhitenedError(config2), tol)); EXPECT_DOUBLES_EQUAL(500.0, constraint.error(config2), tol); } @@ -289,7 +289,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) { Point2 ptBad(2.0, 2.0); config2.insert(key, ptBad); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); - GaussianFactor::shared_ptr expected2(new JacobianFactor(key, eye(2,2), (Vec(2) <<-1.0,0.0), hard_model)); + GaussianFactor::shared_ptr expected2(new JacobianFactor(key, eye(2,2), (Vector(2) <<-1.0,0.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } @@ -347,8 +347,8 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) { config2.insert(key1, x1bad); config2.insert(key2, x2bad); EXPECT(constraint.active(config2)); - EXPECT(assert_equal((Vec(2) << -1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol)); - EXPECT(assert_equal((Vec(2) << -1.0, -1.0), constraint.unwhitenedError(config2), tol)); + EXPECT(assert_equal((Vector(2) << -1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol)); + EXPECT(assert_equal((Vector(2) << -1.0, -1.0), constraint.unwhitenedError(config2), tol)); EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol); } @@ -376,7 +376,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr expected2( new JacobianFactor(key1, -eye(2,2), key2, - eye(2,2), (Vec(2) << 1.0, 1.0), hard_model)); + eye(2,2), (Vector(2) << 1.0, 1.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } @@ -520,7 +520,7 @@ typedef NonlinearEquality2 Point3Equality; TEST (testNonlinearEqualityConstraint, stereo_constrained ) { // create initial estimates - Rot3 faceDownY((Matrix)(Mat(3,3) << + Rot3 faceDownY((Matrix)(Matrix(3,3) << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0)); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 1fbe0a633..247c6a6b0 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -197,7 +197,7 @@ TEST( NonlinearFactor, size ) /* ************************************************************************* */ TEST( NonlinearFactor, linearize_constraint1 ) { - Vector sigmas = (Vec(2) << 0.2, 0.0); + Vector sigmas = (Vector(2) << 0.2, 0.0); SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas); Point2 mu(1., -1.); @@ -208,16 +208,16 @@ TEST( NonlinearFactor, linearize_constraint1 ) GaussianFactor::shared_ptr actual = f0->linearize(config); // create expected - Vector b = (Vec(2) << 0., -3.); - JacobianFactor expected(X(1), (Mat(2, 2) << 5.0, 0.0, 0.0, 1.0), b, - noiseModel::Constrained::MixedSigmas((Vec(2) << 1.0, 0.0))); + Vector b = (Vector(2) << 0., -3.); + JacobianFactor expected(X(1), (Matrix(2, 2) << 5.0, 0.0, 0.0, 1.0), b, + noiseModel::Constrained::MixedSigmas((Vector(2) << 1.0, 0.0))); CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } /* ************************************************************************* */ TEST( NonlinearFactor, linearize_constraint2 ) { - Vector sigmas = (Vec(2) << 0.2, 0.0); + Vector sigmas = (Vector(2) << 0.2, 0.0); SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas); Point2 z3(1.,-1.); @@ -229,10 +229,10 @@ TEST( NonlinearFactor, linearize_constraint2 ) GaussianFactor::shared_ptr actual = f0.linearize(config); // create expected - Matrix A = (Mat(2, 2) << 5.0, 0.0, 0.0, 1.0); - Vector b = (Vec(2) << -15., -3.); + Matrix A = (Matrix(2, 2) << 5.0, 0.0, 0.0, 1.0); + Vector b = (Vector(2) << -15., -3.); JacobianFactor expected(X(1), -1*A, L(1), A, b, - noiseModel::Constrained::MixedSigmas((Vec(2) << 1.0, 0.0))); + noiseModel::Constrained::MixedSigmas((Vector(2) << 1.0, 0.0))); CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } @@ -240,7 +240,7 @@ TEST( NonlinearFactor, linearize_constraint2 ) class TestFactor4 : public NoiseModelFactor4 { public: typedef NoiseModelFactor4 Base; - TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vec(1) << 2.0)), X(1), X(2), X(3), X(4)) {} + TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4)) {} virtual Vector evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4, @@ -249,10 +249,10 @@ public: boost::optional H3 = boost::none, boost::optional H4 = boost::none) const { if(H1) { - *H1 = (Mat(1, 1) << 1.0); - *H2 = (Mat(1, 1) << 2.0); - *H3 = (Mat(1, 1) << 3.0); - *H4 = (Mat(1, 1) << 4.0); + *H1 = (Matrix(1, 1) << 1.0); + *H2 = (Matrix(1, 1) << 2.0); + *H3 = (Matrix(1, 1) << 3.0); + *H4 = (Matrix(1, 1) << 4.0); } return (Vector(1) << x1 + x2 + x3 + x4).finished(); } @@ -270,25 +270,25 @@ TEST(NonlinearFactor, NoiseModelFactor4) { tv.insert(X(2), LieVector(1, 2.0)); tv.insert(X(3), LieVector(1, 3.0)); tv.insert(X(4), LieVector(1, 4.0)); - EXPECT(assert_equal((Vec(1) << 10.0), tf.unwhitenedError(tv))); + EXPECT(assert_equal((Vector(1) << 10.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); LONGS_EQUAL((long)X(3), (long)jf.keys()[2]); LONGS_EQUAL((long)X(4), (long)jf.keys()[3]); - EXPECT(assert_equal((Matrix)(Mat(1, 1) << 0.5), jf.getA(jf.begin()))); - EXPECT(assert_equal((Matrix)(Mat(1, 1) << 1.0), jf.getA(jf.begin()+1))); - EXPECT(assert_equal((Matrix)(Mat(1, 1) << 1.5), jf.getA(jf.begin()+2))); - EXPECT(assert_equal((Matrix)(Mat(1, 1) << 2.0), jf.getA(jf.begin()+3))); - EXPECT(assert_equal((Vector)(Vec(1) << -5.0), jf.getb())); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5), jf.getA(jf.begin()))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0), jf.getA(jf.begin()+1))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5), jf.getA(jf.begin()+2))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0), jf.getA(jf.begin()+3))); + EXPECT(assert_equal((Vector)(Vector(1) << -5.0), jf.getb())); } /* ************************************************************************* */ class TestFactor5 : public NoiseModelFactor5 { public: typedef NoiseModelFactor5 Base; - TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vec(1) << 2.0)), X(1), X(2), X(3), X(4), X(5)) {} + TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4), X(5)) {} virtual Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, @@ -298,11 +298,11 @@ public: boost::optional H4 = boost::none, boost::optional H5 = boost::none) const { if(H1) { - *H1 = (Mat(1, 1) << 1.0); - *H2 = (Mat(1, 1) << 2.0); - *H3 = (Mat(1, 1) << 3.0); - *H4 = (Mat(1, 1) << 4.0); - *H5 = (Mat(1, 1) << 5.0); + *H1 = (Matrix(1, 1) << 1.0); + *H2 = (Matrix(1, 1) << 2.0); + *H3 = (Matrix(1, 1) << 3.0); + *H4 = (Matrix(1, 1) << 4.0); + *H5 = (Matrix(1, 1) << 5.0); } return (Vector(1) << x1 + x2 + x3 + x4 + x5).finished(); } @@ -317,7 +317,7 @@ TEST(NonlinearFactor, NoiseModelFactor5) { tv.insert(X(3), LieVector(1, 3.0)); tv.insert(X(4), LieVector(1, 4.0)); tv.insert(X(5), LieVector(1, 5.0)); - EXPECT(assert_equal((Vec(1) << 15.0), tf.unwhitenedError(tv))); + EXPECT(assert_equal((Vector(1) << 15.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); @@ -325,19 +325,19 @@ TEST(NonlinearFactor, NoiseModelFactor5) { LONGS_EQUAL((long)X(3), (long)jf.keys()[2]); LONGS_EQUAL((long)X(4), (long)jf.keys()[3]); LONGS_EQUAL((long)X(5), (long)jf.keys()[4]); - EXPECT(assert_equal((Matrix)(Mat(1, 1) << 0.5), jf.getA(jf.begin()))); - EXPECT(assert_equal((Matrix)(Mat(1, 1) << 1.0), jf.getA(jf.begin()+1))); - EXPECT(assert_equal((Matrix)(Mat(1, 1) << 1.5), jf.getA(jf.begin()+2))); - EXPECT(assert_equal((Matrix)(Mat(1, 1) << 2.0), jf.getA(jf.begin()+3))); - EXPECT(assert_equal((Matrix)(Mat(1, 1) << 2.5), jf.getA(jf.begin()+4))); - EXPECT(assert_equal((Vector)(Vec(1) << -7.5), jf.getb())); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5), jf.getA(jf.begin()))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0), jf.getA(jf.begin()+1))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5), jf.getA(jf.begin()+2))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0), jf.getA(jf.begin()+3))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.5), jf.getA(jf.begin()+4))); + EXPECT(assert_equal((Vector)(Vector(1) << -7.5), jf.getb())); } /* ************************************************************************* */ class TestFactor6 : public NoiseModelFactor6 { public: typedef NoiseModelFactor6 Base; - TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vec(1) << 2.0)), X(1), X(2), X(3), X(4), X(5), X(6)) {} + TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4), X(5), X(6)) {} virtual Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, @@ -348,12 +348,12 @@ public: boost::optional H5 = boost::none, boost::optional H6 = boost::none) const { if(H1) { - *H1 = (Mat(1, 1) << 1.0); - *H2 = (Mat(1, 1) << 2.0); - *H3 = (Mat(1, 1) << 3.0); - *H4 = (Mat(1, 1) << 4.0); - *H5 = (Mat(1, 1) << 5.0); - *H6 = (Mat(1, 1) << 6.0); + *H1 = (Matrix(1, 1) << 1.0); + *H2 = (Matrix(1, 1) << 2.0); + *H3 = (Matrix(1, 1) << 3.0); + *H4 = (Matrix(1, 1) << 4.0); + *H5 = (Matrix(1, 1) << 5.0); + *H6 = (Matrix(1, 1) << 6.0); } return (Vector(1) << x1 + x2 + x3 + x4 + x5 + x6).finished(); } @@ -370,7 +370,7 @@ TEST(NonlinearFactor, NoiseModelFactor6) { tv.insert(X(4), LieVector(1, 4.0)); tv.insert(X(5), LieVector(1, 5.0)); tv.insert(X(6), LieVector(1, 6.0)); - EXPECT(assert_equal((Vec(1) << 21.0), tf.unwhitenedError(tv))); + EXPECT(assert_equal((Vector(1) << 21.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); @@ -379,13 +379,13 @@ TEST(NonlinearFactor, NoiseModelFactor6) { LONGS_EQUAL((long)X(4), (long)jf.keys()[3]); LONGS_EQUAL((long)X(5), (long)jf.keys()[4]); LONGS_EQUAL((long)X(6), (long)jf.keys()[5]); - EXPECT(assert_equal((Matrix)(Mat(1, 1) << 0.5), jf.getA(jf.begin()))); - EXPECT(assert_equal((Matrix)(Mat(1, 1) << 1.0), jf.getA(jf.begin()+1))); - EXPECT(assert_equal((Matrix)(Mat(1, 1) << 1.5), jf.getA(jf.begin()+2))); - EXPECT(assert_equal((Matrix)(Mat(1, 1) << 2.0), jf.getA(jf.begin()+3))); - EXPECT(assert_equal((Matrix)(Mat(1, 1) << 2.5), jf.getA(jf.begin()+4))); - EXPECT(assert_equal((Matrix)(Mat(1, 1) << 3.0), jf.getA(jf.begin()+5))); - EXPECT(assert_equal((Vector)(Vec(1) << -10.5), jf.getb())); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5), jf.getA(jf.begin()))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0), jf.getA(jf.begin()+1))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5), jf.getA(jf.begin()+2))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0), jf.getA(jf.begin()+3))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.5), jf.getA(jf.begin()+4))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 3.0), jf.getA(jf.begin()+5))); + EXPECT(assert_equal((Vector)(Vector(1) << -10.5), jf.getb())); } diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 13d2423e3..34e0f29d1 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -27,7 +27,7 @@ TEST(testNonlinearISAM, markov_chain ) { NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object - SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vec(3) << 3.0, 3.0, 0.5)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 3.0, 3.0, 0.5)); Sampler sampler(model, 42u); // create initial graph @@ -74,8 +74,8 @@ TEST(testNonlinearISAM, markov_chain_with_disconnects ) { NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object - SharedDiagonal model3 = noiseModel::Diagonal::Sigmas((Vec(3) << 3.0, 3.0, 0.5)); - SharedDiagonal model2 = noiseModel::Diagonal::Sigmas((Vec(2) << 2.0, 2.0)); + SharedDiagonal model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 3.0, 3.0, 0.5)); + SharedDiagonal model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 2.0, 2.0)); Sampler sampler(model3, 42u); // create initial graph @@ -151,8 +151,8 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) { NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object - SharedDiagonal model3 = noiseModel::Diagonal::Sigmas((Vec(3) << 3.0, 3.0, 0.5)); - SharedDiagonal model2 = noiseModel::Diagonal::Sigmas((Vec(2) << 2.0, 2.0)); + SharedDiagonal model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 3.0, 3.0, 0.5)); + SharedDiagonal model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 2.0, 2.0)); Sampler sampler(model3, 42u); // create initial graph diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index cd4f9cd36..765c1e1ca 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -131,12 +131,12 @@ TEST( SubgraphPreconditioner, system ) // y1 = perturbed y0 VectorValues y1 = zeros; - y1[1] = (Vec(2) << 1.0, -1.0); + y1[1] = (Vector(2) << 1.0, -1.0); // Check corresponding x values VectorValues expected_x1 = xtrue, x1 = system.x(y1); - expected_x1[1] = (Vec(2) << 2.01, 2.99); - expected_x1[0] = (Vec(2) << 3.01, 2.99); + expected_x1[1] = (Vector(2) << 2.01, 2.99); + expected_x1[0] = (Vector(2) << 3.01, 2.99); CHECK(assert_equal(xtrue, system.x(y0))); CHECK(assert_equal(expected_x1,system.x(y1))); @@ -150,28 +150,28 @@ TEST( SubgraphPreconditioner, system ) VectorValues expected_gx0 = zeros; VectorValues expected_gx1 = zeros; CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue))); - expected_gx1[2] = (Vec(2) << -100., 100.); - expected_gx1[4] = (Vec(2) << -100., 100.); - expected_gx1[1] = (Vec(2) << 200., -200.); - expected_gx1[3] = (Vec(2) << -100., 100.); - expected_gx1[0] = (Vec(2) << 100., -100.); + expected_gx1[2] = (Vector(2) << -100., 100.); + expected_gx1[4] = (Vector(2) << -100., 100.); + expected_gx1[1] = (Vector(2) << 200., -200.); + expected_gx1[3] = (Vector(2) << -100., 100.); + expected_gx1[0] = (Vector(2) << 100., -100.); CHECK(assert_equal(expected_gx1,gradient(Ab,x1))); // Test gradient in y VectorValues expected_gy0 = zeros; VectorValues expected_gy1 = zeros; - expected_gy1[2] = (Vec(2) << 2., -2.); - expected_gy1[4] = (Vec(2) << -2., 2.); - expected_gy1[1] = (Vec(2) << 3., -3.); - expected_gy1[3] = (Vec(2) << -1., 1.); - expected_gy1[0] = (Vec(2) << 1., -1.); + expected_gy1[2] = (Vector(2) << 2., -2.); + expected_gy1[4] = (Vector(2) << -2., 2.); + expected_gy1[1] = (Vector(2) << 3., -3.); + expected_gy1[3] = (Vector(2) << -1., 1.); + expected_gy1[0] = (Vector(2) << 1., -1.); CHECK(assert_equal(expected_gy0,gradient(system,y0))); CHECK(assert_equal(expected_gy1,gradient(system,y1))); // Check it numerically for good measure // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1) // Vector numerical_g1 = numericalGradient (error, y1, 0.001); - // Vector expected_g1 = (Vec(18) << 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., + // Vector expected_g1 = (Vector(18) << 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., // 3., -3., 0., 0., -1., 1., 1., -1.); // CHECK(assert_equal(expected_g1,numerical_g1)); } @@ -204,7 +204,7 @@ TEST( SubgraphPreconditioner, conjugateGradients ) VectorValues y0 = VectorValues::Zero(xbar); VectorValues y1 = y0; - y1[1] = (Vec(2) << 1.0, -1.0); + y1[1] = (Vector(2) << 1.0, -1.0); VectorValues x1 = system.x(y1); // Solve for the remaining constraints using PCG