diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index f1e1a0010..43814731f 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((Vector(2) << 0.5, 0.5)); + SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(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 5454f4c11..84bca65f3 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) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0); - return (Vector(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).finished(); + return (Vector(2) << q.x() - mx_, q.y() - my_).finished(); } // 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((Vector(3) << 0.2, 0.2, 0.1)); + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)); graph.add(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((Vector(2) << 0.1, 0.1)); // 10cm std on x,y + noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y graph.add(boost::make_shared(1, 0.0, 0.0, unaryNoise)); graph.add(boost::make_shared(2, 2.0, 0.0, unaryNoise)); graph.add(boost::make_shared(3, 4.0, 0.0, unaryNoise)); diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index 70c6e6fb0..b5cd681e5 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((Vector(3) << 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.add(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((Vector(3) << 0.2, 0.2, 0.1)); + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index 91ca423a2..84f9be3a1 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((Vector(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(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta graph.add(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((Vector(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(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); graph.add(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((Vector(2) << 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range + noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(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 ae34278ec..7991f7fbf 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((Vector(3) << 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.add(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((Vector(3) << 0.2, 0.2, 0.1)); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors // Create odometry (Between) factors between consecutive poses diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 8d8f2edc1..564930d74 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -64,7 +64,7 @@ int main(const int argc, const char *argv[]) { // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // - noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); + noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); std::cout << "Adding prior on pose 0 " << std::endl; diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index 86be74c75..46ca02ee4 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -31,14 +31,14 @@ 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((Vector(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).finished()); string graph_file = findExampleDataFile("w100.graph"); boost::tie(graph, initial) = load2D(graph_file, 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((Vector(3) << 0.01, 0.01, 0.01)); + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(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 4b0dc6252..818a9e577 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((Vector(3) << 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(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((Vector(3) << 0.2, 0.2, 0.1)); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(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/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index bb434f3ce..9507797eb 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -43,7 +43,7 @@ int main(const int argc, const char *argv[]) { // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // - noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); + noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); graphWithPrior.print(); diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 8e8e28570..4422586b0 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((Vector(3) << 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(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((Vector(3) << 0.2, 0.2, 0.1)); + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(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((Vector(3) << 0.2, 0.2, 0.1)); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(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/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp index f992c78b1..c0399c139 100644 --- a/examples/Pose3SLAMExample_g2o.cpp +++ b/examples/Pose3SLAMExample_g2o.cpp @@ -43,7 +43,7 @@ int main(const int argc, const char *argv[]) { // Add prior on the first key NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // - noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)); + noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { std::cout << "Adding prior to g2o file " << std::endl; diff --git a/examples/Pose3SLAMExample_initializePose3Chordal.cpp b/examples/Pose3SLAMExample_initializePose3Chordal.cpp index afc66ea1e..645b24cfc 100644 --- a/examples/Pose3SLAMExample_initializePose3Chordal.cpp +++ b/examples/Pose3SLAMExample_initializePose3Chordal.cpp @@ -43,7 +43,7 @@ int main(const int argc, const char *argv[]) { // Add prior on the first key NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // - noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)); + noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { std::cout << "Adding prior to g2o file " << std::endl; diff --git a/examples/Pose3SLAMExample_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp index 9dc410692..99342099a 100644 --- a/examples/Pose3SLAMExample_initializePose3Gradient.cpp +++ b/examples/Pose3SLAMExample_initializePose3Gradient.cpp @@ -43,7 +43,7 @@ int main(const int argc, const char *argv[]) { // Add prior on the first key NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // - noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)); + noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { std::cout << "Adding prior to g2o file " << std::endl; diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 75d14cd72..5be266d11 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((Vector(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)).finished()); // 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/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index b3c5ee5fe..9e9c74edc 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -108,7 +108,7 @@ int main(int argc, char* argv[]) { // Add a prior on pose x0. This indirectly specifies where the origin is. // 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))); + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); graph.push_back(PriorFactor(0, poses[0], poseNoise)); // Because the structure-from-motion problem has a scale ambiguity, the problem is diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index c69afe3b7..8ebf005ab 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((Vector(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)).finished()); // 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((Vector(5) << 500, 500, 0.1, 100, 100)); + noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished()); 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 de4de455a..a5b91ff38 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((Vector(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)).finished()); // 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 8a32f5dcc..8abe84eb6 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -108,7 +108,7 @@ int main(int argc, char* argv[]) { if (i == 0) { // Add a prior on pose x0, with 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))); + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); graph.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // Add a prior on landmark l0 diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index bb25440b1..201ec188b 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((Vector(2) << 0.1, 0.1)); + SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector2(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 = (Vector(2) << 1.0, 0.0); - SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1), true); + Vector u = Vector2(1.0, 0.0); + SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector2(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((Vector(2) << 0.25, 0.25), true); + SharedDiagonal R = noiseModel::Diagonal::Sigmas(Vector2(0.25, 0.25), true); // This simple measurement can be modeled with a PriorFactor Point2 z1(1.0, 0.0); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h index cb66caf5e..a036d8c3b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h @@ -25,14 +25,10 @@ namespace Eigen { * \sa \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished() */ template -struct CommaInitializer : - public internal::dense_xpr_base >::type +struct CommaInitializer { - typedef typename internal::dense_xpr_base >::type Base; - EIGEN_DENSE_PUBLIC_INTERFACE(CommaInitializer) - typedef typename internal::conditional::ret, - XprType, const XprType&>::type ExpressionTypeNested; - typedef typename XprType::InnerIterator InnerIterator; + typedef typename XprType::Scalar Scalar; + typedef typename XprType::Index Index; inline CommaInitializer(XprType& xpr, const Scalar& s) : m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1) @@ -119,82 +115,12 @@ struct CommaInitializer : */ inline XprType& finished() { return m_xpr; } - // The following implement the DenseBase interface - - inline Index rows() const { return m_xpr.rows(); } - inline Index cols() const { return m_xpr.cols(); } - inline Index outerStride() const { return m_xpr.outerStride(); } - inline Index innerStride() const { return m_xpr.innerStride(); } - - inline CoeffReturnType coeff(Index row, Index col) const - { - return m_xpr.coeff(row, col); - } - - inline CoeffReturnType coeff(Index index) const - { - return m_xpr.coeff(index); - } - - inline const Scalar& coeffRef(Index row, Index col) const - { - return m_xpr.const_cast_derived().coeffRef(row, col); - } - - inline const Scalar& coeffRef(Index index) const - { - return m_xpr.const_cast_derived().coeffRef(index); - } - - inline Scalar& coeffRef(Index row, Index col) - { - return m_xpr.const_cast_derived().coeffRef(row, col); - } - - inline Scalar& coeffRef(Index index) - { - return m_xpr.const_cast_derived().coeffRef(index); - } - - template - inline const PacketScalar packet(Index row, Index col) const - { - return m_xpr.template packet(row, col); - } - - template - inline void writePacket(Index row, Index col, const PacketScalar& x) - { - m_xpr.const_cast_derived().template writePacket(row, col, x); - } - - template - inline const PacketScalar packet(Index index) const - { - return m_xpr.template packet(index); - } - - template - inline void writePacket(Index index, const PacketScalar& x) - { - m_xpr.const_cast_derived().template writePacket(index, x); - } - - const XprType& _expression() const { return m_xpr; } - XprType& m_xpr; // target expression Index m_row; // current row id Index m_col; // current col id Index m_currentBlockRows; // current block height }; -namespace internal { - template - struct traits > : traits - { - }; -} - /** \anchor MatrixBaseCommaInitRef * Convenient operator to set the coefficients of a matrix. * diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 1d4aa86e2..5125340be 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 (Vector(1) << (t2.value() - value())); } + Vector localCoordinates(const LieScalar& t2) const { return (Vector(1) << (t2.value() - value())).finished(); } // 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 (Vector(1) << p.value()); } + static Vector Logmap(const LieScalar& p) { return (Vector(1) << p.value()).finished(); } /// 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 99b351ff5..d1b2ad78c 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -42,7 +42,7 @@ struct LieVector : public Vector { #endif /** wrap a double */ - LieVector(double d) : Vector((Vector(1) << d)) {} + LieVector(double d) : Vector((Vector(1) << d).finished()) {} /** constructor with size and initial data, row order ! */ GTSAM_EXPORT LieVector(size_t m, const double* const data); diff --git a/gtsam/base/tests/testCholesky.cpp b/gtsam/base/tests/testCholesky.cpp index b30326633..5120e9ac6 100644 --- a/gtsam/base/tests/testCholesky.cpp +++ b/gtsam/base/tests/testCholesky.cpp @@ -34,7 +34,7 @@ TEST(cholesky, choleskyPartial) { 0., 0., 0., 1.8786, 1.0535, 1.4250, 1.3347, 0., 0., 0., 0., 3.0788, 2.6283, 2.3791, 0., 0., 0., 0., 0., 2.9227, 2.4056, - 0., 0., 0., 0., 0., 0., 2.5776); + 0., 0., 0., 0., 0., 0., 2.5776).finished(); // Do partial Cholesky on 3 frontal scalar variables Matrix RSL(ABC); @@ -57,7 +57,7 @@ TEST(cholesky, choleskyPartial) { TEST(cholesky, BadScalingCholesky) { Matrix A = (Matrix(2,2) << 1e-40, 0.0, - 0.0, 1.0); + 0.0, 1.0).finished(); Matrix R(A.transpose() * A); choleskyPartial(R, 2); @@ -72,7 +72,7 @@ TEST(cholesky, BadScalingCholesky) { TEST(cholesky, BadScalingSVD) { Matrix A = (Matrix(2,2) << 1.0, 0.0, - 0.0, 1e-40); + 0.0, 1e-40).finished(); Matrix U, V; Vector S; diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp index ee8fe14d9..53dd6d4d5 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 = (Matrix(2,2) << 1.0,2.0, 3.0,4.0); + Matrix m = (Matrix(2,2) << 1.0,2.0, 3.0,4.0).finished(); LieMatrix lie1(m), lie2(m); EXPECT(lie1.dim() == 4); @@ -37,7 +37,7 @@ TEST( LieMatrix, construction ) { /* ************************************************************************* */ TEST( LieMatrix, other_constructors ) { - Matrix init = (Matrix(2,2) << 10.0,20.0, 30.0,40.0); + Matrix init = (Matrix(2,2) << 10.0,20.0, 30.0,40.0).finished(); LieMatrix exp(init); double data[] = {10,30,20,40}; LieMatrix b(2,2,data); @@ -46,10 +46,10 @@ TEST( LieMatrix, other_constructors ) { /* ************************************************************************* */ TEST(LieMatrix, retract) { - LieMatrix init((Matrix(2,2) << 1.0,2.0,3.0,4.0)); - Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0); + LieMatrix init((Matrix(2,2) << 1.0,2.0,3.0,4.0).finished()); + Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0).finished(); - LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0)); + LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0).finished()); LieMatrix actual = init.retract(update); EXPECT(assert_equal(expected, actual)); @@ -59,8 +59,8 @@ TEST(LieMatrix, retract) { EXPECT(assert_equal(expectedUpdate, actualUpdate)); - Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4); - Vector actualLogmap = LieMatrix::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0))); + Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4).finished(); + Vector actualLogmap = LieMatrix::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0).finished())); EXPECT(assert_equal(expectedLogmap, actualLogmap)); } diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index 68655cc71..946a342fc 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -42,7 +42,7 @@ TEST( testLieScalar, construction ) { TEST( testLieScalar, localCoordinates ) { LieScalar lie1(1.), lie2(3.); - EXPECT(assert_equal((Vector)(Vector(1) << 2), lie1.localCoordinates(lie2))); + EXPECT(assert_equal((Vector)(Vector(1) << 2).finished(), lie1.localCoordinates(lie2))); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index f66678c25..c1dface2e 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 = (Vector(3) << 1.0, 2.0, 3.0); + Vector v = Vector3(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 = (Vector(2) << 10.0, 20.0); + Vector init = Vector2(10.0, 20.0); LieVector exp(init); double data[] = {10,20}; LieVector b(2,data); diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 9f1851308..5b36d2b02 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 = (Matrix(2, 2) << -5, 3, 0, -5); + Matrix A = (Matrix(2, 2) << -5, 3, 0, -5).finished(); Matrix B(2, 2); B(0, 0) = -5; @@ -46,7 +46,7 @@ TEST( matrix, constructor_data ) /* ************************************************************************* */ TEST( matrix, Matrix_ ) { - Matrix A = (Matrix(2, 2) << -5.0, 3.0, 0.0, -5.0); + Matrix A = (Matrix(2, 2) << -5.0, 3.0, 0.0, -5.0).finished(); Matrix B(2, 2); B(0, 0) = -5; B(0, 1) = 3; @@ -82,17 +82,17 @@ TEST( matrix, special_comma_initializer) expected(1,0) = 3; expected(1,1) = 4; - Matrix actual1 = (Matrix(2,2) << 1, 2, 3, 4); - Matrix actual2((Matrix(2,2) << 1, 2, 3, 4)); + Matrix actual1 = (Matrix(2,2) << 1, 2, 3, 4).finished(); + Matrix actual2((Matrix(2,2) << 1, 2, 3, 4).finished()); - Matrix submat1 = (Matrix(1,2) << 3, 4); - Matrix actual3 = (Matrix(2,2) << 1, 2, submat1); + Matrix submat1 = (Matrix(1,2) << 3, 4).finished(); + Matrix actual3 = (Matrix(2,2) << 1, 2, submat1).finished(); - Matrix submat2 = (Matrix(1,2) << 1, 2); - Matrix actual4 = (Matrix(2,2) << submat2, 3, 4); + Matrix submat2 = (Matrix(1,2) << 1, 2).finished(); + Matrix actual4 = (Matrix(2,2) << submat2, 3, 4).finished(); - Matrix actual5 = testFcn1((Matrix(2,2) << 1, 2, 3, 4)); - Matrix actual6 = testFcn2((Matrix(2,2) << 1, 2, 3, 4)); + Matrix actual5 = testFcn1((Matrix(2,2) << 1, 2, 3, 4).finished()); + Matrix actual6 = testFcn2((Matrix(2,2) << 1, 2, 3, 4).finished()); EXPECT(assert_equal(expected, actual1)); EXPECT(assert_equal(expected, actual2)); @@ -105,7 +105,7 @@ TEST( matrix, special_comma_initializer) /* ************************************************************************* */ TEST( matrix, col_major ) { - Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0); + Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); const double * const a = &A(0, 0); EXPECT_DOUBLES_EQUAL(1, a[0], tol); EXPECT_DOUBLES_EQUAL(3, a[1], tol); @@ -116,8 +116,8 @@ TEST( matrix, col_major ) /* ************************************************************************* */ TEST( matrix, collect1 ) { - 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 A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished(); + Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished(); Matrix AB = collect(2, &A, &B); Matrix C(2, 5); for (int i = 0; i < 2; i++) @@ -133,8 +133,8 @@ TEST( matrix, collect1 ) /* ************************************************************************* */ TEST( matrix, collect2 ) { - 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 A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished(); + Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished(); vector matrices; matrices.push_back(&A); matrices.push_back(&B); @@ -162,7 +162,7 @@ TEST( matrix, collect3 ) Matrix AB = collect(matrices, 2, 3); 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); + 0.0, 1.0, 0.0, 0.0, 1.0, 0.0).finished(); EQUALITY(exp,AB); } @@ -170,8 +170,8 @@ TEST( matrix, collect3 ) /* ************************************************************************* */ TEST( matrix, stack ) { - 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 A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished(); + Matrix B = (Matrix(3, 2) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished(); Matrix AB = stack(2, &A, &B); Matrix C(5, 2); for (int i = 0; i < 2; i++) @@ -195,17 +195,17 @@ TEST( matrix, column ) { 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); + -0.1).finished(); Vector a1 = column(A, 0); - Vector exp1 = (Vector(4) << -1., 0., 1., 0.); + Vector exp1 = (Vector(4) << -1., 0., 1., 0.).finished(); EXPECT(assert_equal(a1, exp1)); Vector a2 = column(A, 3); - Vector exp2 = (Vector(4) << 0., 1., 0., 0.); + Vector exp2 = (Vector(4) << 0., 1., 0., 0.).finished(); EXPECT(assert_equal(a2, exp2)); Vector a3 = column(A, 6); - Vector exp3 = (Vector(4) << -0.2, 0.3, 0.2, -0.1); + Vector exp3 = (Vector(4) << -0.2, 0.3, 0.2, -0.1).finished(); EXPECT(assert_equal(a3, exp3)); } @@ -223,7 +223,7 @@ TEST( matrix, insert_column ) 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, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0); + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0).finished(); EXPECT(assert_equal(expected, big)); } @@ -246,7 +246,7 @@ TEST( matrix, insert_subcolumn ) 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, 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).finished(); EXPECT(assert_equal(expected, big)); } @@ -256,17 +256,17 @@ TEST( matrix, row ) { 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); + -0.1).finished(); Vector a1 = row(A, 0); - Vector exp1 = (Vector(7) << -1., 0., 1., 0., 0., 0., -0.2); + Vector exp1 = (Vector(7) << -1., 0., 1., 0., 0., 0., -0.2).finished(); EXPECT(assert_equal(a1, exp1)); Vector a2 = row(A, 2); - Vector exp2 = (Vector(7) << 1., 0., 0., 0., -1., 0., 0.2); + Vector exp2 = (Vector(7) << 1., 0., 0., 0., -1., 0., 0.2).finished(); EXPECT(assert_equal(a2, exp2)); Vector a3 = row(A, 3); - Vector exp3 = (Vector(7) << 0., 1., 0., 0., 0., -1., -0.1); + Vector exp3 = (Vector(7) << 0., 1., 0., 0., 0., -1., -0.1).finished(); EXPECT(assert_equal(a3, exp3)); } @@ -290,13 +290,13 @@ TEST( matrix, zeros ) TEST( matrix, insert_sub ) { Matrix big = zeros(5, 6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0, - 1.0); + 1.0).finished(); insertSub(big, small, 1, 2); 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); + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(); EXPECT(assert_equal(expected, big)); } @@ -320,7 +320,7 @@ TEST( matrix, diagMatrices ) 0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 3.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 3.0); + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 3.0).finished(); EXPECT(assert_equal(expected, actual)); } @@ -330,7 +330,7 @@ TEST( matrix, stream_read ) { 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); + 1.2, 3.4, 4.5, 6.7).finished(); string matrixAsString = "1.1 2.3 4.2 7.6\n" @@ -362,7 +362,7 @@ TEST( matrix, scale_columns ) A(2, 2) = 1.; A(2, 3) = 1.; - Vector v = (Vector(4) << 2., 3., 4., 5.); + Vector v = (Vector(4) << 2., 3., 4., 5.).finished(); Matrix actual = vector_scale(A, v); @@ -400,7 +400,7 @@ TEST( matrix, scale_rows ) A(2, 2) = 1.; A(2, 3) = 1.; - Vector v = (Vector(3) << 2., 3., 4.); + Vector v = Vector3(2., 3., 4.); Matrix actual = vector_scale(v, A); @@ -438,7 +438,7 @@ TEST( matrix, scale_rows_mask ) A(2, 2) = 1.; A(2, 3) = 1.; - Vector v = (Vector(3) << 2., std::numeric_limits::infinity(), 4.); + Vector v = (Vector(3) << 2., std::numeric_limits::infinity(), 4.).finished(); Matrix actual = vector_scale(v, A, true); @@ -537,18 +537,18 @@ TEST( matrix, equal_nan ) /* ************************************************************************* */ TEST( matrix, addition ) { - 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); + Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); + Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); + Matrix C = (Matrix(2, 2) << 5.0, 5.0, 5.0, 5.0).finished(); EQUALITY(A+B,C); } /* ************************************************************************* */ TEST( matrix, addition_in_place ) { - 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); + Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); + Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); + Matrix C = (Matrix(2, 2) << 5.0, 5.0, 5.0, 5.0).finished(); A += B; EQUALITY(A,C); } @@ -556,18 +556,18 @@ TEST( matrix, addition_in_place ) /* ************************************************************************* */ TEST( matrix, subtraction ) { - 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); + Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); + Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); + Matrix C = (Matrix(2, 2) << -3.0, -1.0, 1.0, 3.0).finished(); EQUALITY(A-B,C); } /* ************************************************************************* */ TEST( matrix, subtraction_in_place ) { - 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); + Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); + Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); + Matrix C = (Matrix(2, 2) << -3.0, -1.0, 1.0, 3.0).finished(); A -= B; EQUALITY(A,C); } @@ -617,10 +617,10 @@ TEST( matrix, matrix_vector_multiplication ) { Vector result(2); - 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.); + Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished(); + Vector v = Vector3(1., 2., 3.); + Vector Av = Vector2(14., 32.); + Vector AtAv = Vector3(142., 188., 234.); EQUALITY(A*v,Av); EQUALITY(A^Av,AtAv); @@ -657,12 +657,12 @@ TEST( matrix, zero_below_diagonal ) { 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); + 1.0, 2.0, 3.0, 4.0).finished(); 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); + 0.0, 0.0, 3.0, 4.0).finished(); Matrix actual1r = A1; zeroBelowDiagonal(actual1r); EXPECT(assert_equal(expected1, actual1r, 1e-10)); @@ -680,13 +680,13 @@ TEST( matrix, zero_below_diagonal ) { 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).finished(); Matrix expected2 = (Matrix(5, 3) << 1.0, 2.0, 3.0, 0.0, 2.0, 3.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0); + 0.0, 0.0, 0.0).finished(); Matrix actual2r = A2; zeroBelowDiagonal(actual2r); @@ -701,7 +701,7 @@ TEST( matrix, zero_below_diagonal ) { 0.0, 2.0, 3.0, 0.0, 2.0, 3.0, 0.0, 2.0, 3.0, - 0.0, 2.0, 3.0); + 0.0, 2.0, 3.0).finished(); actual2c = A2; zeroBelowDiagonal(actual2c, 1); EXPECT(assert_equal(Matrix(expected2_partial), actual2c, 1e-10)); @@ -739,17 +739,17 @@ TEST( matrix, inverse ) EXPECT(assert_equal(expected, Ainv, 1e-4)); // These two matrices failed before version 2003 because we called LU incorrectly - Matrix lMg((Matrix(3, 3) << 0.0, 1.0, -2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0)); + Matrix lMg((Matrix(3, 3) << 0.0, 1.0, -2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0).finished()); EXPECT(assert_equal((Matrix(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, - 0.0, 0.0, 1.0), + 0.0, 0.0, 1.0).finished(), inverse(lMg))); - Matrix gMl((Matrix(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0)); + Matrix gMl((Matrix(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0).finished()); EXPECT(assert_equal((Matrix(3, 3) << 0.0, 1.0,-2.0, -1.0, 0.0, 1.0, - 0.0, 0.0, 1.0), + 0.0, 0.0, 1.0).finished(), inverse(gMl))); } @@ -787,19 +787,19 @@ TEST( matrix, inverse2 ) TEST( matrix, backsubtitution ) { // TEST ONE 2x2 matrix U1*x=b1 - Vector expected1 = (Vector(2) << 3.6250, -0.75); - Matrix U22 = (Matrix(2, 2) << 2., 3., 0., 4.); + Vector expected1 = Vector2(3.6250, -0.75); + Matrix U22 = (Matrix(2, 2) << 2., 3., 0., 4.).finished(); Vector b1 = U22 * expected1; EXPECT( assert_equal(expected1 , backSubstituteUpper(U22, b1), 0.000001)); // TEST TWO 3x3 matrix U2*x=b2 - Vector expected2 = (Vector(3) << 5.5, -8.5, 5.); - Matrix U33 = (Matrix(3, 3) << 3., 5., 6., 0., 2., 3., 0., 0., 1.); + Vector expected2 = Vector3(5.5, -8.5, 5.); + Matrix U33 = (Matrix(3, 3) << 3., 5., 6., 0., 2., 3., 0., 0., 1.).finished(); Vector b2 = U33 * expected2; EXPECT( assert_equal(expected2 , backSubstituteUpper(U33, b2), 0.000001)); // TEST THREE Lower triangular 3x3 matrix L3*x=b3 - Vector expected3 = (Vector(3) << 1., 1., 1.); + Vector expected3 = Vector3(1., 1., 1.); Matrix L3 = trans(U33); Vector b3 = L3 * expected3; EXPECT( assert_equal(expected3 , backSubstituteLower(L3, b3), 0.000001)); @@ -816,11 +816,11 @@ TEST( matrix, householder ) 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); + 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894).finished(); 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 ); + 00, 10,0, 0, 0, -10, -1 ).finished(); householder_(A1, 3); EXPECT(assert_equal(expected1, A1, 1e-3)); @@ -828,11 +828,11 @@ TEST( matrix, householder ) 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); + 0, 0, 4.4721, 0, -4.4721, 0.894).finished(); 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); + 00, 10,0, 0, 0, -10, -1).finished(); householder(A2, 3); EXPECT(assert_equal(expected, A2, 1e-3)); } @@ -845,11 +845,11 @@ TEST( matrix, householder_colMajor ) 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)); + 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894).finished()); 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)); + 00, 10,0, 0, 0, -10, -1).finished()); householder_(A1, 3); EXPECT(assert_equal(expected1, A1, 1e-3)); @@ -857,11 +857,11 @@ TEST( matrix, householder_colMajor ) 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)); + 0, 0, 4.4721, 0, -4.4721, 0.894).finished()); 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)); + 00, 10,0, 0, 0, -10, -1).finished()); householder(A2, 3); EXPECT(assert_equal(expected, A2, 1e-3)); } @@ -875,11 +875,11 @@ TEST( matrix, eigen_QR ) 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)); + 0, 0, 4.4721, 0, -4.4721, 0.894).finished()); 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)); + 00, 10,0, 0, 0, -10, -1).finished()); Matrix actual = A.householderQr().matrixQR(); zeroBelowDiagonal(actual); @@ -889,7 +889,7 @@ TEST( matrix, eigen_QR ) 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)); + 00, 10,0, 0, 0, -10, -1).finished()); inplace_QR(A); EXPECT(assert_equal(expected, A, 1e-3)); } @@ -902,16 +902,16 @@ TEST( matrix, qr ) { 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); + 0, 0, -10, 10, 0, -10, 0).finished(); 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); + 0, -0.5963, 0, 0, -0.4472).finished(); 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); + 7.4536, 0, 00, 0, 0, 10.9545, 00, 0, 0, 0, 00, 0, 0, 0).finished(); Matrix Q, R; boost::tie(Q, R) = qr(A); @@ -924,10 +924,10 @@ TEST( matrix, qr ) TEST( matrix, sub ) { 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); + 0, 00, 10, 0, 0, 0, -10).finished(); Matrix actual = sub(A, 1, 3, 1, 5); - Matrix expected = (Matrix(2, 4) << -5, 0, 5, 0, 00, 0, 0, -10); + Matrix expected = (Matrix(2, 4) << -5, 0, 5, 0, 00, 0, 0, -10).finished(); EQUALITY(actual,expected); } @@ -935,15 +935,15 @@ TEST( matrix, sub ) /* ************************************************************************* */ TEST( matrix, trans ) { - 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); + Matrix A = (Matrix(2, 2) << 1.0, 3.0, 2.0, 4.0).finished(); + Matrix B = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); EQUALITY(trans(A),B); } /* ************************************************************************* */ TEST( matrix, col_major_access ) { - Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0); + Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); const double* a = &A(0, 0); DOUBLES_EQUAL(2.0,a[2],1e-9); } @@ -953,15 +953,15 @@ TEST( matrix, weighted_elimination ) { // create a matrix to eliminate 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 = (Vector(4) << -0.2, 0.3, 0.2, -0.1); - Vector sigmas = (Vector(4) << 0.2, 0.2, 0.1, 0.1); + 1., 0., 0., 0., -1., 0., 0., 1., 0., 0., 0., -1.).finished(); + Vector b = (Vector(4) << -0.2, 0.3, 0.2, -0.1).finished(); + Vector sigmas = (Vector(4) << 0.2, 0.2, 0.1, 0.1).finished(); // expected values 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 = (Vector(4) << 0.2, -0.14, 0.0, 0.2); - Vector newSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607); + -0.2, 0., -0.8, 0., 0., 1., 0., -1., 0., 0., 0., 0., 1., 0., -1.).finished(); + Vector d = (Vector(4) << 0.2, -0.14, 0.0, 0.2).finished(); + Vector newSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished(); Vector r; double di, sigma; @@ -987,11 +987,11 @@ TEST( matrix, weighted_elimination ) TEST( matrix, inverse_square_root ) { Matrix measurement_covariance = (Matrix(3, 3) << 0.25, 0.0, 0.0, 0.0, 0.25, - 0.0, 0.0, 0.0, 0.01); + 0.0, 0.0, 0.0, 0.01).finished(); Matrix actual = inverse_square_root(measurement_covariance); Matrix expected = (Matrix(3, 3) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, - 10.0); + 10.0).finished(); EQUALITY(expected,actual); EQUALITY(measurement_covariance,inverse(actual*actual)); @@ -1007,14 +1007,14 @@ TEST( matrix, inverse_square_root ) 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); + 0.0028726, 0.0175868, 0.0047064, -0.0059468, 0.0816517).finished(); 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, 0.683045380655496, 0.283773848115276, -0.099969232183396, 3.433537147891568, 0.000000000000000, - -0.006740136923185, -0.669325697387650, -0.169716689114923, 0.171493059476284, 3.583921085468937); + -0.006740136923185, -0.669325697387650, -0.169716689114923, 0.171493059476284, 3.583921085468937).finished(); EQUALITY(expected, inverse_square_root(M)); } @@ -1027,14 +1027,14 @@ 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); + -0.0021113, 0.0036415, 0.0909464).finished(); 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, 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, - 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); + 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247).finished(); } TEST( matrix, LLt ) { @@ -1053,8 +1053,8 @@ TEST( matrix, cholesky_inverse ) /* ************************************************************************* */ TEST( matrix, multiplyAdd ) { - 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.), + Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); + Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), expected = e + A * x; multiplyAdd(1, A, x, e); @@ -1064,8 +1064,8 @@ TEST( matrix, multiplyAdd ) /* ************************************************************************* */ TEST( matrix, transposeMultiplyAdd ) { - 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.), + Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); + Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), expected = x + trans(A) * e; transposeMultiplyAdd(1, A, e, x); @@ -1075,31 +1075,31 @@ TEST( matrix, transposeMultiplyAdd ) /* ************************************************************************* */ TEST( matrix, linear_dependent ) { - 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); + Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished(); + Matrix B = (Matrix(2, 3) << -1.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished(); EXPECT(linear_dependent(A, B)); } /* ************************************************************************* */ TEST( matrix, linear_dependent2 ) { - 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); + Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished(); + Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished(); EXPECT(linear_dependent(A, B)); } /* ************************************************************************* */ TEST( matrix, linear_dependent3 ) { - 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); + Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished(); + Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.1, 10.0, 12.0).finished(); EXPECT(linear_independent(A, B)); } /* ************************************************************************* */ TEST( matrix, svd1 ) { - Vector v = (Vector(3) << 2., 1., 0.); + Vector v = Vector3(2., 1., 0.); Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1) * Matrix(trans(V1)); Matrix U, V; @@ -1112,7 +1112,7 @@ TEST( matrix, svd1 ) /* ************************************************************************* */ /// Sample A matrix for SVD -static Matrix sampleA = (Matrix(3, 2) << 0.,-2., 0., 0., 3., 0.); +static Matrix sampleA = (Matrix(3, 2) << 0.,-2., 0., 0., 3., 0.).finished(); static Matrix sampleAt = trans(sampleA); /* ************************************************************************* */ @@ -1121,9 +1121,9 @@ TEST( matrix, svd2 ) Matrix U, V; Vector s; - 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.); + Matrix expectedU = (Matrix(3, 2) << 0.,-1.,0.,0.,1.,0.).finished(); + Vector expected_s = Vector2(3.,2.); + Matrix expectedV = (Matrix(2, 2) << 1.,0.,0.,1.).finished(); svd(sampleA, U, s, V); @@ -1144,9 +1144,9 @@ TEST( matrix, svd3 ) Matrix U, V; Vector s; - 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.); + Matrix expectedU = (Matrix(2, 2) << -1.,0.,0.,-1.).finished(); + Vector expected_s = Vector2(3.0, 2.0); + Matrix expectedV = (Matrix(3, 2) << 0.,1.,0.,0.,-1.,0.).finished(); svd(sampleAt, U, s, V); @@ -1175,18 +1175,18 @@ TEST( matrix, svd4 ) Matrix A = (Matrix(3, 2) << 0.8147, 0.9134, 0.9058, 0.6324, - 0.1270, 0.0975); + 0.1270, 0.0975).finished(); Matrix expectedU = (Matrix(3, 2) << 0.7397, 0.6724, 0.6659, -0.7370, - 0.0970, -0.0689); + 0.0970, -0.0689).finished(); - Vector expected_s = (Vector(2) << 1.6455, 0.1910); + Vector expected_s = Vector2(1.6455, 0.1910); Matrix expectedV = (Matrix(2, 2) << 0.7403, -0.6723, - 0.6723, 0.7403); + 0.6723, 0.7403).finished(); svd(A, U, s, V); @@ -1220,12 +1220,12 @@ TEST( matrix, DLT ) 1.56, 0.42, 4.56, -3.38, -0.91, -9.88, 22.36, 6.02, 65.36, 1.89, 2.24, 3.99, 3.24, 3.84, 6.84, 18.09, 21.44, 38.19, 2.24, 2.48, 6.24, 3.08, 3.41, 8.58, 24.64, 27.28, 68.64 - ); + ).finished(); int rank; double error; Vector actual; boost::tie(rank,error,actual) = DLT(A); - Vector expected = (Vector(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).finished(); 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 b4a9b91d6..d3a2b929f 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -65,7 +65,7 @@ TEST(testNumericalDerivative, numericalHessian2) { Vector2 x(v); Matrix expected = (Matrix(2, 2) << -cos(x(1)) * sin(x(0)), -sin(x(1)) - * cos(x(0)), -cos(x(0)) * sin(x(1)), -sin(x(0)) * cos(x(1))); + * cos(x(0)), -cos(x(0)) * sin(x(1)), -sin(x(0)) * cos(x(1))).finished(); Matrix actual = numericalHessian(f2, x); @@ -82,15 +82,15 @@ double f3(double x1, double x2) { TEST(testNumericalDerivative, numericalHessian211) { double x1 = 1, x2 = 5; - Matrix expected11 = (Matrix(1, 1) << -sin(x1) * cos(x2)); + Matrix expected11 = (Matrix(1, 1) << -sin(x1) * cos(x2)).finished(); Matrix actual11 = numericalHessian211(f3, x1, x2); EXPECT(assert_equal(expected11, actual11, 1e-5)); - Matrix expected12 = (Matrix(1, 1) << -cos(x1) * sin(x2)); + Matrix expected12 = (Matrix(1, 1) << -cos(x1) * sin(x2)).finished(); Matrix actual12 = numericalHessian212(f3, x1, x2); EXPECT(assert_equal(expected12, actual12, 1e-5)); - Matrix expected22 = (Matrix(1, 1) << -sin(x1) * cos(x2)); + Matrix expected22 = (Matrix(1, 1) << -sin(x1) * cos(x2)).finished(); Matrix actual22 = numericalHessian222(f3, x1, x2); EXPECT(assert_equal(expected22, actual22, 1e-5)); } @@ -104,27 +104,27 @@ double f4(double x, double y, double z) { // TEST(testNumericalDerivative, numericalHessian311) { double x = 1, y = 2, z = 3; - Matrix expected11 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z); + Matrix expected11 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z).finished(); Matrix actual11 = numericalHessian311(f4, x, y, z); EXPECT(assert_equal(expected11, actual11, 1e-5)); - Matrix expected12 = (Matrix(1, 1) << -cos(x) * sin(y) * z * z); + Matrix expected12 = (Matrix(1, 1) << -cos(x) * sin(y) * z * z).finished(); Matrix actual12 = numericalHessian312(f4, x, y, z); EXPECT(assert_equal(expected12, actual12, 1e-5)); - Matrix expected13 = (Matrix(1, 1) << cos(x) * cos(y) * 2 * z); + Matrix expected13 = (Matrix(1, 1) << cos(x) * cos(y) * 2 * z).finished(); Matrix actual13 = numericalHessian313(f4, x, y, z); EXPECT(assert_equal(expected13, actual13, 1e-5)); - Matrix expected22 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z); + Matrix expected22 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z).finished(); Matrix actual22 = numericalHessian322(f4, x, y, z); EXPECT(assert_equal(expected22, actual22, 1e-5)); - Matrix expected23 = (Matrix(1, 1) << -sin(x) * sin(y) * 2 * z); + Matrix expected23 = (Matrix(1, 1) << -sin(x) * sin(y) * 2 * z).finished(); Matrix actual23 = numericalHessian323(f4, x, y, z); EXPECT(assert_equal(expected23, actual23, 1e-5)); - Matrix expected33 = (Matrix(1, 1) << sin(x) * cos(y) * 2); + Matrix expected33 = (Matrix(1, 1) << sin(x) * cos(y) * 2).finished(); Matrix actual33 = numericalHessian333(f4, x, y, z); EXPECT(assert_equal(expected33, actual33, 1e-5)); } diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp index 9b28eacf6..fceb2f4b4 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 = (Vector(2) << 1.0, 2.0); -Vector v2 = (Vector(2) << 3.0, 4.0); -Vector v3 = (Vector(2) << 5.0, 6.0); +Vector v1 = Vector2(1.0, 2.0); +Vector v2 = Vector2(3.0, 4.0); +Vector v3 = Vector2(5.0, 6.0); /* ************************************************************************* */ TEST (Serialization, FastList) { @@ -84,23 +84,23 @@ TEST (Serialization, FastVector) { /* ************************************************************************* */ TEST (Serialization, matrix_vector) { - EXPECT(equality((Vector(4) << 1.0, 2.0, 3.0, 4.0))); + EXPECT(equality((Vector(4) << 1.0, 2.0, 3.0, 4.0).finished())); 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((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0))); + EXPECT(equality((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished())); - EXPECT(equalityXML((Vector(4) << 1.0, 2.0, 3.0, 4.0))); + EXPECT(equalityXML((Vector(4) << 1.0, 2.0, 3.0, 4.0).finished())); 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((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0))); + EXPECT(equalityXML((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished())); - EXPECT(equalityBinary((Vector(4) << 1.0, 2.0, 3.0, 4.0))); + EXPECT(equalityBinary((Vector(4) << 1.0, 2.0, 3.0, 4.0).finished())); 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((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0))); + EXPECT(equalityBinary((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished())); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testSymmetricBlockMatrix.cpp b/gtsam/base/tests/testSymmetricBlockMatrix.cpp index ed9d43e5a..df3403cf4 100644 --- a/gtsam/base/tests/testSymmetricBlockMatrix.cpp +++ b/gtsam/base/tests/testSymmetricBlockMatrix.cpp @@ -31,7 +31,7 @@ static SymmetricBlockMatrix testBlockMatrix( 3, 9, 15, 16, 17, 18, 4, 10, 16, 22, 23, 24, 5, 11, 17, 23, 29, 30, - 6, 12, 18, 24, 30, 36)); + 6, 12, 18, 24, 30, 36).finished()); /* ************************************************************************* */ TEST(SymmetricBlockMatrix, ReadBlocks) @@ -39,7 +39,7 @@ TEST(SymmetricBlockMatrix, ReadBlocks) // On the diagonal Matrix expected1 = (Matrix(2, 2) << 22, 23, - 23, 29); + 23, 29).finished(); Matrix actual1 = testBlockMatrix(1, 1); // Test only writing the upper triangle for efficiency Matrix actual1t = Matrix::Zero(2, 2); @@ -51,14 +51,14 @@ TEST(SymmetricBlockMatrix, ReadBlocks) Matrix expected2 = (Matrix(3, 2) << 4, 5, 10, 11, - 16, 17); + 16, 17).finished(); Matrix actual2 = testBlockMatrix(0, 1); EXPECT(assert_equal(expected2, actual2)); // Below the diagonal Matrix expected3 = (Matrix(2, 3) << 4, 10, 16, - 5, 11, 17); + 5, 11, 17).finished(); Matrix actual3 = testBlockMatrix(1, 0); EXPECT(assert_equal(expected3, actual3)); } @@ -102,7 +102,7 @@ TEST(SymmetricBlockMatrix, Ranges) Matrix expected1 = (Matrix(3, 3) << 22, 23, 24, 23, 29, 30, - 24, 30, 36); + 24, 30, 36).finished(); Matrix actual1 = testBlockMatrix.range(1, 3, 1, 3).selfadjointView(); Matrix actual1a = testBlockMatrix.range(1, 3, 1, 3); EXPECT(assert_equal(expected1, actual1)); @@ -112,7 +112,7 @@ TEST(SymmetricBlockMatrix, Ranges) Matrix expected2 = (Matrix(3, 1) << 24, 30, - 36); + 36).finished(); Matrix actual2 = testBlockMatrix.range(1, 3, 2, 3).knownOffDiagonal(); Matrix actual2a = testBlockMatrix.range(1, 3, 2, 3); EXPECT(assert_equal(expected2, actual2)); @@ -122,7 +122,7 @@ TEST(SymmetricBlockMatrix, Ranges) Matrix expected3 = (Matrix(3, 3) << 4, 10, 16, 5, 11, 17, - 6, 12, 18); + 6, 12, 18).finished(); Matrix actual3 = testBlockMatrix.range(1, 3, 0, 1).knownOffDiagonal(); Matrix actual3a = testBlockMatrix.range(1, 3, 0, 1); EXPECT(assert_equal(expected3, actual3)); @@ -138,7 +138,7 @@ TEST(SymmetricBlockMatrix, expressions) 0, 0, 4, 6, 8, 0, 0, 0, 0, 9, 12, 0, 0, 0, 0, 0, 16, 0, - 0, 0, 0, 0, 0, 0)); + 0, 0, 0, 0, 0, 0).finished()); SymmetricBlockMatrix expected2(list_of(2)(3)(1), (Matrix(6, 6) << 0, 0, 10, 15, 20, 0, @@ -146,10 +146,10 @@ TEST(SymmetricBlockMatrix, expressions) 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, 0, 0, 0).finished()); - Matrix a = (Matrix(1, 3) << 2, 3, 4); - Matrix b = (Matrix(1, 2) << 5, 6); + Matrix a = (Matrix(1, 3) << 2, 3, 4).finished(); + Matrix b = (Matrix(1, 2) << 5, 6).finished(); 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 ee0d94366..460ff6cd6 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -47,17 +47,17 @@ TEST( TestVector, special_comma_initializer) expected(1) = 2; expected(2) = 3; - Vector actual1 = (Vector(3) << 1, 2, 3); - Vector actual2((Vector(3) << 1, 2, 3)); + Vector actual1 = Vector3(1, 2, 3); + Vector actual2(Vector3(1, 2, 3)); - Vector subvec1 = (Vector(2) << 2, 3); - Vector actual4 = (Vector(3) << 1, subvec1); + Vector subvec1 = Vector2(2, 3); + Vector actual4 = (Vector(3) << 1, subvec1).finished(); - Vector subvec2 = (Vector(2) << 1, 2); - Vector actual5 = (Vector(3) << subvec2, 3); + Vector subvec2 = Vector2(1, 2); + Vector actual5 = (Vector(3) << subvec2, 3).finished(); - Vector actual6 = testFcn1((Vector(3) << 1, 2, 3)); - Vector actual7 = testFcn2((Vector(3) << 1, 2, 3)); + Vector actual6 = testFcn1(Vector3(1, 2, 3)); + Vector actual7 = testFcn2(Vector3(1, 2, 3)); EXPECT(assert_equal(expected, actual1)); EXPECT(assert_equal(expected, actual2)); @@ -142,7 +142,7 @@ TEST( TestVector, subInsert ) size_t i = 2; subInsert(big, small, i); - Vector expected = (Vector(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).finished(); EXPECT(assert_equal(expected, big)); } @@ -240,13 +240,13 @@ TEST( TestVector, weightedPseudoinverse_constraint ) /* ************************************************************************* */ TEST( TestVector, weightedPseudoinverse_nan ) { - Vector a = (Vector(4) << 1., 0., 0., 0.); - Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.); + Vector a = (Vector(4) << 1., 0., 0., 0.).finished(); + Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished(); Vector weights = reciprocal(emul(sigmas,sigmas)); Vector pseudo; double precision; boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights); - Vector expected = (Vector(4) << 1., 0., 0.,0.); + Vector expected = (Vector(4) << 1., 0., 0.,0.).finished(); EXPECT(assert_equal(expected, pseudo)); DOUBLES_EQUAL(100, precision, 1e-5); } @@ -254,31 +254,31 @@ TEST( TestVector, weightedPseudoinverse_nan ) /* ************************************************************************* */ TEST( TestVector, ediv ) { - Vector a = (Vector(3) << 10., 20., 30.); - Vector b = (Vector(3) << 2.0, 5.0, 6.0); + Vector a = Vector3(10., 20., 30.); + Vector b = Vector3(2.0, 5.0, 6.0); Vector actual(ediv(a,b)); - Vector c = (Vector(3) << 5.0, 4.0, 5.0); + Vector c = Vector3(5.0, 4.0, 5.0); EXPECT(assert_equal(c,actual)); } /* ************************************************************************* */ TEST( TestVector, dot ) { - Vector a = (Vector(3) << 10., 20., 30.); - Vector b = (Vector(3) << 2.0, 5.0, 6.0); + Vector a = Vector3(10., 20., 30.); + Vector b = Vector3(2.0, 5.0, 6.0); DOUBLES_EQUAL(20+100+180,dot(a,b),1e-9); } /* ************************************************************************* */ TEST( TestVector, axpy ) { - Vector x = (Vector(3) << 10., 20., 30.); - Vector y0 = (Vector(3) << 2.0, 5.0, 6.0); + Vector x = Vector3(10., 20., 30.); + Vector y0 = Vector3(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 = (Vector(3) << 3.0, 7.0, 9.0); + Vector expected = Vector3(3.0, 7.0, 9.0); EXPECT(assert_equal(expected,y1)); EXPECT(assert_equal(expected,Vector(y2))); } @@ -286,8 +286,8 @@ TEST( TestVector, axpy ) /* ************************************************************************* */ TEST( TestVector, equals ) { - Vector v1 = (Vector(1) << 0.0/std::numeric_limits::quiet_NaN()); //testing nan - Vector v2 = (Vector(1) << 1.0); + Vector v1 = (Vector(1) << 0.0/std::numeric_limits::quiet_NaN()).finished(); //testing nan + Vector v2 = (Vector(1) << 1.0).finished(); double tol = 1.; EXPECT(!equal_with_abs_tol(v1, v2, tol)); } @@ -295,7 +295,7 @@ TEST( TestVector, equals ) /* ************************************************************************* */ TEST( TestVector, greater_than ) { - Vector v1 = (Vector(3) << 1.0, 2.0, 3.0), + Vector v1 = Vector3(1.0, 2.0, 3.0), v2 = zero(3); EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than EXPECT(greaterThanOrEqual(v1, v2)); // test equals @@ -304,31 +304,31 @@ TEST( TestVector, greater_than ) /* ************************************************************************* */ TEST( TestVector, reciprocal ) { - Vector v = (Vector(3) << 1.0, 2.0, 4.0); - EXPECT(assert_equal((Vector(3) << 1.0, 0.5, 0.25),reciprocal(v))); + Vector v = Vector3(1.0, 2.0, 4.0); + EXPECT(assert_equal(Vector3(1.0, 0.5, 0.25),reciprocal(v))); } /* ************************************************************************* */ TEST( TestVector, linear_dependent ) { - Vector v1 = (Vector(3) << 1.0, 2.0, 3.0); - Vector v2 = (Vector(3) << -2.0, -4.0, -6.0); + Vector v1 = Vector3(1.0, 2.0, 3.0); + Vector v2 = Vector3(-2.0, -4.0, -6.0); EXPECT(linear_dependent(v1, v2)); } /* ************************************************************************* */ TEST( TestVector, linear_dependent2 ) { - Vector v1 = (Vector(3) << 0.0, 2.0, 0.0); - Vector v2 = (Vector(3) << 0.0, -4.0, 0.0); + Vector v1 = Vector3(0.0, 2.0, 0.0); + Vector v2 = Vector3(0.0, -4.0, 0.0); EXPECT(linear_dependent(v1, v2)); } /* ************************************************************************* */ TEST( TestVector, linear_dependent3 ) { - Vector v1 = (Vector(3) << 0.0, 2.0, 0.0); - Vector v2 = (Vector(3) << 0.1, -4.1, 0.0); + Vector v1 = Vector3(0.0, 2.0, 0.0); + Vector v2 = Vector3(0.1, -4.1, 0.0); EXPECT(!linear_dependent(v1, v2)); } diff --git a/gtsam/base/tests/testVerticalBlockMatrix.cpp b/gtsam/base/tests/testVerticalBlockMatrix.cpp index c504752aa..8bb4474a4 100644 --- a/gtsam/base/tests/testVerticalBlockMatrix.cpp +++ b/gtsam/base/tests/testVerticalBlockMatrix.cpp @@ -43,7 +43,7 @@ TEST(VerticalBlockMatrix, Constructor2) { 3, 9, 15, 16, 17, 18, // 4, 10, 16, 22, 23, 24, // 5, 11, 17, 23, 29, 30, // - 6, 12, 18, 24, 30, 36)); + 6, 12, 18, 24, 30, 36).finished()); EXPECT_LONGS_EQUAL(6,actual.rows()); EXPECT_LONGS_EQUAL(6,actual.cols()); EXPECT_LONGS_EQUAL(3,actual.nBlocks()); diff --git a/gtsam/discrete/tests/testDiscreteMarginals.cpp b/gtsam/discrete/tests/testDiscreteMarginals.cpp index c088bcb4e..24bec4fa1 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((Vector(2) << 0.359631, 0.640369), actualCvector, 1e-6)); + EXPECT(assert_equal(Vector2(0.359631, 0.640369), actualCvector, 1e-6)); actualCvector = marginals.marginalProbabilities(Mark); - EXPECT(assert_equal((Vector(2) << 0.48628, 0.51372), actualCvector, 1e-6)); + EXPECT(assert_equal(Vector2(0.48628, 0.51372), actualCvector, 1e-6)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index ab5fde629..8ff728d26 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -42,17 +42,17 @@ Matrix Cal3Bundler::K() const { /* ************************************************************************* */ Vector Cal3Bundler::k() const { - return (Vector(4) << k1_, k2_, 0, 0); + return (Vector(4) << k1_, k2_, 0, 0).finished(); } /* ************************************************************************* */ Vector3 Cal3Bundler::vector() const { - return (Vector(3) << f_, k1_, k2_); + return (Vector(3) << f_, k1_, k2_).finished(); } /* ************************************************************************* */ void Cal3Bundler::print(const std::string& s) const { - gtsam::print((Vector)(Vector(5) << f_, k1_, k2_, u0_, v0_), s + ".K"); + gtsam::print((Vector)(Vector(5) << f_, k1_, k2_, u0_, v0_).finished(), s + ".K"); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index e4397a449..1105fecfb 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -29,12 +29,12 @@ Cal3DS2_Base::Cal3DS2_Base(const Vector &v): /* ************************************************************************* */ Matrix Cal3DS2_Base::K() const { - return (Matrix(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).finished(); } /* ************************************************************************* */ Vector Cal3DS2_Base::vector() const { - return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_); + return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_).finished(); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index 6113714a1..6bc4c4bb3 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -30,13 +30,13 @@ Cal3Unified::Cal3Unified(const Vector &v): /* ************************************************************************* */ Vector Cal3Unified::vector() const { - return (Vector(10) << Base::vector(), xi_); + return (Vector(10) << Base::vector(), xi_).finished(); } /* ************************************************************************* */ void Cal3Unified::print(const std::string& s) const { Base::print(s); - gtsam::print((Vector)(Vector(1) << xi_), s + ".xi"); + gtsam::print((Vector)(Vector(1) << xi_).finished(), s + ".xi"); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 2f0253e4f..6317d251d 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 (Matrix(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).finished(); } /** @deprecated The following function has been deprecated, use K above */ @@ -138,7 +138,7 @@ public: Matrix matrix_inverse() const { const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_; return (Matrix(3, 3) << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, - 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0); + 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0).finished(); } /** diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 35079827b..392a53858 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 = (Matrix(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).finished(); } return Point2(P.x() / P.z(), P.y() / P.z()); } @@ -85,13 +85,13 @@ Point2 CalibratedCamera::project(const Point3& point, const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v; if (Dpose) *Dpose = (Matrix(2, 6) << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), - -uv, -u, 0., -d, d * v); + -uv, -u, 0., -d, d * v).finished(); if (Dpoint) { const Matrix R(pose_.rotation().matrix()); *Dpoint = d * (Matrix(2, 3) << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), 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)); + R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2)).finished(); } #endif } diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 636b5b7a2..e65e5d097 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -55,8 +55,8 @@ EssentialMatrix EssentialMatrix::retract(const Vector& xi) const { /* ************************************************************************* */ Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { - return Vector(5) << // - aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_); + return (Vector(5) << + aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_)).finished(); } /* ************************************************************************* */ diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index d596001e5..fd5f45160 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -32,7 +32,7 @@ public: /// Static function to convert Point2 to homogeneous coordinates static Vector Homogeneous(const Point2& p) { - return Vector(3) << p.x(), p.y(), 1; + return (Vector(3) << p.x(), p.y(), 1).finished(); } /// @name Constructors and named constructors diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index dc42a5564..a4e0cc296 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -171,7 +171,7 @@ public: static inline Point2 Expmap(const Vector& v) { return Point2(v); } /// Log map around identity - just return the Point2 as a vector - static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()); } + static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()).finished(); } /// @} /// @name Vector Space diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 90c3f5f8c..7a693ed3d 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 = (Matrix(3, 1) << t_.x(), t_.y(), 1.0); + Matrix T = (Matrix(3, 1) << t_.x(), t_.y(), 1.0).finished(); 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 (Vector(3) << t.x(), t.y(), w); + return (Vector(3) << t.x(), t.y(), w).finished(); 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 (Vector(3) << v.x(), v.y(), w); + return (Vector(3) << v.x(), v.y(), w).finished(); } } @@ -101,7 +101,7 @@ Vector Pose2::localCoordinates(const Pose2& p2) const { return Logmap(between(p2)); #else Pose2 r = between(p2); - return (Vector(3) << r.x(), r.y(), r.theta()); + return (Vector(3) << r.x(), r.y(), r.theta()).finished(); #endif } @@ -114,7 +114,7 @@ Matrix Pose2::AdjointMap() const { c, -s, y, s, c, -x, 0.0, 0.0, 1.0 - ); + ).finished(); } /* ************************************************************************* */ @@ -153,7 +153,7 @@ Point2 Pose2::transform_to(const Point2& point, if (!H1 && !H2) return q; if (H1) *H1 = (Matrix(2, 3) << -1.0, 0.0, q.y(), - 0.0, -1.0, -q.x()); + 0.0, -1.0, -q.x()).finished(); if (H2) *H2 = r_.transpose(); return q; } @@ -175,7 +175,7 @@ Point2 Pose2::transform_from(const Point2& p, const Point2 q = r_ * p; if (H1 || H2) { const Matrix R = r_.matrix(); - const Matrix Drotate1 = (Matrix(2, 1) << -q.y(), q.x()); + const Matrix Drotate1 = (Matrix(2, 1) << -q.y(), q.x()).finished(); if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q] if (H2) *H2 = R; // R } @@ -265,7 +265,7 @@ Pose2 Pose2::between(const Pose2& p2, boost::optional H1, *H1 = (Matrix(3, 3) << -c, -s, dt1, s, -c, dt2, - 0.0, 0.0,-1.0); + 0.0, 0.0,-1.0).finished(); } if (H2) *H2 = I3; @@ -305,7 +305,7 @@ double Pose2::range(const Point2& point, double r = d.norm(H); if (H1) *H1 = H * (Matrix(2, 3) << -r_.c(), r_.s(), 0.0, - -r_.s(), -r_.c(), 0.0); + -r_.s(), -r_.c(), 0.0).finished(); if (H2) *H2 = H; return r; } @@ -320,10 +320,10 @@ double Pose2::range(const Pose2& pose2, double r = d.norm(H); if (H1) *H1 = H * (Matrix(2, 3) << -r_.c(), r_.s(), 0.0, - -r_.s(), -r_.c(), 0.0); + -r_.s(), -r_.c(), 0.0).finished(); if (H2) *H2 = H * (Matrix(2, 3) << pose2.r_.c(), -pose2.r_.s(), 0.0, - pose2.r_.s(), pose2.r_.c(), 0.0); + pose2.r_.s(), pose2.r_.c(), 0.0).finished(); return r; } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 88456676d..d43be6afb 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -183,7 +183,7 @@ public: return (Matrix(3,3) << 0.,-w, vx, w, 0., vy, - 0., 0., 0.); + 0., 0., 0.).finished(); } /// @} diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 7c4ac34e2..b134553d9 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -99,7 +99,7 @@ Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, Matrix6 Pose3::dExpInv_exp(const Vector& xi) { // Bernoulli numbers, from Wikipedia static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0, - 0.0, 1.0 / 42.0, 0.0, -1.0 / 30); + 0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished(); static const int N = 5; // order of approximation Matrix res = I6; Matrix6 ad_i = I6; @@ -333,7 +333,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 = (Matrix(1, 3) << x / n, y / n, z / n); + Matrix D_result_d = (Matrix(1, 3) << x / n, y / n, z / n).finished(); if (H1) *H1 = D_result_d * (*H1); if (H2) diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 8275c2feb..2090558a6 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -203,7 +203,7 @@ public: * as detailed in [Kobilarov09siggraph] eq. (15) * The full formula is documented in [Celledoni99cmame] * Elena Celledoni and Brynjulf Owren. Lie group methods for rigid body dynamics and - * time integration on manifolds. Comput. meth. in Appl. Mech. and Eng., 19(3,4):421� 438, 2003. + * time integration on manifolds. Comput. meth. in Appl. Mech. and Eng., 19(3,4):421-438, 2003. * and in [Hairer06book] in formula (4.5), pg. 84, Lemma 4.2 * Ernst Hairer, et al., Geometric Numerical Integration, * Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd edition, Springer-Verlag, 2006. @@ -222,7 +222,7 @@ public: 0.,-wz, wy, vx, wz, 0.,-wx, vy, -wy, wx, 0., vz, - 0., 0., 0., 0.); + 0., 0., 0., 0.).finished(); } /// @} diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 27f6f9cd8..0133c9440 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -65,12 +65,12 @@ Rot2& Rot2::normalize() { /* ************************************************************************* */ Matrix Rot2::matrix() const { - return (Matrix(2, 2) << c_, -s_, s_, c_); + return (Matrix(2, 2) << c_, -s_, s_, c_).finished(); } /* ************************************************************************* */ Matrix Rot2::transpose() const { - return (Matrix(2, 2) << c_, s_, -s_, c_); + return (Matrix(2, 2) << c_, s_, -s_, c_).finished(); } /* ************************************************************************* */ @@ -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 = (Matrix(2, 1) << -q.y(), q.x()); + if (H1) *H1 = (Matrix(2, 1) << -q.y(), q.x()).finished(); 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 = (Matrix(2, 1) << q.y(), -q.x()); // R_{pi/2}q + if (H1) *H1 = (Matrix(2, 1) << q.y(), -q.x()).finished(); // 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 = (Matrix(1, 2) << -y / d2, x / d2); + if (H) *H = (Matrix(1, 2) << -y / d2, x / d2).finished(); return Rot2::fromCosSin(x / n, y / n); } else { - if (H) *H = (Matrix(1, 2) << 0.0, 0.0); + if (H) *H = (Matrix(1, 2) << 0.0, 0.0).finished(); return Rot2(); } } diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index fe3d72c19..af9148fd3 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -170,7 +170,7 @@ namespace gtsam { ///Log map at identity - return the canonical coordinates of this rotation static inline Vector Logmap(const Rot2& r) { - return (Vector(1) << r.theta()); + return (Vector(1) << r.theta()).finished(); } /// @} diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 8f3ab9fbd..ef2d19750 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -193,7 +193,7 @@ namespace gtsam { * @return incremental rotation matrix */ static Rot3 rodriguez(double wx, double wy, double wz) - { return rodriguez((Vector(3) << wx, wy, wz));} + { return rodriguez((Vector(3) << wx, wy, wz).finished());} /// @} /// @name Testable diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index ed531a2bd..f48c188aa 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -68,7 +68,7 @@ namespace gtsam { 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 - ); + ).finished(); } if (H2) { const Matrix R(leftCamPose_.rotation().matrix()); @@ -76,7 +76,7 @@ namespace gtsam { 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 - ); + ).finished(); } #endif } @@ -94,7 +94,7 @@ namespace gtsam { 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() - ); + ).finished(); } } diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 82ba979fd..5e13129cc 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -162,9 +162,9 @@ Vector Unit3::localCoordinates(const Unit3& y) const { // Check for special cases if (std::abs(dot - 1.0) < 1e-16) - return (Vector(2) << 0, 0); + return Vector2(0, 0); else if (std::abs(dot + 1.0) < 1e-16) - return (Vector(2) << M_PI, 0); + return (Vector(2) << M_PI, 0).finished(); else { // no special case double theta = acos(dot); diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index eddd3a674..c5a6be2d6 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -36,7 +36,7 @@ TEST( Cal3DS2, uncalibrate) double g = 1+k[0]*r+k[1]*r*r ; double tx = 2*k[2]*p.x()*p.y() + k[3]*(r+2*p.x()*p.x()) ; double ty = k[2]*(r+2*p.y()*p.y()) + 2*k[3]*p.x()*p.y() ; - Vector v_hat = (Vector(3) << g*p.x() + tx, g*p.y() + ty, 1.0) ; + Vector v_hat = (Vector(3) << g*p.x() + tx, g*p.y() + ty, 1.0).finished(); Vector v_i = K.K() * v_hat ; Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ; Point2 q = K.uncalibrate(p); diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index de5ca1ed1..5aeee03d4 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -31,7 +31,7 @@ static const Pose3 pose1((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. - ), + ).finished(), Point3(0,0,0.5)); static const CalibratedCamera camera(pose1); diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 9d6f798cd..d2990a747 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -62,16 +62,16 @@ TEST (EssentialMatrix, retract0) { //************************************************************************* TEST (EssentialMatrix, retract1) { - EssentialMatrix expected(c1Rc2.retract((Vector(3) << 0.1, 0, 0)), Unit3(c1Tc2)); - EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0)); + EssentialMatrix expected(c1Rc2.retract(Vector3(0.1, 0, 0)), Unit3(c1Tc2)); + EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0).finished()); EXPECT(assert_equal(expected, actual)); } //************************************************************************* TEST (EssentialMatrix, retract2) { EssentialMatrix expected(c1Rc2, - Unit3(c1Tc2).retract((Vector(2) << 0.1, 0))); - EssentialMatrix actual = trueE.retract((Vector(5) << 0, 0, 0, 0.1, 0)); + Unit3(c1Tc2).retract(Vector2(0.1, 0))); + EssentialMatrix actual = trueE.retract((Vector(5) << 0, 0, 0, 0.1, 0).finished()); EXPECT(assert_equal(expected, actual)); } @@ -85,7 +85,7 @@ TEST (EssentialMatrix, transform_to) { * Rot3::roll(M_PI / 6.0); Point3 aTb2(19.2, 3.7, 5.9); EssentialMatrix E(aRb2, Unit3(aTb2)); - //EssentialMatrix E(aRb, Unit3(aTb).retract((Vector(2) << 0.1, 0))); + //EssentialMatrix E(aRb, Unit3(aTb).retract(Vector2(0.1, 0))); static Point3 P(0.2, 0.7, -2); Matrix actH1, actH2; E.transform_to(P, actH1, actH2); diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 101070940..ddeae2b7d 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -33,7 +33,7 @@ static const Pose3 pose1((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. - ), + ).finished(), Point3(0,0,0.5)); typedef PinholeCamera Camera; diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 215f376f6..ab40928ab 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((Vector(2) << 4., 5.)))); - EXPECT(assert_equal((Vector(2) << 3.,3.), p1.localCoordinates(p2))); + EXPECT(assert_equal(Point2(5,7), p1.retract(Vector2(4., 5.)))); + EXPECT(assert_equal(Vector2(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 = (Matrix(1, 2) << 1.0, 1.0); + expectedH = (Matrix(1, 2) << 1.0, 1.0).finished(); EXPECT(assert_equal(expectedH,actualH)); actual = x2.norm(actualH); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index a46031209..18273b182 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -40,8 +40,8 @@ TEST(Point3, Lie) { EXPECT(assert_equal(-eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(5, 7, 9), p1.retract((Vector(3) << 4., 5., 6.)))); - EXPECT(assert_equal((Vector)(Vector(3) << 3.,3.,3.), p1.localCoordinates(p2))); + EXPECT(assert_equal(Point3(5, 7, 9), p1.retract(Vector3(4., 5., 6.)))); + EXPECT(assert_equal((Vector)Vector3(3.,3.,3.), p1.localCoordinates(p2))); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 12266c16f..b4de6eb7c 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((Vector(3) << 0.01, -0.015, 0.99)); + Pose2 actual = pose.retract(Vector3(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, (Vector(3) << 0.01, -0.015, 0.99)); + Pose2 actual = expmap_default(pose, Vector3(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, (Vector(3) << 0.01, -0.015, 0.99)); + Pose2 actual = expmap_default(pose, Vector3(0.01, -0.015, 0.99)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -95,11 +95,11 @@ TEST(Pose2, expmap3) { Matrix A = (Matrix(3,3) << 0.0, -0.99, 0.01, 0.99, 0.0, -0.015, - 0.0, 0.0, 0.0); + 0.0, 0.0, 0.0).finished(); 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 = (Vector(3) << 0.01, -0.015, 0.99); + Vector v = Vector3(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((Vector(3) << 0.01, -0.015, 0.018)); + Pose2 actual = Pose2::Expmap(Vector3(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((Vector(3) << M_PI/2, 0.0, M_PI/2)); + Pose2 actual = Pose2::Expmap((Vector(3) << M_PI/2, 0.0, M_PI/2).finished()); 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((Vector(3) << M_PI, 0.0, M_PI)); + Pose2 actual = Pose2::Expmap((Vector(3) << M_PI, 0.0, M_PI).finished()); 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((Vector(3) << 2*M_PI, 0.0, 2*M_PI)); + Pose2 actual = Pose2::Expmap((Vector(3) << 2*M_PI, 0.0, 2*M_PI).finished()); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -161,7 +161,7 @@ TEST(Pose3, expmap_c) TEST(Pose2, expmap_c_full) { double w=0.3; - Vector xi = (Vector(3) << 0.0, w, w); + Vector xi = (Vector(3) << 0.0, w, w).finished(); 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 = (Vector(3) << 0.00986473, -0.0150896, 0.018); + Vector expected = Vector3(0.00986473, -0.0150896, 0.018); #else - Vector expected = (Vector(3) << 0.01, -0.015, 0.018); + Vector expected = Vector3(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 = (Vector(3) << 0.00986473, -0.0150896, 0.018); + Vector expected = Vector3(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 = (Matrix(2,3) << -1.0, 0.0, 2.0, 0.0, -1.0, -2.0); - Matrix expectedH2 = (Matrix(2,2) << 0.0, 1.0, -1.0, 0.0); + Matrix expectedH1 = (Matrix(2,3) << -1.0, 0.0, 2.0, 0.0, -1.0, -2.0).finished(); + Matrix expectedH2 = (Matrix(2,2) << 0.0, 1.0, -1.0, 0.0).finished(); // actual Matrix actualH1, actualH2; @@ -236,8 +236,8 @@ TEST (Pose2, transform_from) Point2 expected(0., 2.); EXPECT(assert_equal(expected, actual)); - Matrix H1_expected = (Matrix(2, 3) << 0., -1., -2., 1., 0., -1.); - Matrix H2_expected = (Matrix(2, 2) << 0., -1., 1., 0.); + Matrix H1_expected = (Matrix(2, 3) << 0., -1., -2., 1., 0., -1.).finished(); + Matrix H2_expected = (Matrix(2, 2) << 0., -1., 1., 0.).finished(); Matrix numericalH1 = numericalDerivative21(transform_from_proxy, pose, pt); EXPECT(assert_equal(H1_expected, H1)); @@ -265,7 +265,7 @@ TEST(Pose2, compose_a) 0.0, 1.0, 0.0, -1.0, 0.0, 2.0, 0.0, 0.0, 1.0 - ); + ).finished(); Matrix expectedH2 = eye(3); Matrix numericalH1 = numericalDerivative21(testing::compose, pose1, pose2); Matrix numericalH2 = numericalDerivative22(testing::compose, pose1, pose2); @@ -348,7 +348,7 @@ TEST(Pose2, inverse ) namespace { /* ************************************************************************* */ Vector homogeneous(const Point2& p) { - return (Vector(3) << p.x(), p.y(), 1.0); + return (Vector(3) << p.x(), p.y(), 1.0).finished(); } /* ************************************************************************* */ @@ -358,7 +358,7 @@ namespace { 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); + 0.0, 0.0, 1.0).finished(); } } @@ -371,14 +371,14 @@ TEST( Pose2, matrix ) EXPECT(assert_equal((Matrix(3,3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, - 0.0, 0.0, 1.0), + 0.0, 0.0, 1.0).finished(), gMl)); Rot2 gR1 = gTl.r(); EXPECT(assert_equal(homogeneous(t),gMl*homogeneous(origin))); Point2 x_axis(1,0), y_axis(0,1); EXPECT(assert_equal((Matrix(2,2) << 0.0, -1.0, - 1.0, 0.0), + 1.0, 0.0).finished(), gR1.matrix())); EXPECT(assert_equal(Point2(0,1),gR1*x_axis)); EXPECT(assert_equal(Point2(-1,0),gR1*y_axis)); @@ -390,7 +390,7 @@ TEST( Pose2, matrix ) EXPECT(assert_equal((Matrix(3,3) << 0.0, 1.0,-2.0, -1.0, 0.0, 1.0, - 0.0, 0.0, 1.0), + 0.0, 0.0, 1.0).finished(), lMg)); } @@ -425,7 +425,7 @@ TEST( Pose2, between ) 0.0,-1.0,-2.0, 1.0, 0.0,-2.0, 0.0, 0.0,-1.0 - ); + ).finished(); Matrix numericalH1 = numericalDerivative21(testing::between, gT1, gT2); EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(numericalH1,actualH1)); @@ -436,7 +436,7 @@ TEST( Pose2, between ) 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 - ); + ).finished(); Matrix numericalH2 = numericalDerivative22(testing::between, gT1, gT2); EXPECT(assert_equal(expectedH2,actualH2)); EXPECT(assert_equal(numericalH2,actualH2)); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 2a775379d..52f721f41 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((Vector(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).finished()); 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 = (Vector(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).finished(); 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 = (Vector(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).finished(); 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 = (Vector(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).finished(); 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 = (Vector(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).finished(); 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 = (Vector(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).finished(); 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 = (Vector(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).finished(); // 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) @@ -674,7 +674,7 @@ TEST(Pose3, align_2) { /* ************************************************************************* */ /// exp(xi) exp(y) = exp(xi + x) /// Hence, y = log (exp(-xi)*exp(xi+x)) -Vector xi = (Vector(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).finished(); Vector testDerivExpmapInv(const Vector6& dxi) { return Pose3::Logmap(Pose3::Expmap(-xi) * Pose3::Expmap(xi + dxi)); @@ -713,8 +713,8 @@ Vector testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) { } TEST( Pose3, adjointTranspose) { - 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 xi = (Vector(6) << 0.01, 0.02, 0.03, 1.0, 2.0, 3.0).finished(); + Vector v = (Vector(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0).finished(); Vector expected = testDerivAdjointTranspose(xi, v); Matrix actualH; diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index c67031a13..8a1f942d2 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -96,7 +96,7 @@ TEST(Rot2, logmap) { Rot2 rot0(Rot2::fromAngle(M_PI/2.0)); Rot2 rot(Rot2::fromAngle(M_PI)); - Vector expected = (Vector(1) << M_PI/2.0); + Vector expected = (Vector(1) << M_PI/2.0).finished(); Vector actual = rot0.localCoordinates(rot); CHECK(assert_equal(expected, actual)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 63dc75876..358524488 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -50,7 +50,7 @@ TEST( Rot3, constructor) /* ************************************************************************* */ TEST( Rot3, constructor2) { - Matrix R = (Matrix(3, 3) << 0, 1, 0, 1, 0, 0, 0, 0, -1); + Matrix R = (Matrix(3, 3) << 0, 1, 0, 1, 0, 0, 0, 0, -1).finished(); Rot3 actual(R); Rot3 expected(0, 1, 0, 1, 0, 0, 0, 0, -1); CHECK(assert_equal(actual,expected)); @@ -94,7 +94,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) { TEST( Rot3, rodriguez) { Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0); - Vector w = (Vector(3) << epsilon, 0., 0.); + Vector w = (Vector(3) << epsilon, 0., 0.).finished(); Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); } @@ -102,7 +102,7 @@ TEST( Rot3, rodriguez) /* ************************************************************************* */ TEST( Rot3, rodriguez2) { - Vector axis = (Vector(3) << 0., 1., 0.); // rotation around Y + Vector axis = Vector3(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, @@ -114,7 +114,7 @@ TEST( Rot3, rodriguez2) /* ************************************************************************* */ TEST( Rot3, rodriguez3) { - Vector w = (Vector(3) << 0.1, 0.2, 0.3); + Vector w = Vector3(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)); @@ -123,7 +123,7 @@ TEST( Rot3, rodriguez3) /* ************************************************************************* */ TEST( Rot3, rodriguez4) { - Vector axis = (Vector(3) << 0., 0., 1.); // rotation around Z + Vector axis = Vector3(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); @@ -154,7 +154,7 @@ TEST(Rot3, log) Rot3 R; #define CHECK_OMEGA(X,Y,Z) \ - w = (Vector(3) << (double)X, (double)Y, double(Z)); \ + w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \ R = Rot3::rodriguez(w); \ EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12)); @@ -196,7 +196,7 @@ TEST(Rot3, log) // Check 360 degree rotations #define CHECK_OMEGA_ZERO(X,Y,Z) \ - w = (Vector(3) << (double)X, (double)Y, double(Z)); \ + w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \ R = Rot3::rodriguez(w); \ EXPECT(assert_equal(zero(3), Rot3::Logmap(R))); @@ -256,7 +256,7 @@ TEST(Rot3, manifold_expmap) CHECK(assert_equal(d12,-d21)); // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = (Vector(3) << 0.1, 0.2, 0.3); + Vector d = Vector3(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) @@ -369,7 +369,7 @@ TEST( Rot3, between ) 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); + 0.0, 0.0, 1.0).finished(); EXPECT(assert_equal(expectedr1, r1.matrix())); Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); @@ -393,7 +393,7 @@ TEST( Rot3, between ) } /* ************************************************************************* */ -Vector w = (Vector(3) << 0.1, 0.27, -0.2); +Vector w = Vector3(0.1, 0.27, -0.2); // Left trivialization Derivative of exp(w) wrpt w: // How does exp(w) change when w changes? @@ -462,7 +462,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)(Vector(3) << 0.1, 0.2, 0.3),expected.ypr())); + CHECK(assert_equal((Vector)Vector3(0.1, 0.2, 0.3),expected.ypr())); } /* ************************************************************************* */ @@ -472,25 +472,25 @@ TEST( Rot3, RQ) Matrix actualK; Vector actual; boost::tie(actualK, actual) = RQ(R.matrix()); - Vector expected = (Vector(3) << 0.14715, 0.385821, 0.231671); + Vector expected = Vector3(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)(Vector(3) << 0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); + CHECK(assert_equal((Vector)Vector3(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)(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())); + CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); + CHECK(assert_equal((Vector)Vector3(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)(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())); + CHECK(assert_equal((Vector)Vector3(0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); + CHECK(assert_equal((Vector)Vector3(0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); + CHECK(assert_equal((Vector)Vector3(0.0,0.0,0.1),Rot3::roll (0.1).ypr())); // Try RQ to recover calibration from 3*3 sub-block of projection matrix - Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0); + Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0).finished(); Matrix A = K * R.matrix(); boost::tie(actualK, actual) = RQ(A); CHECK(assert_equal(K,actualK)); @@ -499,13 +499,13 @@ TEST( Rot3, RQ) /* ************************************************************************* */ TEST( Rot3, expmapStability ) { - Vector w = (Vector(3) << 78e-9, 5e-8, 97e-7); + Vector w = Vector3(78e-9, 5e-8, 97e-7); double theta = w.norm(); double theta2 = theta*theta; Rot3 actualR = Rot3::Expmap(w); Matrix W = (Matrix(3, 3) << 0.0, -w(2), w(1), w(2), 0.0, -w(0), - -w(1), w(0), 0.0 ); + -w(1), w(0), 0.0 ).finished(); Matrix W2 = W*W; Matrix Rmat = I3 + (1.0-theta2/6.0 + theta2*theta2/120.0 - theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ; @@ -515,7 +515,7 @@ TEST( Rot3, expmapStability ) { /* ************************************************************************* */ TEST( Rot3, logmapStability ) { - Vector w = (Vector(3) << 1e-8, 0.0, 0.0); + Vector w = Vector3(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); @@ -533,13 +533,13 @@ TEST(Rot3, quaternion) { 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)); + -0.769319620053772, 0.637998195662053, 0.033250932803219).finished()); Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053); 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)); + 0.424630407073532, 0.893945571198514, -0.143353873763946).finished()); // Check creating Rot3 from quaternion EXPECT(assert_equal(R1, Rot3(q1))); diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 876b3845f..1313a3be5 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -56,7 +56,7 @@ TEST(Rot3, manifold_cayley) CHECK(assert_equal(d12,-d21)); // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = (Vector(3) << 0.1, 0.2, 0.3); + Vector d = Vector3(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) @@ -84,7 +84,7 @@ TEST(Rot3, manifold_slow_cayley) CHECK(assert_equal(d12,-d21)); // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = (Vector(3) << 0.1, 0.2, 0.3); + Vector d = Vector3(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) diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 366d09d49..002cbe51b 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -32,7 +32,7 @@ static const Pose3 pose1((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. - ), + ).finished(), Point3(0,0,0.5)); static const SimpleCamera camera(pose1, K); @@ -147,7 +147,7 @@ TEST( SimpleCamera, simpleCamera) 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); + 7.07107e-1, -3.53553e-1,6.12372e-1, -9.18559e2).finished(); SimpleCamera actual = simpleCamera(P); // Note precision of numbers given in book CHECK(assert_equal(expected, actual,1e-1)); diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index a7a82c9de..eebe2d60a 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -78,7 +78,7 @@ static Pose3 camera1((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. - ), + ).finished(), Point3(0,0,6.25)); static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); diff --git a/gtsam/geometry/tests/testStereoPoint2.cpp b/gtsam/geometry/tests/testStereoPoint2.cpp index 598febcc8..8e504ba0e 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((Vector(3) << 4., 5., 6.)))); - EXPECT(assert_equal((Vector(3) << 3., 3., 3.), p1.localCoordinates(p2))); + EXPECT(assert_equal(StereoPoint2(5,7,9), p1.retract(Vector3(4., 5., 6.)))); + EXPECT(assert_equal(Vector3(3., 3., 3.), p1.localCoordinates(p2))); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 53ae2fc58..0102477b3 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -106,11 +106,11 @@ TEST(Unit3, unrotate) { //******************************************************************************* TEST(Unit3, error) { - Unit3 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // - r = p.retract((Vector(2) << 0.8, 0)); - EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8)); - EXPECT(assert_equal((Vector(2) << 0.479426, 0), p.error(q), 1e-5)); - EXPECT(assert_equal((Vector(2) << 0.717356, 0), p.error(r), 1e-5)); + Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // + r = p.retract(Vector2(0.8, 0)); + EXPECT(assert_equal(Vector2(0, 0), p.error(p), 1e-8)); + EXPECT(assert_equal(Vector2(0.479426, 0), p.error(q), 1e-5)); + EXPECT(assert_equal(Vector2(0.717356, 0), p.error(r), 1e-5)); Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian @@ -130,8 +130,8 @@ TEST(Unit3, error) { //******************************************************************************* TEST(Unit3, distance) { - Unit3 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // - r = p.retract((Vector(2) << 0.8, 0)); + Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // + r = p.retract(Vector2(0.8, 0)); EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8); EXPECT_DOUBLES_EQUAL(0.47942553860420301, p.distance(q), 1e-8); EXPECT_DOUBLES_EQUAL(0.71735609089952279, p.distance(r), 1e-8); @@ -169,7 +169,7 @@ TEST(Unit3, localCoordinates1) { //******************************************************************************* TEST(Unit3, localCoordinates2) { Unit3 p, q(-1, 0, 0); - Vector expected = (Vector(2) << M_PI, 0); + Vector expected = (Vector(2) << M_PI, 0).finished(); Vector actual = p.localCoordinates(q); CHECK(assert_equal(expected, actual, 1e-8)); } @@ -228,9 +228,9 @@ inline static Vector randomVector(const Vector& minLimits, TEST(Unit3, localCoordinates_retract) { size_t numIterations = 10000; - Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit = - (Vector(3) << 1.0, 1.0, 1.0); - Vector minXiLimit = (Vector(2) << -1.0, -1.0), maxXiLimit = (Vector(2) << 1.0, 1.0); + Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit = + Vector3(1.0, 1.0, 1.0); + Vector minXiLimit = Vector2(-1.0, -1.0), maxXiLimit = Vector2(1.0, 1.0); for (size_t i = 0; i < numIterations; i++) { // Sleep for the random number generator (TODO?: Better create all of them first). @@ -258,9 +258,9 @@ TEST(Unit3, localCoordinates_retract) { TEST(Unit3, localCoordinates_retract_expmap) { size_t numIterations = 10000; - Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit = - (Vector(3) << 1.0, 1.0, 1.0); - Vector minXiLimit = (Vector(2) << -M_PI, -M_PI), maxXiLimit = (Vector(2) << M_PI, M_PI); + Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit = + Vector3(1.0, 1.0, 1.0); + Vector minXiLimit = (Vector(2) << -M_PI, -M_PI).finished(), maxXiLimit = (Vector(2) << M_PI, M_PI).finished(); for (size_t i = 0; i < numIterations; i++) { // Sleep for the random number generator (TODO?: Better create all of them first). diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 502d9314b..290249b68 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -49,15 +49,15 @@ namespace gtsam { * Example: * \code VectorValues values; - values.insert(3, (Vector(3) << 1.0, 2.0, 3.0)); - values.insert(4, (Vector(2) << 4.0, 5.0)); - values.insert(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0)); + values.insert(3, Vector3(1.0, 2.0, 3.0)); + values.insert(4, Vector2(4.0, 5.0)); + values.insert(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0).finished()); // Prints [ 3.0 4.0 ] gtsam::print(values[1]); // Prints [ 8.0 9.0 ] - values[1] = (Vector(2) << 8.0, 9.0); + values[1] = Vector2(8.0, 9.0); gtsam::print(values[1]); \endcode * diff --git a/gtsam/linear/tests/testErrors.cpp b/gtsam/linear/tests/testErrors.cpp index c78382316..9c8eb468d 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 += (Vector(2) << 1.0,2.0), (Vector(3) << 3.0,4.0,5.0); + e += Vector2(1.0,2.0), Vector3(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 += (Vector(2) << 3.0,6.0), (Vector(3) << 9.0,12.0,15.0); + expected += Vector2(3.0,6.0), Vector3(9.0,12.0,15.0); CHECK(assert_equal(expected,e)); } diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index cd5231e49..fb3884542 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -38,8 +38,8 @@ using namespace gtsam; static const Key _x_=0, _y_=1; static GaussianBayesNet smallBayesNet = list_of - (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))); + (GaussianConditional(_x_, (Vector(1) << 9.0).finished(), (Matrix(1, 1) << 1.0).finished(), _y_, (Matrix(1, 1) << 1.0).finished())) + (GaussianConditional(_y_, (Vector(1) << 5.0).finished(), (Matrix(1, 1) << 1.0).finished())); /* ************************************************************************* */ TEST( GaussianBayesNet, matrix ) @@ -50,8 +50,8 @@ TEST( GaussianBayesNet, matrix ) Matrix R1 = (Matrix(2, 2) << 1.0, 1.0, 0.0, 1.0 - ); - Vector d1 = (Vector(2) << 9.0, 5.0); + ).finished(); + Vector d1 = Vector2(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_, (Vector(1) << 4.0)) - (_y_, (Vector(1) << 5.0)); + (_x_, (Vector(1) << 4.0).finished()) + (_y_, (Vector(1) << 5.0).finished()); EXPECT(assert_equal(expected,actual)); } @@ -73,16 +73,16 @@ TEST( GaussianBayesNet, optimize ) TEST( GaussianBayesNet, optimizeIncomplete ) { static GaussianBayesNet incompleteBayesNet = list_of - (GaussianConditional(_x_, (Vector(1) << 9.0), (Matrix(1, 1) << 1.0), _y_, (Matrix(1, 1) << 1.0))); + (GaussianConditional(_x_, (Vector(1) << 9.0).finished(), (Matrix(1, 1) << 1.0).finished(), _y_, (Matrix(1, 1) << 1.0).finished())); VectorValues solutionForMissing = map_list_of - (_y_, (Vector(1) << 5.0)); + (_y_, (Vector(1) << 5.0).finished()); VectorValues actual = incompleteBayesNet.optimize(solutionForMissing); VectorValues expected = map_list_of - (_x_, (Vector(1) << 4.0)) - (_y_, (Vector(1) << 5.0)); + (_x_, (Vector(1) << 4.0).finished()) + (_y_, (Vector(1) << 5.0).finished()); EXPECT(assert_equal(expected,actual)); } @@ -96,13 +96,13 @@ TEST( GaussianBayesNet, optimize3 ) // NOTE: we are supplying a new RHS here VectorValues expected = map_list_of - (_x_, (Vector(1) << -1.0)) - (_y_, (Vector(1) << 5.0)); + (_x_, (Vector(1) << -1.0).finished()) + (_y_, (Vector(1) << 5.0).finished()); // Test different RHS version VectorValues gx = map_list_of - (_x_, (Vector(1) << 4.0)) - (_y_, (Vector(1) << 5.0)); + (_x_, (Vector(1) << 4.0).finished()) + (_y_, (Vector(1) << 5.0).finished()); VectorValues actual = smallBayesNet.backSubstitute(gx); EXPECT(assert_equal(expected, actual)); } @@ -115,11 +115,11 @@ TEST( GaussianBayesNet, backSubstituteTranspose ) // 5 1 1 3 VectorValues x = map_list_of - (_x_, (Vector(1) << 2.0)) - (_y_, (Vector(1) << 5.0)), + (_x_, (Vector(1) << 2.0).finished()) + (_y_, (Vector(1) << 5.0).finished()), expected = map_list_of - (_x_, (Vector(1) << 2.0)) - (_y_, (Vector(1) << 3.0)); + (_x_, (Vector(1) << 2.0).finished()) + (_y_, (Vector(1) << 3.0).finished()); VectorValues actual = smallBayesNet.backSubstituteTranspose(x); EXPECT(assert_equal(expected, actual)); @@ -131,15 +131,15 @@ TEST( GaussianBayesNet, DeterminantTest ) { GaussianBayesNet cbn; cbn += GaussianConditional( - 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)); + 0, Vector2(3.0, 4.0 ), (Matrix(2, 2) << 1.0, 3.0, 0.0, 4.0 ).finished(), + 1, (Matrix(2, 2) << 2.0, 1.0, 2.0, 3.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); cbn += GaussianConditional( - 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)); + 1, Vector2(5.0, 6.0 ), (Matrix(2, 2) << 1.0, 1.0, 0.0, 3.0 ).finished(), + 2, (Matrix(2, 2) << 1.0, 0.0, 5.0, 2.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); cbn += GaussianConditional( - 3, (Vector(2) << 7.0, 8.0 ), (Matrix(2, 2) << 1.0, 1.0, 0.0, 5.0 ), noiseModel::Isotropic::Sigma(2, 2.0)); + 3, Vector2(7.0, 8.0 ), (Matrix(2, 2) << 1.0, 1.0, 0.0, 5.0 ).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); double expectedDeterminant = 60.0 / 64.0; double actualDeterminant = cbn.determinant(); @@ -163,21 +163,21 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 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))); + 0, Vector2(1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(), + 3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0).finished(), + 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 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))); + 1, Vector2(15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(), + 2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0).finished(), + 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 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))); + 2, Vector2(29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(), + 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 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))); + 3, Vector2(39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(), + 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0))); + 4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished())); // Compute the Hessian numerically Matrix hessian = numericalHessian( diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 217fab543..78fe5187a 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -37,10 +37,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, (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)); + (JacobianFactor(x2, (Matrix(1, 1) << 1.).finished(), x1, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)) + (JacobianFactor(x2, (Matrix(1, 1) << 1.).finished(), x3, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)) + (JacobianFactor(x3, (Matrix(1, 1) << 1.).finished(), x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)) + (JacobianFactor(x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)); const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4)); /* ************************************************************************* */ @@ -83,13 +83,13 @@ TEST( GaussianBayesTree, eliminate ) { GaussianBayesTree bt = *chain.eliminateMultifrontal(chainOrdering); - Matrix two = (Matrix(1, 1) << 2.); - Matrix one = (Matrix(1, 1) << 1.); + Matrix two = (Matrix(1, 1) << 2.).finished(); + Matrix one = (Matrix(1, 1) << 1.).finished(); GaussianBayesTree bayesTree_expected; bayesTree_expected.insertRoot( - 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.)))))); + MakeClique(GaussianConditional(pair_list_of(x3, (Matrix(2, 1) << 2., 0.).finished()) (x4, (Matrix(2, 1) << 2., 2.).finished()), 2, Vector2(2., 2.)), list_of + (MakeClique(GaussianConditional(pair_list_of(x2, (Matrix(2, 1) << -2.*sqrt(2.), 0.).finished()) (x1, (Matrix(2, 1) << -sqrt(2.), -sqrt(2.)).finished()) (x3, (Matrix(2, 1) << -sqrt(2.), sqrt(2.)).finished()), 2, (Vector(2) << -2.*sqrt(2.), 0.).finished()))))); EXPECT(assert_equal(bayesTree_expected, bt)); } @@ -98,10 +98,10 @@ TEST( GaussianBayesTree, eliminate ) TEST( GaussianBayesTree, optimizeMultiFrontal ) { VectorValues expected = pair_list_of - (x1, (Vector(1) << 0.)) - (x2, (Vector(1) << 1.)) - (x3, (Vector(1) << 0.)) - (x4, (Vector(1) << 1.)); + (x1, (Vector(1) << 0.).finished()) + (x2, (Vector(1) << 1.).finished()) + (x3, (Vector(1) << 0.).finished()) + (x4, (Vector(1) << 1.).finished()); VectorValues actual = chain.eliminateMultifrontal(chainOrdering)->optimize(); EXPECT(assert_equal(expected,actual)); @@ -113,39 +113,39 @@ TEST(GaussianBayesTree, complicatedMarginal) { // Create the conditionals to go in the BayesTree GaussianBayesTree bt; bt.insertRoot( - MakeClique(GaussianConditional(pair_list_of (11, (Matrix(3,1) << 0.0971, 0, 0)) - (12, (Matrix(3,2) << 0.3171, 0.4387, 0.9502, 0.3816, 0, 0.7655)), - 2, (Vector(3) << 0.2638, 0.1455, 0.1361)), list_of - (MakeClique(GaussianConditional(pair_list_of (9, (Matrix(3,1) << 0.7952, 0, 0)) - (10, (Matrix(3,2) << 0.4456, 0.7547, 0.6463, 0.2760, 0, 0.6797)) - (11, (Matrix(3,1) << 0.6551, 0.1626, 0.1190)) - (12, (Matrix(3,2) << 0.4984, 0.5853, 0.9597, 0.2238, 0.3404, 0.7513)), - 2, (Vector(3) << 0.4314, 0.9106, 0.1818)))) - (MakeClique(GaussianConditional(pair_list_of (7, (Matrix(3,1) << 0.2551, 0, 0)) - (8, (Matrix(3,2) << 0.8909, 0.1386, 0.9593, 0.1493, 0, 0.2575)) - (11, (Matrix(3,1) << 0.8407, 0.2543, 0.8143)), - 2, (Vector(3) << 0.3998, 0.2599, 0.8001)), list_of - (MakeClique(GaussianConditional(pair_list_of (5, (Matrix(3,1) << 0.2435, 0, 0)) - (6, (Matrix(3,2) << 0.4733, 0.1966, 0.3517, 0.2511, 0.8308, 0.0)) + MakeClique(GaussianConditional(pair_list_of (11, (Matrix(3,1) << 0.0971, 0, 0).finished()) + (12, (Matrix(3,2) << 0.3171, 0.4387, 0.9502, 0.3816, 0, 0.7655).finished()), + 2, Vector3(0.2638, 0.1455, 0.1361)), list_of + (MakeClique(GaussianConditional(pair_list_of (9, (Matrix(3,1) << 0.7952, 0, 0).finished()) + (10, (Matrix(3,2) << 0.4456, 0.7547, 0.6463, 0.2760, 0, 0.6797).finished()) + (11, (Matrix(3,1) << 0.6551, 0.1626, 0.1190).finished()) + (12, (Matrix(3,2) << 0.4984, 0.5853, 0.9597, 0.2238, 0.3404, 0.7513).finished()), + 2, Vector3(0.4314, 0.9106, 0.1818)))) + (MakeClique(GaussianConditional(pair_list_of (7, (Matrix(3,1) << 0.2551, 0, 0).finished()) + (8, (Matrix(3,2) << 0.8909, 0.1386, 0.9593, 0.1493, 0, 0.2575).finished()) + (11, (Matrix(3,1) << 0.8407, 0.2543, 0.8143).finished()), + 2, Vector3(0.3998, 0.2599, 0.8001)), list_of + (MakeClique(GaussianConditional(pair_list_of (5, (Matrix(3,1) << 0.2435, 0, 0).finished()) + (6, (Matrix(3,2) << 0.4733, 0.1966, 0.3517, 0.2511, 0.8308, 0.0).finished()) // NOTE the non-upper-triangular form // here since this test was written when we had column permutations // from LDL. The code still works currently (does not enfore // upper-triangularity in this case) but this test will need to be // redone if this stops working in the future - (7, (Matrix(3,1) << 0.5853, 0.5497, 0.9172)) - (8, (Matrix(3,2) << 0.2858, 0.3804, 0.7572, 0.5678, 0.7537, 0.0759)), - 2, (Vector(3) << 0.8173, 0.8687, 0.0844)), list_of - (MakeClique(GaussianConditional(pair_list_of (3, (Matrix(3,1) << 0.0540, 0, 0)) - (4, (Matrix(3,2) << 0.9340, 0.4694, 0.1299, 0.0119, 0, 0.3371)) - (6, (Matrix(3,2) << 0.1622, 0.5285, 0.7943, 0.1656, 0.3112, 0.6020)), - 2, (Vector(3) << 0.9619, 0.0046, 0.7749)))) - (MakeClique(GaussianConditional(pair_list_of (1, (Matrix(3,1) << 0.2630, 0, 0)) - (2, (Matrix(3,2) << 0.7482, 0.2290, 0.4505, 0.9133, 0, 0.1524)) - (5, (Matrix(3,1) << 0.8258, 0.5383, 0.9961)), - 2, (Vector(3) << 0.0782, 0.4427, 0.1067)))))))))); + (7, (Matrix(3,1) << 0.5853, 0.5497, 0.9172).finished()) + (8, (Matrix(3,2) << 0.2858, 0.3804, 0.7572, 0.5678, 0.7537, 0.0759).finished()), + 2, Vector3(0.8173, 0.8687, 0.0844)), list_of + (MakeClique(GaussianConditional(pair_list_of (3, (Matrix(3,1) << 0.0540, 0, 0).finished()) + (4, (Matrix(3,2) << 0.9340, 0.4694, 0.1299, 0.0119, 0, 0.3371).finished()) + (6, (Matrix(3,2) << 0.1622, 0.5285, 0.7943, 0.1656, 0.3112, 0.6020).finished()), + 2, Vector3(0.9619, 0.0046, 0.7749)))) + (MakeClique(GaussianConditional(pair_list_of (1, (Matrix(3,1) << 0.2630, 0, 0).finished()) + (2, (Matrix(3,2) << 0.7482, 0.2290, 0.4505, 0.9133, 0, 0.1524).finished()) + (5, (Matrix(3,1) << 0.8258, 0.5383, 0.9961).finished()), + 2, Vector3(0.0782, 0.4427, 0.1067)))))))))); // Marginal on 5 - Matrix expectedCov = (Matrix(1,1) << 236.5166); + Matrix expectedCov = (Matrix(1,1) << 236.5166).finished(); //GaussianConditional actualJacobianChol = *bt.marginalFactor(5, EliminateCholesky); GaussianConditional actualJacobianQR = *bt.marginalFactor(5, EliminateQR); //EXPECT(assert_equal(actualJacobianChol, actualJacobianQR)); // Check that Chol and QR obtained marginals are the same @@ -162,7 +162,7 @@ TEST(GaussianBayesTree, complicatedMarginal) { // 2886.2, 1015.8); expectedCov = (Matrix(2,2) << 1015.8, 2886.2, - 2886.2, 8471.2); + 2886.2, 8471.2).finished(); //actualJacobianChol = bt.marginalFactor(6, EliminateCholesky); actualJacobianQR = *bt.marginalFactor(6, EliminateQR); //EXPECT(assert_equal(actualJacobianChol, actualJacobianQR)); // Check that Chol and QR obtained marginals are the same @@ -196,50 +196,50 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { 0.0,0.0, 0.0,0.0, 0.0,0.0, - 0.0,0.0)) + 0.0,0.0).finished()) (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)) + 0.0,0.0).finished()) (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, (Vector(6) << 29.0,30.0,39.0,40.0,49.0,50.0)), list_of + 0.0,54.0).finished()), + 3, (Vector(6) << 29.0,30.0,39.0,40.0,49.0,50.0).finished()), list_of (MakeClique(GaussianConditional( pair_list_of (0, (Matrix(4, 2) << 3.0,4.0, 0.0,6.0, 0.0,0.0, - 0.0,0.0)) + 0.0,0.0).finished()) (1, (Matrix(4, 2) << 0.0,0.0, 0.0,0.0, 17.0,18.0, - 0.0,20.0)) + 0.0,20.0).finished()) (2, (Matrix(4, 2) << 0.0,0.0, 0.0,0.0, 21.0,22.0, - 23.0,24.0)) + 23.0,24.0).finished()) (3, (Matrix(4, 2) << 7.0,8.0, 9.0,10.0, 0.0,0.0, - 0.0,0.0)) + 0.0,0.0).finished()) (4, (Matrix(4, 2) << 11.0,12.0, 13.0,14.0, 25.0,26.0, - 27.0,28.0)), - 2, (Vector(4) << 1.0,2.0,15.0,16.0)))))); + 27.0,28.0).finished()), + 2, (Vector(4) << 1.0,2.0,15.0,16.0).finished()))))); // Compute the Hessian numerically Matrix hessian = numericalHessian( @@ -261,11 +261,11 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { // Known steepest descent point from Bayes' net version VectorValues expectedFromBN = pair_list_of - (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)); + (0, Vector2(0.000129034, 0.000688183)) + (1, Vector2(0.0109679, 0.0253767)) + (2, Vector2(0.0680441, 0.114496)) + (3, Vector2(0.16125, 0.241294)) + (4, Vector2(0.300134, 0.423233)); // Compute the steepest descent point with the dogleg function VectorValues actual = bt.optimizeGradientSearch(); diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index d7f0c2dd5..60387694e 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -43,23 +43,23 @@ static const double tol = 1e-5; static Matrix R = (Matrix(2, 2) << -12.1244, -5.1962, - 0., 4.6904); + 0., 4.6904).finished(); /* ************************************************************************* */ TEST(GaussianConditional, constructor) { Matrix S1 = (Matrix(2, 2) << -5.2786, -8.6603, - 5.0254, 5.5432); + 5.0254, 5.5432).finished(); Matrix S2 = (Matrix(2, 2) << -10.5573, -5.9385, - 5.5737, 3.0153); + 5.5737, 3.0153).finished(); Matrix S3 = (Matrix(2, 2) << -11.3820, -7.2581, - -3.0153, -3.5635); + -3.0153, -3.5635).finished(); - Vector d = (Vector(2) << 1.0, 2.0); - SharedDiagonal s = noiseModel::Diagonal::Sigmas((Vector(2) << 3.0, 4.0)); + Vector d = Vector2(1.0, 2.0); + SharedDiagonal s = noiseModel::Diagonal::Sigmas(Vector2(3.0, 4.0)); vector > terms = pair_list_of (1, R) @@ -116,9 +116,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((Vector(2) << 1.0, 0.34)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(1.0, 0.34)); - Vector d = (Vector(2) << 0.2, 0.5); + Vector d = Vector2(0.2, 0.5); GaussianConditional expected(1, d, R, 2, A1, 10, A2, model), @@ -136,13 +136,13 @@ TEST( GaussianConditional, solve ) // create a conditional Gaussian node Matrix R = (Matrix(2, 2) << 1., 0., - 0., 1.); + 0., 1.).finished(); Matrix A1 = (Matrix(2, 2) << 1., 2., - 3., 4.); + 3., 4.).finished(); Matrix A2 = (Matrix(2, 2) << 5., 6., - 7., 8.); + 7., 8.).finished(); Vector d(2); d << 20.0, 40.0; @@ -179,7 +179,7 @@ TEST( GaussianConditional, solve_simple ) GaussianConditional cg(list_of(1)(2), 1, blockMatrix); // partial solution - Vector sx1 = (Vector(2) << 9.0, 10.0); + Vector sx1 = Vector2(9.0, 10.0); // elimination order: 1, 2 VectorValues actual = map_list_of @@ -187,7 +187,7 @@ TEST( GaussianConditional, solve_simple ) VectorValues expected = map_list_of (2, sx1) - (1, (Vector(4) << -3.1,-3.4,-11.9,-13.2)); + (1, (Vector(4) << -3.1,-3.4,-11.9,-13.2).finished()); // verify indices/size EXPECT_LONGS_EQUAL(2, (long)cg.size()); @@ -215,15 +215,15 @@ TEST( GaussianConditional, solve_multifrontal ) EXPECT(assert_equal(Vector(blockMatrix.full().rightCols(1)), cg.get_d())); // partial solution - Vector sl1 = (Vector(2) << 9.0, 10.0); + Vector sl1 = Vector2(9.0, 10.0); // elimination order; _x_, _x1_, _l1_ VectorValues actual = map_list_of (10, sl1); // parent VectorValues expected = map_list_of - (1, (Vector)(Vector(2) << -3.1,-3.4)) - (2, (Vector)(Vector(2) << -11.9,-13.2)) + (1, Vector2(-3.1,-3.4)) + (2, Vector2(-11.9,-13.2)) (10, sl1); // verify indices/size @@ -243,8 +243,8 @@ TEST( GaussianConditional, solveTranspose ) { * 1 1 9 * 1 5 */ - Matrix R11 = (Matrix(1, 1) << 1.0), S12 = (Matrix(1, 1) << 1.0); - Matrix R22 = (Matrix(1, 1) << 1.0); + Matrix R11 = (Matrix(1, 1) << 1.0).finished(), S12 = (Matrix(1, 1) << 1.0).finished(); + Matrix R22 = (Matrix(1, 1) << 1.0).finished(); Vector d1(1), d2(1); d1(0) = 9; d2(0) = 5; @@ -260,11 +260,11 @@ TEST( GaussianConditional, solveTranspose ) { VectorValues x = map_list_of - (1, (Vector(1) << 2.)) - (2, (Vector(1) << 5.)), + (1, (Vector(1) << 2.).finished()) + (2, (Vector(1) << 5.).finished()), y = map_list_of - (1, (Vector(1) << 2.)) - (2, (Vector(1) << 3.)); + (1, (Vector(1) << 2.).finished()) + (2, (Vector(1) << 3.).finished()); // test functional version VectorValues actual = cbn.backSubstituteTranspose(x); diff --git a/gtsam/linear/tests/testGaussianDensity.cpp b/gtsam/linear/tests/testGaussianDensity.cpp index f87862399..081b1d3d1 100644 --- a/gtsam/linear/tests/testGaussianDensity.cpp +++ b/gtsam/linear/tests/testGaussianDensity.cpp @@ -27,9 +27,9 @@ TEST(GaussianDensity, constructor) { Matrix R = (Matrix(2,2) << -12.1244, -5.1962, - 0., 4.6904); + 0., 4.6904).finished(); - Vector d = (Vector(2) << 1.0, 2.0), s = (Vector(2) << 3.0, 4.0); + Vector d = Vector2(1.0, 2.0), s = Vector2(3.0, 4.0); GaussianConditional conditional(1, d, R, noiseModel::Diagonal::Sigmas(s)); GaussianDensity copied(conditional); diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index d789c42fd..d87d842de 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -48,9 +48,9 @@ TEST(GaussianFactorGraph, initialization) { fg += JacobianFactor(0, 10*eye(2), -1.0*ones(2), 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); + JacobianFactor(0, -10*eye(2),1, 10*eye(2), Vector2(2.0, -1.0), unit2), + JacobianFactor(0, -5*eye(2), 2, 5*eye(2), Vector2(0.0, 1.0), unit2), + JacobianFactor(1, -5*eye(2), 2, 5*eye(2), Vector2(-1.0, 1.5), unit2); EXPECT_LONGS_EQUAL(4, (long)fg.size()); @@ -60,7 +60,7 @@ TEST(GaussianFactorGraph, initialization) { 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 - ); + ).finished(); Matrix actualIJS = fg.sparseJacobian_(); EQUALITY(expectedIJS, actualIJS); } @@ -91,14 +91,14 @@ TEST(GaussianFactorGraph, sparseJacobian) { 4., 4.,28., 4., 5.,30., 3., 6.,26., - 4., 6.,32.); + 4., 6.,32.).finished(); Matrix expected = expectedT.transpose(); GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5); - 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); + gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.).finished(), Vector2(4., 8.), model); + gfg.add(0, (Matrix(2, 3) << 9.,10., 0., 0., 0., 0.).finished(), 1, (Matrix(2, 2) << 11., 12., 14., 15.).finished(), Vector2(13.,16.), model); Matrix actual = gfg.sparseJacobian_(); @@ -114,14 +114,14 @@ TEST(GaussianFactorGraph, matrices) { // 9 10 0 11 12 13 // 0 0 0 14 15 16 - Matrix A00 = (Matrix(2, 3) << 1, 2, 3, 5, 6, 7); - Matrix A10 = (Matrix(2, 3) << 9, 10, 0, 0, 0, 0); - Matrix A11 = (Matrix(2, 2) << 11, 12, 14, 15); + Matrix A00 = (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(); + Matrix A10 = (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(); + Matrix A11 = (Matrix(2, 2) << 11, 12, 14, 15).finished(); GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Unit::Create(2); - gfg.add(0, A00, (Vector(2) << 4., 8.), model); - gfg.add(0, A10, 1, A11, (Vector(2) << 13.,16.), model); + gfg.add(0, A00, Vector2(4., 8.), model); + gfg.add(0, A10, 1, A11, Vector2(13.,16.), model); Matrix Ab(4,6); Ab << @@ -150,8 +150,8 @@ TEST(GaussianFactorGraph, matrices) { // hessianBlockDiagonal VectorValues expectLdiagonal; // Make explicit that diagonal is sum-squares of columns - expectLdiagonal.insert(0, (Vector(3) << 1+25+81, 4+36+100, 9+49)); - expectLdiagonal.insert(1, (Vector(2) << 121+196, 144+225)); + expectLdiagonal.insert(0, Vector3(1+25+81, 4+36+100, 9+49)); + expectLdiagonal.insert(1, Vector2(121+196, 144+225)); EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal())); // hessianBlockDiagonal @@ -168,11 +168,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(0, 10*eye(2), 2, -10*eye(2), (Vector(2) << 2.0, -1.0), unit2); + fg += JacobianFactor(0, 10*eye(2), 2, -10*eye(2), Vector2(2.0, -1.0), unit2); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(1, 5*eye(2), 2, -5*eye(2), (Vector(2) << 0.0, 1.0), unit2); + fg += JacobianFactor(1, 5*eye(2), 2, -5*eye(2), Vector2(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), (Vector(2) << -1.0, 1.5), unit2); + fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), Vector2(-1.0, 1.5), unit2); return fg; } @@ -185,9 +185,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, (Vector(2) << 5.0, -12.5)) - (2, (Vector(2) << 30.0, 5.0)) - (0, (Vector(2) << -25.0, 17.5)); + (1, Vector2(5.0, -12.5)) + (2, Vector2(30.0, 5.0)) + (0, Vector2(-25.0, 17.5)); // Check the gradient at delta=0 VectorValues zero = VectorValues::Zero(expected); @@ -207,15 +207,15 @@ TEST( GaussianFactorGraph, transposeMultiplication ) GaussianFactorGraph A = createSimpleGaussianFactorGraph(); Errors e; e += - (Vector(2) << 0.0, 0.0), - (Vector(2) << 15.0, 0.0), - (Vector(2) << 0.0,-5.0), - (Vector(2) << -7.5,-5.0); + Vector2(0.0, 0.0), + Vector2(15.0, 0.0), + Vector2(0.0,-5.0), + Vector2(-7.5,-5.0); VectorValues expected; - 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)); + expected.insert(1, Vector2(-37.5,-50.0)); + expected.insert(2, Vector2(-150.0, 25.0)); + expected.insert(0, Vector2(187.5, 25.0)); VectorValues actual = A.transposeMultiply(e); EXPECT(assert_equal(expected, actual)); @@ -259,14 +259,14 @@ TEST( GaussianFactorGraph, multiplyHessianAdd ) GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); VectorValues x = map_list_of - (0, (Vector(2) << 1,2)) - (1, (Vector(2) << 3,4)) - (2, (Vector(2) << 5,6)); + (0, Vector2(1,2)) + (1, Vector2(3,4)) + (2, Vector2(5,6)); VectorValues expected; - expected.insert(0, (Vector(2) << -450, -450)); - expected.insert(1, (Vector(2) << 0, 0)); - expected.insert(2, (Vector(2) << 950, 1050)); + expected.insert(0, Vector2(-450, -450)); + expected.insert(1, Vector2(0, 0)); + expected.insert(2, Vector2(950, 1050)); VectorValues actual; gfg.multiplyHessianAdd(1.0, x, actual); @@ -280,8 +280,8 @@ TEST( GaussianFactorGraph, multiplyHessianAdd ) /* ************************************************************************* */ static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() { GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); - 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); + gfg += HessianFactor(1, 2, 100*eye(2,2), zeros(2,2), Vector2(0.0, 1.0), + 400*eye(2,2), Vector2(1.0, 1.0), 3.0); return gfg; } @@ -297,14 +297,14 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 ) EXPECT(assert_equal(Y,AtA*X)); VectorValues x = map_list_of - (0, (Vector(2) << 1,2)) - (1, (Vector(2) << 3,4)) - (2, (Vector(2) << 5,6)); + (0, Vector2(1,2)) + (1, Vector2(3,4)) + (2, Vector2(5,6)); VectorValues expected; - expected.insert(0, (Vector(2) << -450, -450)); - expected.insert(1, (Vector(2) << 300, 400)); - expected.insert(2, (Vector(2) << 2950, 3450)); + expected.insert(0, Vector2(-450, -450)); + expected.insert(1, Vector2(300, 400)); + expected.insert(2, Vector2(2950, 3450)); VectorValues actual; gfg.multiplyHessianAdd(1.0, x, actual); @@ -346,7 +346,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 = - (Vector(6) << -25, 17.5, 5, -13.5, 29, 4); + Vector expected = - (Vector(6) << -25, 17.5, 5, -13.5, 29, 4).finished(); EXPECT(assert_equal(expected, eta)); EXPECT(assert_equal(A.transpose()*b, eta)); } @@ -358,9 +358,9 @@ TEST( GaussianFactorGraph, gradientAtZero ) GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); VectorValues expected; VectorValues actual = gfg.gradientAtZero(); - expected.insert(0, (Vector(2) << -25, 17.5)); - expected.insert(1, (Vector(2) << 5, -13.5)); - expected.insert(2, (Vector(2) << 29, 4)); + expected.insert(0, Vector2(-25, 17.5)); + expected.insert(1, Vector2(5, -13.5)); + expected.insert(2, Vector2(29, 4)); EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 90a9cceda..cba65580e 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -59,25 +59,25 @@ TEST(HessianFactor, ConversionConstructor) 0.0, -25.0000, 0.0, 25.0000, 0.0, 0.0, 7.5000, -100.0000, 0.0, 0.0, 0.0, 100.0000, 0.0, -20.0000, 0.0, -100.0000, 0.0, 0.0, 0.0, 100.0000, 10.0000, - 25.0000, -17.5000, -5.0000, 7.5000, -20.0000, 10.0000, 8.2500))); + 25.0000, -17.5000, -5.0000, 7.5000, -20.0000, 10.0000, 8.2500).finished())); JacobianFactor jacobian( 0, (Matrix(4,2) << -1., 0., +0.,-1., 1., 0., - +0.,1.), + +0.,1.).finished(), 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 - (Vector(4) << -0.2, 0.3, 0.2, -0.1), - noiseModel::Diagonal::Sigmas((Vector(4) << 0.2, 0.2, 0.1, 0.1))); + 0., 0., 0.00, -1.).finished(), // f2 + (Vector(4) << -0.2, 0.3, 0.2, -0.1).finished(), + noiseModel::Diagonal::Sigmas((Vector(4) << 0.2, 0.2, 0.1, 0.1).finished())); HessianFactor actual(jacobian); VectorValues values = pair_list_of - (0, (Vector(2) << 1.0, 2.0)) - (1, (Vector(4) << 3.0, 4.0, 5.0, 6.0)); + (0, Vector2(1.0, 2.0)) + (1, (Vector(4) << 3.0, 4.0, 5.0, 6.0).finished()); EXPECT_LONGS_EQUAL(2, (long)actual.size()); EXPECT(assert_equal(expected, actual, 1e-9)); @@ -87,8 +87,8 @@ TEST(HessianFactor, ConversionConstructor) /* ************************************************************************* */ TEST(HessianFactor, Constructor1) { - Matrix G = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0); - Vector g = (Vector(2) << -8.0, -9.0); + Matrix G = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished(); + Vector g = Vector2(-8.0, -9.0); double f = 10.0; HessianFactor factor(0, G, g, f); @@ -98,7 +98,7 @@ TEST(HessianFactor, Constructor1) EXPECT(assert_equal(g, Vector(factor.linearTerm()))); EXPECT_LONGS_EQUAL(1, (long)factor.size()); - VectorValues dx = pair_list_of(0, (Vector(2) << 1.5, 2.5)); + VectorValues dx = pair_list_of(0, Vector2(1.5, 2.5)); // error 0.5*(f - 2*x'*g + x'*G*x) double expected = 80.375; @@ -112,7 +112,7 @@ TEST(HessianFactor, Constructor1) /* ************************************************************************* */ TEST(HessianFactor, Constructor1b) { - Vector mu = (Vector(2) << 1.0,2.0); + Vector mu = Vector2(1.0,2.0); Matrix Sigma = eye(2,2); HessianFactor factor(0, mu, Sigma); @@ -131,15 +131,15 @@ TEST(HessianFactor, Constructor1b) /* ************************************************************************* */ TEST(HessianFactor, Constructor2) { - 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); + Matrix G11 = (Matrix(1,1) << 1.0).finished(); + Matrix G12 = (Matrix(1,2) << 2.0, 4.0).finished(); + Matrix G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished(); + Vector g1 = (Vector(1) << -7.0).finished(); + Vector g2 = Vector2(-8.0, -9.0); double f = 10.0; - Vector dx0 = (Vector(1) << 0.5); - Vector dx1 = (Vector(2) << 1.5, 2.5); + Vector dx0 = (Vector(1) << 0.5).finished(); + Vector dx1 = Vector2(1.5, 2.5); VectorValues dx = pair_list_of (0, dx0) @@ -165,31 +165,31 @@ TEST(HessianFactor, Constructor2) VectorValues dxLarge = pair_list_of (0, dx0) (1, dx1) - (2, (Vector(2) << 0.1, 0.2)); + (2, Vector2(0.1, 0.2)); EXPECT_DOUBLES_EQUAL(expected, factor.error(dxLarge), 1e-10); } /* ************************************************************************* */ TEST(HessianFactor, Constructor3) { - 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 G11 = (Matrix(1,1) << 1.0).finished(); + Matrix G12 = (Matrix(1,2) << 2.0, 4.0).finished(); + Matrix G13 = (Matrix(1,3) << 3.0, 6.0, 9.0).finished(); - 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 G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished(); + Matrix G23 = (Matrix(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0).finished(); - Matrix G33 = (Matrix(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).finished(); - Vector g1 = (Vector(1) << -7.0); - Vector g2 = (Vector(2) << -8.0, -9.0); - Vector g3 = (Vector(3) << 1.0, 2.0, 3.0); + Vector g1 = (Vector(1) << -7.0).finished(); + Vector g2 = Vector2(-8.0, -9.0); + Vector g3 = Vector3(1.0, 2.0, 3.0); double f = 10.0; - Vector dx0 = (Vector(1) << 0.5); - Vector dx1 = (Vector(2) << 1.5, 2.5); - Vector dx2 = (Vector(3) << 1.5, 2.5, 3.5); + Vector dx0 = (Vector(1) << 0.5).finished(); + Vector dx1 = Vector2(1.5, 2.5); + Vector dx2 = Vector3(1.5, 2.5, 3.5); VectorValues dx = pair_list_of (0, dx0) @@ -219,24 +219,24 @@ TEST(HessianFactor, Constructor3) /* ************************************************************************* */ TEST(HessianFactor, ConstructorNWay) { - 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 G11 = (Matrix(1,1) << 1.0).finished(); + Matrix G12 = (Matrix(1,2) << 2.0, 4.0).finished(); + Matrix G13 = (Matrix(1,3) << 3.0, 6.0, 9.0).finished(); - 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 G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished(); + Matrix G23 = (Matrix(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0).finished(); - Matrix G33 = (Matrix(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).finished(); - Vector g1 = (Vector(1) << -7.0); - Vector g2 = (Vector(2) << -8.0, -9.0); - Vector g3 = (Vector(3) << 1.0, 2.0, 3.0); + Vector g1 = (Vector(1) << -7.0).finished(); + Vector g2 = Vector2(-8.0, -9.0); + Vector g3 = Vector3(1.0, 2.0, 3.0); double f = 10.0; - Vector dx0 = (Vector(1) << 0.5); - Vector dx1 = (Vector(2) << 1.5, 2.5); - Vector dx2 = (Vector(3) << 1.5, 2.5, 3.5); + Vector dx0 = (Vector(1) << 0.5).finished(); + Vector dx1 = Vector2(1.5, 2.5); + Vector dx2 = Vector3(1.5, 2.5, 3.5); VectorValues dx = pair_list_of (0, dx0) @@ -275,25 +275,25 @@ TEST(HessianFactor, CombineAndEliminate) Matrix A01 = (Matrix(3,3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0); + 0.0, 0.0, 1.0).finished(); Vector3 b0(1.5, 1.5, 1.5); Vector3 s0(1.6, 1.6, 1.6); Matrix A10 = (Matrix(3,3) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, - 0.0, 0.0, 2.0); + 0.0, 0.0, 2.0).finished(); Matrix A11 = (Matrix(3,3) << -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, - 0.0, 0.0, -2.0); + 0.0, 0.0, -2.0).finished(); Vector3 b1(2.5, 2.5, 2.5); Vector3 s1(2.6, 2.6, 2.6); Matrix A21 = (Matrix(3,3) << 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, - 0.0, 0.0, 3.0); + 0.0, 0.0, 3.0).finished(); Vector3 b2(3.5, 3.5, 3.5); Vector3 s2(3.6, 3.6, 3.6); @@ -331,7 +331,7 @@ TEST(HessianFactor, eliminate2 ) // sigmas double sigma1 = 0.2; double sigma2 = 0.1; - Vector sigmas = (Vector(4) << sigma1, sigma1, sigma2, sigma2); + Vector sigmas = (Vector(4) << sigma1, sigma1, sigma2, sigma2).finished(); // the combined linear factor Matrix Ax2 = (Matrix(4,2) << @@ -340,7 +340,7 @@ TEST(HessianFactor, eliminate2 ) +0.,-1., 1., 0., +0.,1. - ); + ).finished(); Matrix Al1x1 = (Matrix(4,4) << // l1 x1 @@ -348,7 +348,7 @@ TEST(HessianFactor, eliminate2 ) 0., 1., 0.00, 0., // f4 0., 0., -1., 0., // f2 0., 0., 0.00,-1. // f2 - ); + ).finished(); // the RHS Vector b2(4); @@ -374,12 +374,12 @@ TEST(HessianFactor, eliminate2 ) Matrix R11 = (Matrix(2,2) << 1.00, 0.00, 0.00, 1.00 - )/oldSigma; + ).finished()/oldSigma; Matrix S12 = (Matrix(2,4) << -0.20, 0.00,-0.80, 0.00, +0.00,-0.20,+0.00,-0.80 - )/oldSigma; - Vector d = (Vector(2) << 0.2,-0.14)/oldSigma; + ).finished()/oldSigma; + Vector d = Vector2(0.2,-0.14)/oldSigma; GaussianConditional expectedCG(0, d, R11, 1, S12); EXPECT(assert_equal(expectedCG, *actual_Chol.first, 1e-4)); @@ -389,8 +389,8 @@ TEST(HessianFactor, eliminate2 ) // l1 x1 1.00, 0.00, -1.00, 0.00, 0.00, 1.00, +0.00, -1.00 - )/sigma; - Vector b1 = (Vector(2) << 0.0,0.894427); + ).finished()/sigma; + Vector b1 = Vector2(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)); } @@ -401,14 +401,14 @@ TEST(HessianFactor, combine) { // update the information matrix with a single jacobian factor Matrix A0 = (Matrix(2, 2) << 11.1803399, 0.0, - 0.0, 11.1803399); + 0.0, 11.1803399).finished(); Matrix A1 = (Matrix(2, 2) << -2.23606798, 0.0, - 0.0, -2.23606798); + 0.0, -2.23606798).finished(); Matrix A2 = (Matrix(2, 2) << -8.94427191, 0.0, - 0.0, -8.94427191); - Vector b = (Vector(2) << 2.23606798,-1.56524758); + 0.0, -8.94427191).finished(); + Vector b = Vector2(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); @@ -423,7 +423,7 @@ TEST(HessianFactor, combine) { 0.0, -25.0000, 0.0, 5.0000, 0.0, 20.0000, 3.5000, -100.0000, 0.0, 20.0000, 0.0, 80.0000, 0.0, -20.0000, 0.0, -100.0000, 0.0, 20.0000, 0.0, 80.0000, 14.0000, - 25.0000, -17.5000, -5.0000, 3.5000, -20.0000, 14.0000, 7.4500); + 25.0000, -17.5000, -5.0000, 3.5000, -20.0000, 14.0000, 7.4500).finished(); EXPECT(assert_equal(expected, Matrix(actual.matrixObject().full()), tol)); } @@ -431,11 +431,11 @@ TEST(HessianFactor, combine) { /* ************************************************************************* */ TEST(HessianFactor, gradientAtZero) { - Matrix G11 = (Matrix(1, 1) << 1); - Matrix G12 = (Matrix(1, 2) << 0, 0); - Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1); - Vector g1 = (Vector(1) << -7); - Vector g2 = (Vector(2) << -8, -9); + Matrix G11 = (Matrix(1, 1) << 1).finished(); + Matrix G12 = (Matrix(1, 2) << 0, 0).finished(); + Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1).finished(); + Vector g1 = (Vector(1) << -7).finished(); + Vector g2 = Vector2(-8, -9); double f = 194; HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); @@ -452,19 +452,19 @@ TEST(HessianFactor, gradientAtZero) /* ************************************************************************* */ TEST(HessianFactor, hessianDiagonal) { - Matrix G11 = (Matrix(1, 1) << 1); - Matrix G12 = (Matrix(1, 2) << 0, 0); - Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1); - Vector g1 = (Vector(1) << -7); - Vector g2 = (Vector(2) << -8, -9); + Matrix G11 = (Matrix(1, 1) << 1).finished(); + Matrix G12 = (Matrix(1, 2) << 0, 0).finished(); + Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1).finished(); + Vector g1 = (Vector(1) << -7).finished(); + Vector g2 = Vector2(-8, -9); double f = 194; HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); // hessianDiagonal VectorValues expected; - expected.insert(0, (Vector(1) << 1)); - expected.insert(1, (Vector(2) << 1,1)); + expected.insert(0, (Vector(1) << 1).finished()); + expected.insert(1, Vector2(1,1)); EXPECT(assert_equal(expected, factor.hessianDiagonal())); // hessianBlockDiagonal diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 1650df0ec..7b2d59171 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -42,8 +42,8 @@ namespace { (make_pair(15, 3*Matrix3::Identity())); // RHS and sigmas - const Vector b = (Vector(3) << 1., 2., 3.); - const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.5)); + const Vector b = Vector3(1., 2., 3.); + const SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.5)); } } @@ -149,14 +149,14 @@ TEST(JabobianFactor, Hessian_conversion) { 1.57, 2.695, -1.1, -2.35, 2.695, 11.3125, -0.65, -10.225, -1.1, -0.65, 1, 0.5, - -2.35, -10.225, 0.5, 9.25), - (Vector(4) << -7.885, -28.5175, 2.75, 25.675), + -2.35, -10.225, 0.5, 9.25).finished(), + (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), 73.1725); JacobianFactor expected(0, (Matrix(2,4) << 1.2530, 2.1508, -0.8779, -1.8755, - 0, 2.5858, 0.4789, -2.3943), - (Vector(2) << -6.2929, -5.7941)); + 0, 2.5858, 0.4789, -2.3943).finished(), + Vector2(-6.2929, -5.7941)); EXPECT(assert_equal(expected, JacobianFactor(hessian), 1e-3)); } @@ -296,9 +296,9 @@ TEST(JacobianFactor, matrices) // hessianDiagonal VectorValues expectDiagonal; // below we divide by the variance 0.5^2 - expectDiagonal.insert(5, (Vector(3) << 1, 1, 1)/0.25); - expectDiagonal.insert(10, (Vector(3) << 4, 4, 4)/0.25); - expectDiagonal.insert(15, (Vector(3) << 9, 9, 9)/0.25); + expectDiagonal.insert(5, Vector3(1, 1, 1)/0.25); + expectDiagonal.insert(10, Vector3(4, 4, 4)/0.25); + expectDiagonal.insert(15, Vector3(9, 9, 9)/0.25); EXPECT(assert_equal(expectDiagonal, factor.hessianDiagonal())); // hessianBlockDiagonal @@ -315,22 +315,22 @@ TEST(JacobianFactor, operators ) SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); Matrix I = eye(2); - Vector b = (Vector(2) << 0.2,-0.1); + Vector b = Vector2(0.2,-0.1); JacobianFactor lf(1, -I, 2, I, b, sigma0_1); VectorValues c; - c.insert(1, (Vector(2) << 10.,20.)); - c.insert(2, (Vector(2) << 30.,60.)); + c.insert(1, Vector2(10.,20.)); + c.insert(2, Vector2(30.,60.)); // test A*x - Vector expectedE = (Vector(2) << 200.,400.); + Vector expectedE = Vector2(200.,400.); Vector actualE = lf * c; EXPECT(assert_equal(expectedE, actualE)); // test A^e VectorValues expectedX; - expectedX.insert(1, (Vector(2) << -2000.,-4000.)); - expectedX.insert(2, (Vector(2) << 2000., 4000.)); + expectedX.insert(1, Vector2(-2000.,-4000.)); + expectedX.insert(2, Vector2(2000., 4000.)); VectorValues actualX = VectorValues::Zero(expectedX); lf.transposeMultiplyAdd(1.0, actualE, actualX); EXPECT(assert_equal(expectedX, actualX)); @@ -338,8 +338,8 @@ TEST(JacobianFactor, operators ) // test gradient at zero Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian(); VectorValues expectedG; - expectedG.insert(1, (Vector(2) << 20,-10)); - expectedG.insert(2, (Vector(2) << -20, 10)); + expectedG.insert(1, Vector2(20,-10)); + expectedG.insert(2, Vector2(-20, 10)); FastVector keys; keys += 1,2; EXPECT(assert_equal(-A.transpose()*b2, expectedG.vector(keys))); VectorValues actualG = lf.gradientAtZero(); @@ -368,25 +368,25 @@ TEST(JacobianFactor, eliminate) Matrix A01 = (Matrix(3, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0); + 0.0, 0.0, 1.0).finished(); Vector3 b0(1.5, 1.5, 1.5); Vector3 s0(1.6, 1.6, 1.6); Matrix A10 = (Matrix(3, 3) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, - 0.0, 0.0, 2.0); + 0.0, 0.0, 2.0).finished(); Matrix A11 = (Matrix(3, 3) << -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, - 0.0, 0.0, -2.0); + 0.0, 0.0, -2.0).finished(); Vector3 b1(2.5, 2.5, 2.5); Vector3 s1(2.6, 2.6, 2.6); Matrix A21 = (Matrix(3, 3) << 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, - 0.0, 0.0, 3.0); + 0.0, 0.0, 3.0).finished(); Vector3 b2(3.5, 3.5, 3.5); Vector3 s2(3.6, 3.6, 3.6); @@ -420,7 +420,7 @@ TEST(JacobianFactor, eliminate2 ) // sigmas double sigma1 = 0.2; double sigma2 = 0.1; - Vector sigmas = (Vector(4) << sigma1, sigma1, sigma2, sigma2); + Vector sigmas = (Vector(4) << sigma1, sigma1, sigma2, sigma2).finished(); // the combined linear factor Matrix Ax2 = (Matrix(4, 2) << @@ -429,7 +429,7 @@ TEST(JacobianFactor, eliminate2 ) +0.,-1., 1., 0., +0.,1. - ); + ).finished(); Matrix Al1x1 = (Matrix(4, 4) << // l1 x1 @@ -437,7 +437,7 @@ TEST(JacobianFactor, eliminate2 ) 0., 1., 0.00, 0., // f4 0., 0., -1., 0., // f2 0., 0., 0.00,-1. // f2 - ); + ).finished(); // the RHS Vector b2(4); @@ -460,12 +460,12 @@ TEST(JacobianFactor, eliminate2 ) Matrix R11 = (Matrix(2, 2) << 1.00, 0.00, 0.00, 1.00 - )/oldSigma; + ).finished()/oldSigma; Matrix S12 = (Matrix(2, 4) << -0.20, 0.00,-0.80, 0.00, +0.00,-0.20,+0.00,-0.80 - )/oldSigma; - Vector d = (Vector(2) << 0.2,-0.14)/oldSigma; + ).finished()/oldSigma; + Vector d = Vector2(0.2,-0.14)/oldSigma; GaussianConditional expectedCG(2, d, R11, 11, S12); EXPECT(assert_equal(expectedCG, *actual.first, 1e-4)); @@ -476,8 +476,8 @@ TEST(JacobianFactor, eliminate2 ) // l1 x1 1.00, 0.00, -1.00, 0.00, 0.00, 1.00, +0.00, -1.00 - )/sigma; - Vector b1 = (Vector(2) << 0.0, 0.894427); + ).finished()/sigma; + Vector b1 = Vector2(0.0, 0.894427); JacobianFactor expectedLF(11, Bl1x1, b1); EXPECT(assert_equal(expectedLF, *actual.second,1e-3)); } @@ -500,7 +500,7 @@ TEST(JacobianFactor, EliminateQR) 0., 0., 0., 0., 1., 1., 9., 1., 5., 5., 3., 0., 0., 0., 0., 1., 1., 3., 3., 2., 0., 5., 0., 0., 0., 0., 0., 0., 0., 0., 2., 4., 6., - 0., 0., 0., 0., 0., 0., 0., 0., 6., 3., 4.); + 0., 0., 0., 0., 0., 0., 0., 0., 6., 3., 4.).finished(); // Create factor graph const SharedDiagonal sig_4D = noiseModel::Isotropic::Sigma(4, 0.5); @@ -527,7 +527,7 @@ TEST(JacobianFactor, EliminateQR) 0., 0., 0., 0., 0., 0., 0., -4.6479, -1.9367, -6.5170, -3.7685, 0., 0., 0., 0., 0., 0., 0., 0., 8.2503, 3.3757, 6.8476, 0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090, - 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -7.1635); + 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -7.1635).finished(); GaussianConditional expectedFragment( list_of(3)(5)(7)(9)(11), 3, VerticalBlockMatrix(list_of(2)(2)(2)(2)(2)(1), R)); @@ -561,7 +561,7 @@ TEST ( JacobianFactor, constraint_eliminate1 ) EXPECT(actual.second->size() == 0); // verify conditional Gaussian - Vector sigmas = (Vector(2) << 0.0, 0.0); + Vector sigmas = Vector2(0.0, 0.0); GaussianConditional expCG(1, v, eye(2), noiseModel::Diagonal::Sigmas(sigmas)); EXPECT(assert_equal(expCG, *actual.first)); } @@ -600,12 +600,12 @@ TEST ( JacobianFactor, constraint_eliminate2 ) // verify CG Matrix R = (Matrix(2, 2) << 1.0, 2.0, - 0.0, 1.0); + 0.0, 1.0).finished(); Matrix S = (Matrix(2, 2) << 1.0, 2.0, - 0.0, 0.0); - Vector d = (Vector(2) << 3.0, 0.6666); - Vector sigmas = (Vector(2) << 0.0, 0.0); + 0.0, 0.0).finished(); + Vector d = Vector2(3.0, 0.6666); + Vector sigmas = Vector2(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 87ff419c8..999541ff5 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((Vector(2) << x, y)) { + Vector((Vector(2) << x, y).finished()) { } }; @@ -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 = (Matrix(2, 2) << 0.01, 0.0, 0.0, 0.01); + Matrix Sigma = (Matrix(2, 2) << 0.01, 0.0, 0.0, 0.01).finished(); 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 = (Vector(2) << 1.0, 0.0); + Vector u = Vector2(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 = (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 F = (Matrix(2, 2) << 1.0, 0.1, 0.2, 1.1).finished(); + Matrix B = (Matrix(2, 3) << 1.0, 0.1, 0.2, 1.1, 1.2, 0.8).finished(); + Vector u = Vector3(1.0, 0.0, 2.0); + Matrix R = (Matrix(2, 2) << 1.0, 0.5, 0.0, 3.0).finished(); Matrix M = trans(R)*R; Matrix Q = inverse(M); @@ -177,7 +177,7 @@ TEST( KalmanFilter, QRvsCholesky ) { 0.0, 0.0, 0.0, -0.6, 10.1, 61.1, 0.0, 0.0, 0.0, 0.0, -63.8, 0.0, 0.0, 0.0, 0.0, 625.0, 0.0, 0.0, 63.8, -0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 625.0, 0.0, - -0.6, -0.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 625.0); + -0.6, -0.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 625.0).finished(); // Create two Kalman filter of dimension 9, one using QR the other Cholesky KalmanFilter kfa(9, KalmanFilter::QR), kfb(9, KalmanFilter::CHOLESKY); @@ -196,7 +196,7 @@ TEST( KalmanFilter, QRvsCholesky ) { 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0); + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0).finished(); Matrix B = zeros(9, 1); Vector u = zero(1); Matrix dt_Q_k = 1e-6 * (Matrix(9, 9) << @@ -208,7 +208,7 @@ TEST( KalmanFilter, QRvsCholesky ) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 22.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 22.2, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 22.2); + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 22.2).finished(); // Do prediction step KalmanFilter::State pa = kfa.predictQ(p0a, Psi_k, B, u, dt_Q_k); @@ -219,7 +219,7 @@ TEST( KalmanFilter, QRvsCholesky ) { EXPECT(assert_equal(pa->information(), pb->information(), 1e-7)); // and in addition attain the correct covariance - Vector expectedMean = (Vector(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.).finished(); EXPECT(assert_equal(expectedMean, pa->mean(), 1e-7)); EXPECT(assert_equal(expectedMean, pb->mean(), 1e-7)); Matrix expected = 1e-6 * (Matrix(9, 9) << @@ -231,7 +231,7 @@ TEST( KalmanFilter, QRvsCholesky ) { 0.0, 0.2, 1.2, -0.6, 10.1, 61.3, 0.0, 0.0, 0.0, 0.0, -63.8, 0.0, 0.0, 0.0, 0.0, 647.2, 0.0, 0.0, 63.8, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 647.2, 0.0, - -0.6, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 647.2); + -0.6, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 647.2).finished(); EXPECT(assert_equal(expected, pa->covariance(), 1e-7)); EXPECT(assert_equal(expected, pb->covariance(), 1e-7)); @@ -239,9 +239,9 @@ TEST( KalmanFilter, QRvsCholesky ) { 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 = (Vector(3) << 0.2599 , 1.3327 , 0.2007); - Vector sigmas = (Vector(3) << 0.3323 , 0.2470 , 0.1904); + -83.6, 5.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.).finished(); + Vector z = Vector3(0.2599 , 1.3327 , 0.2007); + Vector sigmas = Vector3(0.3323 , 0.2470 , 0.1904); SharedDiagonal modelR = noiseModel::Diagonal::Sigmas(sigmas); // do update @@ -253,7 +253,7 @@ TEST( KalmanFilter, QRvsCholesky ) { EXPECT(assert_equal(pa2->information(), pb2->information(), 1e-7)); // and in addition attain the correct mean and covariance - Vector expectedMean2 = (Vector(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).finished(); 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 * (Matrix(9, 9) << @@ -265,7 +265,7 @@ TEST( KalmanFilter, QRvsCholesky ) { 0.0, 0.2, 1.2, -0.6, 10.1, 61.3, -0.0, 0.0, 0.0, 0.0, -64.0, -0.0, -0.0, -0.0, -0.0, 647.2, -0.0, 0.0, 63.9, -0.0, 0.1, -0.0, -0.0, 0.0, -0.0, 647.2, 0.1, - -0.5, -0.1, 0.0, -0.0, -0.0, 0.0, 0.0, 0.1, 635.8); + -0.5, -0.1, 0.0, -0.0, -0.0, 0.0, 0.0, 0.1, 635.8).finished(); EXPECT(assert_equal(expected2, pa2->covariance(), 1e-7)); EXPECT(assert_equal(expected2, pb2->covariance(), 1e-7)); diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index df0f8a774..5c6b2729d 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -37,28 +37,28 @@ static double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var; static Matrix R = (Matrix(3, 3) << s_1, 0.0, 0.0, 0.0, s_1, 0.0, - 0.0, 0.0, s_1); + 0.0, 0.0, s_1).finished(); static Matrix Sigma = (Matrix(3, 3) << var, 0.0, 0.0, 0.0, var, 0.0, - 0.0, 0.0, var); + 0.0, 0.0, var).finished(); //static double inf = numeric_limits::infinity(); /* ************************************************************************* */ TEST(NoiseModel, constructors) { - Vector whitened = (Vector(3) << 5.0,10.0,15.0); - Vector unwhitened = (Vector(3) << 10.0,20.0,30.0); + Vector whitened = Vector3(5.0,10.0,15.0); + Vector unwhitened = Vector3(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((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(Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma).finished())); + m.push_back(Diagonal::Variances((Vector(3) << var, var, var).finished())); + m.push_back(Diagonal::Precisions((Vector(3) << prc, prc, prc).finished())); m.push_back(Isotropic::Sigma(3, sigma)); m.push_back(Isotropic::Variance(3, var)); m.push_back(Isotropic::Precision(3, prc)); @@ -80,7 +80,7 @@ TEST(NoiseModel, constructors) Matrix expectedR((Matrix(3, 3) << s_1, 0.0, 0.0, 0.0, s_1, 0.0, - 0.0, 0.0, s_1)); + 0.0, 0.0, s_1).finished()); BOOST_FOREACH(Gaussian::shared_ptr mi, m) EXPECT(assert_equal(expectedR,mi->R())); @@ -89,12 +89,12 @@ TEST(NoiseModel, constructors) 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)); + 1.0, 0.0, 0.0, 1.0).finished()); 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)); + s_1, 0.0, 0.0, s_1).finished()); BOOST_FOREACH(Gaussian::shared_ptr mi, m) EXPECT(assert_equal(expected,mi->Whiten(H))); @@ -107,7 +107,7 @@ TEST(NoiseModel, constructors) /* ************************************************************************* */ TEST(NoiseModel, Unit) { - Vector v = (Vector(3) << 5.0,10.0,15.0); + Vector v = Vector3(5.0,10.0,15.0); Gaussian::shared_ptr u(Unit::Create(3)); EXPECT(assert_equal(v,u->whiten(v))); } @@ -117,8 +117,8 @@ TEST(NoiseModel, equals) { Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R), g2 = Gaussian::SqrtInformation(eye(3,3)); - Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma)), - d2 = Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3)); + Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma).finished()), + d2 = Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)); Isotropic::shared_ptr i1 = Isotropic::Sigma(3, sigma), i2 = Isotropic::Sigma(3, 0.7); @@ -155,8 +155,8 @@ TEST(NoiseModel, ConstrainedConstructors ) Constrained::shared_ptr actual; size_t d = 3; double m = 100.0; - Vector sigmas = (Vector(3) << sigma, 0.0, 0.0); - Vector mu = (Vector(3) << 200.0, 300.0, 400.0); + Vector sigmas = (Vector(3) << sigma, 0.0, 0.0).finished(); + Vector mu = Vector3(200.0, 300.0, 400.0); actual = Constrained::All(d); // TODO: why should this be a thousand ??? Dummy variable? EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu())); @@ -180,12 +180,12 @@ TEST(NoiseModel, ConstrainedConstructors ) /* ************************************************************************* */ TEST(NoiseModel, ConstrainedMixed ) { - 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)); + Vector feasible = Vector3(1.0, 0.0, 1.0), + infeasible = Vector3(1.0, 1.0, 1.0); + Diagonal::shared_ptr d = Constrained::MixedSigmas((Vector(3) << sigma, 0.0, sigma).finished()); // NOTE: we catch constrained variables elsewhere, so whitening does nothing - 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))); + EXPECT(assert_equal(Vector3(0.5, 1.0, 0.5),d->whiten(infeasible))); + EXPECT(assert_equal(Vector3(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); @@ -194,13 +194,13 @@ TEST(NoiseModel, ConstrainedMixed ) /* ************************************************************************* */ TEST(NoiseModel, ConstrainedAll ) { - Vector feasible = (Vector(3) << 0.0, 0.0, 0.0), - infeasible = (Vector(3) << 1.0, 1.0, 1.0); + Vector feasible = Vector3(0.0, 0.0, 0.0), + infeasible = Vector3(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((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))); + EXPECT(assert_equal(Vector3(1.0, 1.0, 1.0),i->whiten(infeasible))); + EXPECT(assert_equal(Vector3(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); @@ -213,15 +213,15 @@ namespace exampleQR { -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 = (Vector(4) << 0.2, 0.2, 0.1, 0.1); + 0., 1., 0., 0., 0., -1., -0.1).finished(); + Vector sigmas = (Vector(4) << 0.2, 0.2, 0.1, 0.1).finished(); // the matrix AB yields the following factorized version: 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, - 0.0, 0.0, 0.0, 4.47214, 0.0, -4.47214, 0.894427); + 0.0, 0.0, 0.0, 4.47214, 0.0, -4.47214, 0.894427).finished(); SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas); } @@ -232,7 +232,7 @@ TEST( NoiseModel, QR ) Matrix Ab2 = exampleQR::Ab; // otherwise overwritten ! // Expected result - Vector expectedSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607); + Vector expectedSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished(); SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas); // Call Gaussian version @@ -249,7 +249,7 @@ TEST( NoiseModel, QR ) 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, - 0., 0., 0., 1., 0., -1., 0.2); + 0., 0., 0., 1., 0., -1., 0.2).finished(); EXPECT(linear_dependent(expectedRd2,Ab2,1e-6)); // Ab was modified in place !!! } @@ -257,10 +257,10 @@ TEST( NoiseModel, QR ) TEST(NoiseModel, QRNan ) { SharedDiagonal constrained = noiseModel::Constrained::All(2); - Matrix Ab = (Matrix(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.).finished(); SharedDiagonal expected = noiseModel::Constrained::All(2); - Matrix expectedAb = (Matrix(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).finished(); SharedDiagonal actual = constrained->QR(Ab); EXPECT(assert_equal(*expected,*actual)); @@ -317,7 +317,7 @@ TEST(NoiseModel, ScalarOrVector ) /* ************************************************************************* */ TEST(NoiseModel, WhitenInPlace) { - Vector sigmas = (Vector(3) << 0.1, 0.1, 0.1); + Vector sigmas = Vector3(0.1, 0.1, 0.1); SharedDiagonal model = Diagonal::Sigmas(sigmas); Matrix A = eye(3); model->WhitenInPlace(A); @@ -340,8 +340,8 @@ TEST(NoiseModel, robustFunction) TEST(NoiseModel, robustNoise) { const double k = 10.0, error1 = 1.0, error2 = 100.0; - Matrix A = (Matrix(2, 2) << 1.0, 10.0, 100.0, 1000.0); - Vector b = (Vector(2) << error1, error2); + Matrix A = (Matrix(2, 2) << 1.0, 10.0, 100.0, 1000.0).finished(); + Vector b = (Vector(2) << error1, error2).finished(); const Robust::shared_ptr robust = Robust::Create( mEstimator::Huber::Create(k, mEstimator::Huber::Scalar), Unit::Create(2)); diff --git a/gtsam/linear/tests/testSampler.cpp b/gtsam/linear/tests/testSampler.cpp index 9db8b7edc..bd718500e 100644 --- a/gtsam/linear/tests/testSampler.cpp +++ b/gtsam/linear/tests/testSampler.cpp @@ -24,7 +24,7 @@ const double tol = 1e-5; /* ************************************************************************* */ TEST(testSampler, basic) { - Vector sigmas = (Vector(3) << 1.0, 0.1, 0.0); + Vector sigmas = Vector3(1.0, 0.1, 0.0); noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas); char seed = 'A'; Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1); diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index 5e08a7827..f93788af9 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((Vector(3) << 0.1, 0.2, 0.3)); +static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector3(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((Vector(3) << 0.0, 0.0, 0.1)); +static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector3(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, (Vector(1) << 1.0)); - values.insert(1, (Vector(2) << 2.0,3.0)); - values.insert(2, (Vector(2) << 4.0,5.0)); + values.insert(0, (Vector(1) << 1.0).finished()); + values.insert(1, Vector2(2.0,3.0)); + values.insert(2, Vector2(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((Vector(3) << 1.0, 2.0, 3.0)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(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 = (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); + Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished(); + Matrix A2 = (Matrix(2, 2) << 6., 0.2, 8., 0.4).finished(); + Matrix R = (Matrix(2, 2) << 0.1, 0.3, 0.0, 0.34).finished(); 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 = (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); + Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished(); + Matrix A2 = (Matrix(2, 2) << 6., 0.2, 8., 0.4).finished(); + Matrix R = (Matrix(2, 2) << 0.1, 0.3, 0.0, 0.34).finished(); 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((Vector(3) << 1.0, 2.0, 3.0)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(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, (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)); + (JacobianFactor(x2, (Matrix(1, 1) << 1.).finished(), x1, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)) + (JacobianFactor(x2, (Matrix(1, 1) << 1.).finished(), x3, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)) + (JacobianFactor(x3, (Matrix(1, 1) << 1.).finished(), x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)) + (JacobianFactor(x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)); GaussianBayesTree init = *chain.eliminateMultifrontal(chainOrdering); GaussianBayesTree expected = *chain.eliminateMultifrontal(chainOrdering); diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index 75114921b..949ec3a59 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -36,10 +36,10 @@ TEST(VectorValues, basics) // insert VectorValues actual; - 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)); + actual.insert(0, (Vector(1) << 1).finished()); + actual.insert(1, Vector2(2, 3)); + actual.insert(5, Vector2(6, 7)); + actual.insert(2, Vector2(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((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])); + EXPECT(assert_equal((Vector(1) << 1).finished(), actual[0])); + EXPECT(assert_equal(Vector2(2, 3), actual[1])); + EXPECT(assert_equal(Vector2(4, 5), actual[2])); + EXPECT(assert_equal(Vector2(6, 7), actual[5])); FastVector keys = list_of(0)(1)(2)(5); - EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7), actual.vector(keys))); + EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7).finished(), 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, (Vector(1) << 1)); - expected.insert(1, (Vector(2) << 2, 3)); - expected.insert(5, (Vector(2) << 6, 7)); - expected.insert(2, (Vector(2) << 4, 5)); + expected.insert(0, (Vector(1) << 1).finished()); + expected.insert(1, Vector2(2, 3)); + expected.insert(5, Vector2(6, 7)); + expected.insert(2, Vector2(4, 5)); VectorValues first; - first.insert(0, (Vector(1) << 1)); - first.insert(1, (Vector(2) << 2, 3)); + first.insert(0, (Vector(1) << 1).finished()); + first.insert(1, Vector2(2, 3)); VectorValues second; - second.insert(5, (Vector(2) << 6, 7)); - second.insert(2, (Vector(2) << 4, 5)); + second.insert(5, Vector2(6, 7)); + second.insert(2, Vector2(4, 5)); VectorValues actual(first, second); @@ -96,14 +96,14 @@ TEST(VectorValues, combine) TEST(VectorValues, subvector) { VectorValues init; - 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)); + init.insert(10, (Vector(1) << 1).finished()); + init.insert(11, Vector2(2, 3)); + init.insert(12, Vector2(4, 5)); + init.insert(13, Vector2(6, 7)); std::vector keys; keys += 10, 12, 13; - Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7); + Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7).finished(); EXPECT(assert_equal(expSubVector, init.vector(keys))); } @@ -111,16 +111,16 @@ TEST(VectorValues, subvector) TEST(VectorValues, LinearAlgebra) { VectorValues test1; - 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)); + test1.insert(0, (Vector(1) << 1).finished()); + test1.insert(1, Vector2(2, 3)); + test1.insert(5, Vector2(6, 7)); + test1.insert(2, Vector2(4, 5)); VectorValues test2; - 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)); + test2.insert(0, (Vector(1) << 6).finished()); + test2.insert(1, Vector2(1, 6)); + test2.insert(5, Vector2(4, 3)); + test2.insert(2, Vector2(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, (Vector(1) << 1)); - expected.insert(1, (Vector(2) << 2, 3)); - expected.insert(2, (Vector(2) << 4, 5)); - expected.insert(5, (Vector(2) << 6, 7)); + expected.insert(0, (Vector(1) << 1).finished()); + expected.insert(1, Vector2(2, 3)); + expected.insert(2, Vector2(4, 5)); + expected.insert(5, Vector2(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, (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)); + vv.insert(0, (Vector(1) << 1).finished()); + vv.insert(1, Vector2(2, 3)); + vv.insert(2, Vector2(4, 5)); + vv.insert(5, Vector2(6, 7)); + vv.insert(7, Vector2(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 f5b5fd234..bd321d51d 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((Vector(3) << 0.1,0.2,0.4)); - Vector bias_gyro((Vector(3) << -0.2, 0.5, 0.03)); + Vector bias_acc(Vector3(0.1,0.2,0.4)); + Vector bias_gyro(Vector3(-0.2, 0.5, 0.03)); // Default Constructor gtsam::imuBias::ConstantBias bias1; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 7508a593b..e5133b19d 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -167,9 +167,9 @@ TEST( ImuFactor, Error ) // Linearization point imuBias::ConstantBias bias; // Bias Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); - Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v1(Vector3(0.5, 0.0, 0.0)); Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); - Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -239,16 +239,16 @@ TEST( ImuFactor, ErrorWithBiases ) // Linearization point // Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); +// Vector3 v1(Vector3(0.5, 0.0, 0.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); +// Vector3 v2(Vector3(0.5, 0.0, 0.0)); imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v1(Vector3(0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -446,9 +446,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) //{ // // Linearization point // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); +// Vector3 v1(Vector3(0.5, 0.0, 0.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); +// Vector3 v2(Vector3(0.5, 0.0, 0.0)); // imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012)); // // // Pre-integrator @@ -502,9 +502,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v1(Vector3(0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index a98ef851f..c2e728a2b 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -122,8 +122,8 @@ struct GTSAM_EXPORT ISAM2Params { * entries would be added with: * \code FastMap thresholds; - thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold - thresholds['l'] = (Vector(3) << 1.0, 1.0, 1.0); // 1.0 m landmark position threshold + thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished(); // 0.1 rad rotation threshold, 0.5 m translation threshold + thresholds['l'] = Vector3(1.0, 1.0, 1.0); // 1.0 m landmark position threshold params.relinearizeThreshold = thresholds; \endcode */ diff --git a/gtsam/nonlinear/WhiteNoiseFactor.h b/gtsam/nonlinear/WhiteNoiseFactor.h index a7987d73b..5ce7fa7b3 100644 --- a/gtsam/nonlinear/WhiteNoiseFactor.h +++ b/gtsam/nonlinear/WhiteNoiseFactor.h @@ -74,11 +74,11 @@ namespace gtsam { Key j1, Key j2) { double e = u - z, e2 = e * e; double c = 2 * logSqrt2PI - log(p) + e2 * 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)); + Vector g1 = (Vector(1) << -e * p).finished(); + Vector g2 = (Vector(1) << 0.5 / p - 0.5 * e2).finished(); + Matrix G11 = (Matrix(1, 1) << p).finished(); + Matrix G12 = (Matrix(1, 1) << e).finished(); + Matrix G22 = (Matrix(1, 1) << 0.5 / (p * p)).finished(); return HessianFactor::shared_ptr( new HessianFactor(j1, j2, G11, G12, g1, G22, g2, c)); } @@ -136,7 +136,7 @@ namespace gtsam { * TODO: Where is this used? should disappear. */ virtual Vector unwhitenedError(const Values& x) const { - return (Vector(1) << std::sqrt(2 * error(x))); + return (Vector(1) << std::sqrt(2 * error(x))).finished(); } /** diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index fd70519dc..bd862ef94 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -19,7 +19,7 @@ using namespace std; using namespace boost::assign; using namespace gtsam; -const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 1.0)); +const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas(Vector2(1.0, 1.0)); const double tol = 1e-5; gtsam::Key l1 = 101, l2 = 102, x1 = 1, x2 = 2; @@ -32,11 +32,11 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) { Matrix A1 = (Matrix(2, 2) << 2.74222, -0.0067457, - 0.0, 2.63624); + 0.0, 2.63624).finished(); Matrix A2 = (Matrix(2, 2) << -0.0455167, -0.0443573, - -0.0222154, -0.102489); - Vector b = (Vector(2) << 0.0277052, + -0.0222154, -0.102489).finished(); + Vector b = Vector2(0.0277052, -0.0533393); JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2); @@ -66,11 +66,11 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { Matrix A1 = (Matrix(2, 2) << 2.74222, -0.0067457, - 0.0, 2.63624); + 0.0, 2.63624).finished(); Matrix A2 = (Matrix(2, 2) << -0.0455167, -0.0443573, - -0.0222154, -0.102489); - Vector b = (Vector(2) << 0.0277052, + -0.0222154, -0.102489).finished(); + Vector b = Vector2(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 = (Vector(2) << 1.0, 2.0); - Vector delta_l2 = (Vector(2) << 3.0, 4.0); + Vector delta_l1 = Vector2(1.0, 2.0); + Vector delta_l2 = Vector2(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 = (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 G11 = (Matrix(1, 1) << 1.0).finished(); + Matrix G12 = (Matrix(1, 2) << 2.0, 4.0).finished(); + Matrix G13 = (Matrix(1, 3) << 3.0, 6.0, 9.0).finished(); Matrix G22 = (Matrix(2, 2) << 3.0, 5.0, - 0.0, 6.0); + 0.0, 6.0).finished(); Matrix G23 = (Matrix(2, 3) << 4.0, 6.0, 8.0, - 1.0, 2.0, 4.0); + 1.0, 2.0, 4.0).finished(); Matrix G33 = (Matrix(3, 3) << 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, - 0.0, 0.0, 9.0); + 0.0, 0.0, 9.0).finished(); - Vector g1 = (Vector(1) << -7.0); - Vector g2 = (Vector(2) << -8.0, -9.0); - Vector g3 = (Vector(3) << 1.0, 2.0, 3.0); + Vector g1 = (Vector(1) << -7.0).finished(); + Vector g2 = Vector2(-8.0, -9.0); + Vector g3 = Vector3(1.0, 2.0, 3.0); double f = 10.0; @@ -161,18 +161,18 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { Matrix G11 = (Matrix(3, 3) << 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, - 0.0, 0.0, 9.0); + 0.0, 0.0, 9.0).finished(); Matrix G12 = (Matrix(3, 2) << 1.0, 2.0, 3.0, 5.0, - 4.0, 6.0); - Vector g1 = (Vector(3) << 1.0, 2.0, 3.0); + 4.0, 6.0).finished(); + Vector g1 = Vector3(1.0, 2.0, 3.0); Matrix G22 = (Matrix(2, 2) << 0.5, 0.2, - 0.0, 0.6); + 0.0, 0.6).finished(); - Vector g2 = (Vector(2) << -8.0, -9.0); + Vector g2 = Vector2(-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 = (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); + Vector delta_l1 = Vector2(1.0, 2.0); + Vector delta_x1 = Vector3(3.0, 4.0, 0.5); + Vector delta_x2 = Vector3(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((Vector(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).finished(), 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 = (Vector(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).finished(); 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 219cacfd1..09fe0f253 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -194,7 +194,7 @@ TEST(Values, basic_functions) //TEST(Values, dim_zero) //{ // Values config0; -// config0.insert(key1, Vector2((Vector(2) << 2.0, 3.0)); +// config0.insert(key1, Vector2(Vector2(2.0, 3.0)); // config0.insert(key2, Vector3(5.0, 6.0, 7.0)); // LONGS_EQUAL(5, config0.dim()); // @@ -212,8 +212,8 @@ TEST(Values, expmap_a) config0.insert(key2, Vector3(5.0, 6.0, 7.0)); VectorValues increment = pair_list_of - (key1, (Vector(3) << 1.0, 1.1, 1.2)) - (key2, (Vector(3) << 1.3, 1.4, 1.5)); + (key1, Vector3(1.0, 1.1, 1.2)) + (key2, Vector3(1.3, 1.4, 1.5)); Values expected; expected.insert(key1, Vector3(2.0, 3.1, 4.2)); @@ -230,7 +230,7 @@ TEST(Values, expmap_b) config0.insert(key2, Vector3(5.0, 6.0, 7.0)); VectorValues increment = pair_list_of - (key2, (Vector(3) << 1.3, 1.4, 1.5)); + (key2, Vector3(1.3, 1.4, 1.5)); Values expected; expected.insert(key1, Vector3(1.0, 2.0, 3.0)); @@ -283,8 +283,8 @@ TEST(Values, localCoordinates) valuesA.insert(key2, Vector3(5.0, 6.0, 7.0)); VectorValues expDelta = pair_list_of - (key1, (Vector(3) << 0.1, 0.2, 0.3)) - (key2, (Vector(3) << 0.4, 0.5, 0.6)); + (key1, Vector3(0.1, 0.2, 0.3)) + (key2, Vector3(0.4, 0.5, 0.6)); Values valuesB = valuesA.retract(expDelta); diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index cf5760695..ef02b5cb1 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -96,7 +96,7 @@ namespace gtsam { Vector e1 = Rot::Logmap(measuredBearing_.between(y1)); double y2 = pose.range(point, H21_, H22_); - Vector e2 = (Vector(1) << y2 - measuredRange_); + Vector e2 = (Vector(1) << y2 - measuredRange_).finished(); if (H1) *H1 = gtsam::stack(2, &H11, &H21); if (H2) *H2 = gtsam::stack(2, &H12, &H22); diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index 78b8abd1f..9cb1e3017 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -74,9 +74,9 @@ struct BoundingConstraint1: public NoiseModelFactor1 { } if (isGreaterThan_) - return (Vector(1) << error); + return (Vector(1) << error).finished(); else - return -1.0 * (Vector(1) << error); + return -1.0 * (Vector(1) << error).finished(); } private: @@ -147,9 +147,9 @@ struct BoundingConstraint2: public NoiseModelFactor2 { } if (isGreaterThan_) - return (Vector(1) << error); + return (Vector(1) << error).finished(); else - return -1.0 * (Vector(1) << error); + return -1.0 * (Vector(1) << error).finished(); } private: diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 97c8a541e..26580f41e 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -65,7 +65,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { linearGraph.add(key1, -I9, key2, M9, zero9, model); } // prior on the anchor orientation - linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0), model); + linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0).finished(), model); return linearGraph; } @@ -291,7 +291,7 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl double th = logRot.norm(); if(th != th){ // the second case means that th = nan (logRot does not work well for +/-pi) - Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)) ); // some perturbation + Rot3 R1pert = R1.compose( Rot3::Expmap(Vector3(0.01, 0.01, 0.01)) ); // some perturbation logRot = Rot3::Logmap(R1pert.between(R2)); th = logRot.norm(); } diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index fe140a298..af1c1a1bd 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -73,7 +73,7 @@ namespace gtsam { } else { hx = pose.range(point, H1, H2); } - return (Vector(1) << hx - measured_); + return (Vector(1) << hx - measured_).finished(); } /** return the measured */ diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index 3b40a0ba6..ea8811d17 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -55,7 +55,7 @@ public: // predict p_ as q = R*z_, derivative H will be filled if not none Point3 q = R.rotate(z_,H); // error is just difference, and note derivative of that wrpt q is I3 - return Vector(3) << q.x()-p_.x(), q.y()-p_.y(), q.z()-p_.z(); + return (Vector(3) << q.x()-p_.x(), q.y()-p_.y(), q.z()-p_.z()).finished(); } }; diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index b6f04d449..a6fb2d830 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -313,7 +313,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, // Create noise model noiseModel::Diagonal::shared_ptr measurementNoise = - noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); + noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std).finished()); // Add to graph *graph += BearingRangeFactor(id1, L(id2), bearing, range, diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index 1f5d0f2df..b2528b9d8 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -35,9 +35,9 @@ static const Matrix I3 = eye(3); static const Key keyAnchor = symbol('Z', 9999999); static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = - noiseModel::Diagonal::Sigmas((Vector(1) << 0)); + noiseModel::Diagonal::Sigmas((Vector(1) << 0).finished()); static const noiseModel::Diagonal::shared_ptr priorPose2Noise = - noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); + noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); /* ************************************************************************* */ /** @@ -143,7 +143,7 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, if (!pose2Between) throw invalid_argument( "buildLinearOrientationGraph: invalid between factor!"); - deltaTheta = (Vector(1) << pose2Between->measured().theta()); + deltaTheta = (Vector(1) << pose2Between->measured().theta()).finished(); // Retrieve the noise model for the relative rotation SharedNoiseModel model = pose2Between->get_noiseModel(); @@ -152,7 +152,7 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, if (!diagonalModel) throw invalid_argument("buildLinearOrientationGraph: invalid noise model " "(current version assumes diagonal noise model)!"); - Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2)); // std on the angular measurement + Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2)).finished(); // std on the angular measurement model_deltaTheta = noiseModel::Diagonal::Sigmas(std_deltaTheta); } @@ -185,11 +185,11 @@ GaussianFactorGraph buildLinearOrientationGraph( double k = boost::math::round(k2pi_noise / (2 * M_PI)); //if (k2pi_noise - 2*k*M_PI > 1e-5) cout << k2pi_noise - 2*k*M_PI << endl; // for debug Vector deltaThetaRegularized = (Vector(1) - << key1_DeltaTheta_key2 - 2 * k * M_PI); + << key1_DeltaTheta_key2 - 2 * k * M_PI).finished(); lagoGraph.add(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta); } // prior on the anchor orientation - lagoGraph.add(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise); + lagoGraph.add(keyAnchor, I, (Vector(1) << 0.0).finished(), priorOrientationNoise); return lagoGraph; } @@ -321,8 +321,8 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, double dy = pose2Between->measured().y(); Vector globalDeltaCart = // - (Vector(2) << c1 * dx - s1 * dy, s1 * dx + c1 * dy); - Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot); // rhs + (Vector(2) << c1 * dx - s1 * dy, s1 * dx + c1 * dy).finished(); + Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot).finished(); // rhs Matrix J1 = -I3; J1(0, 2) = s1 * dx + c1 * dy; J1(1, 2) = -c1 * dx + s1 * dy; @@ -338,7 +338,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, } } // add prior - linearPose2graph.add(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0), + linearPose2graph.add(keyAnchor, I3, Vector3(0.0, 0.0, 0.0), priorPose2Noise); // optimize diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index a40ddce3d..d6a5ffa8c 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -110,7 +110,7 @@ TEST( dataSet, readG2o) expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934)); EXPECT(assert_equal(expectedValues,*actualValues,1e-5)); - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699)); NonlinearFactorGraph expectedGraph; expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); @@ -159,7 +159,7 @@ TEST( dataSet, readG2o3D) EXPECT(assert_equal(expectedValues,*actualValues,1e-5)); - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(6) << 10000.0,10000.0,10000.0,10000.0,10000.0,10000.0)); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(6) << 10000.0,10000.0,10000.0,10000.0,10000.0,10000.0).finished()); NonlinearFactorGraph expectedGraph; Point3 p01 = Point3(1.001367, 0.015390, 0.004948); @@ -238,7 +238,7 @@ TEST( dataSet, readG2oHuber) bool is3D = false; boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeHUBER); - noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); + noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699)); SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel); NonlinearFactorGraph expectedGraph; @@ -266,7 +266,7 @@ TEST( dataSet, readG2oTukey) bool is3D = false; boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY); - noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); + noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699)); SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel); NonlinearFactorGraph expectedGraph; diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 46e283d34..e8acb0db0 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -71,7 +71,7 @@ TEST (EssentialMatrixFactor, testData) { EXPECT(assert_equal(Point2(-1, 0.2), pB(4), 1e-8)); // Check homogeneous version - EXPECT(assert_equal((Vector(3) << -1, 0.2, 1), vB(4), 1e-8)); + EXPECT(assert_equal(Vector3(-1, 0.2, 1), vB(4), 1e-8)); // Check epipolar constraint for (size_t i = 0; i < 5; i++) @@ -126,7 +126,7 @@ TEST (EssentialMatrixFactor, minimization) { // Check error at initial estimate Values initial; EssentialMatrix initialE = trueE.retract( - (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1)); + (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); @@ -341,7 +341,7 @@ TEST (EssentialMatrixFactor, extraMinimization) { // Check error at initial estimate Values initial; EssentialMatrix initialE = trueE.retract( - (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1)); + (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index a7c91de3f..8adbaa62d 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 = (Vector(2) << 323.,240.); + Vector z = Vector2(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(((Vector) (Vector(2) << -3.0, 0.0)), factor->unwhitenedError(values))); + EXPECT(assert_equal(((Vector) Vector2(-3.0, 0.0)), factor->unwhitenedError(values))); } static const double baseline = 5.0 ; @@ -315,7 +315,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { focal_noise, focal_noise, // f_x, f_y skew_noise, // s trans_noise, trans_noise // ux, uy - ) ; + ).finished(); values.insert(X(i), cameras[i].retract(delta)) ; } } diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index a0377f087..9cc634b37 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 = (Vector(2) << 323.,240.); + Vector z = Vector2(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((Vector(2) << -3.0, 0.0), factor->unwhitenedError(values))); + EXPECT(assert_equal(Vector2(-3.0, 0.0), factor->unwhitenedError(values))); } @@ -312,7 +312,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) { rot_noise, rot_noise, rot_noise, // rotation trans_noise, trans_noise, trans_noise, // translation focal_noise, distort_noise, distort_noise // f, k1, k2 - ) ; + ).finished(); values.insert(X(i), cameras[i].retract(delta)) ; } } diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 0cea2cead..61c4566bf 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -47,13 +47,13 @@ namespace simple { // x0 // static Point3 p0 = Point3(0,0,0); -static Rot3 R0 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,0.0 ) ); +static Rot3 R0 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,0.0 ).finished() ); static Point3 p1 = Point3(1,2,0); -static Rot3 R1 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,1.570796 ) ); +static Rot3 R1 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,1.570796 ).finished() ); static Point3 p2 = Point3(0,2,0); -static Rot3 R2 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,3.141593 ) ); +static Rot3 R2 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,3.141593 ).finished() ); static Point3 p3 = Point3(-1,1,0); -static Rot3 R3 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,4.712389 ) ); +static Rot3 R3 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,4.712389 ).finished() ); static Pose3 pose0 = Pose3(R0,p0); static Pose3 pose1 = Pose3(R1,p1); @@ -145,7 +145,7 @@ TEST( InitializePose3, iterationGradient ) { NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); // Wrong initial guess - initialization should fix the rotations - Rot3 Rpert = Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)); + Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); Values givenPoses; givenPoses.insert(x0,simple::pose0); givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) )); @@ -158,25 +158,25 @@ TEST( InitializePose3, iterationGradient ) { Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281, 0.033572116359134, 0.999436104312325, -0.000621610948719, - -0.000983333645009, 0.000654992453817, 0.999999302019670); + -0.000983333645009, 0.000654992453817, 0.999999302019670).finished(); Rot3 R0Expected = Rot3(M0); EXPECT(assert_equal(R0Expected, orientations.at(x0), 1e-5)); Matrix M1 = (Matrix(3,3) << 0.999905367545392, -0.010866391403031, 0.008436675399114, 0.010943459008004, 0.999898317528125, -0.009143047050380, - -0.008336465609239, 0.009234508232789, 0.999922610604863); + -0.008336465609239, 0.009234508232789, 0.999922610604863).finished(); Rot3 R1Expected = Rot3(M1); EXPECT(assert_equal(R1Expected, orientations.at(x1), 1e-5)); Matrix M2 = (Matrix(3,3) << 0.998936644682875, 0.045376417678595, -0.008158469732553, -0.045306446926148, 0.998936408933058, 0.008566024448664, - 0.008538487960253, -0.008187284445083, 0.999930028850403); + 0.008538487960253, -0.008187284445083, 0.999930028850403).finished(); Rot3 R2Expected = Rot3(M2); EXPECT(assert_equal(R2Expected, orientations.at(x2), 1e-5)); Matrix M3 = (Matrix(3,3) << 0.999898767273093, -0.010834701971459, 0.009223038487275, 0.010911315499947, 0.999906044037258, -0.008297366559388, - -0.009132272433995, 0.008397162077148, 0.999923041673329); + -0.009132272433995, 0.008397162077148, 0.999923041673329).finished(); Rot3 R3Expected = Rot3(M3); EXPECT(assert_equal(R3Expected, orientations.at(x3), 1e-5)); } @@ -186,7 +186,7 @@ TEST( InitializePose3, orientationsGradient ) { NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); // Wrong initial guess - initialization should fix the rotations - Rot3 Rpert = Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)); + Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); Values givenPoses; givenPoses.insert(x0,simple::pose0); givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) )); diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index 95455f078..2d1793417 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -129,11 +129,11 @@ TEST( Lago, regularizedMeasurements ) { GaussianFactorGraph lagoGraph = lago::buildLinearOrientationGraph(spanningTreeIds, chordsIds, g, orientationsToRoot, tree); std::pair actualAb = lagoGraph.jacobian(); // jacobian corresponding to the orientation measurements (last entry is the prior on the anchor and is disregarded) - Vector actual = (Vector(5) << actualAb.second(0),actualAb.second(1),actualAb.second(2),actualAb.second(3),actualAb.second(4)); + Vector actual = (Vector(5) << actualAb.second(0),actualAb.second(1),actualAb.second(2),actualAb.second(3),actualAb.second(4)).finished(); // this is the whitened error, so we multiply by the std to unwhiten actual = 0.1 * actual; // Expected regularized measurements (same for the spanning tree, corrected for the chordsIds) - Vector expected = (Vector(5) << M_PI/2, M_PI, -M_PI/2, M_PI/2 - 2*M_PI , M_PI/2); + Vector expected = (Vector(5) << M_PI/2, M_PI, -M_PI/2, M_PI/2 - 2*M_PI , M_PI/2).finished(); EXPECT(assert_equal(expected, actual, 1e-6)); } @@ -144,10 +144,10 @@ TEST( Lago, smallGraphVectorValues ) { VectorValues initial = lago::initializeOrientations(simpleLago::graph(), useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); - EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI).finished(), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI).finished(), initial.at(x3), 1e-6)); } /* *************************************************************************** */ @@ -156,10 +156,10 @@ TEST( Lago, smallGraphVectorValuesSP ) { VectorValues initial = lago::initializeOrientations(simpleLago::graph()); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); - EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ).finished(), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ).finished(), initial.at(x3), 1e-6)); } /* *************************************************************************** */ @@ -170,10 +170,10 @@ TEST( Lago, multiplePosePriors ) { VectorValues initial = lago::initializeOrientations(g, useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); - EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI).finished(), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI).finished(), initial.at(x3), 1e-6)); } /* *************************************************************************** */ @@ -183,10 +183,10 @@ TEST( Lago, multiplePosePriorsSP ) { VectorValues initial = lago::initializeOrientations(g); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); - EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ).finished(), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ).finished(), initial.at(x3), 1e-6)); } /* *************************************************************************** */ @@ -197,10 +197,10 @@ TEST( Lago, multiplePoseAndRotPriors ) { VectorValues initial = lago::initializeOrientations(g, useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); - EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI).finished(), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI).finished(), initial.at(x3), 1e-6)); } /* *************************************************************************** */ @@ -210,10 +210,10 @@ TEST( Lago, multiplePoseAndRotPriorsSP ) { VectorValues initial = lago::initializeOrientations(g); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); - EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ).finished(), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ).finished(), initial.at(x3), 1e-6)); } /* *************************************************************************** */ @@ -265,7 +265,7 @@ TEST( Lago, largeGraphNoisy_orientations ) { // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = *g; - noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); + noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances(Vector3(1e-2, 1e-2, 1e-4)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); VectorValues actualVV = lago::initializeOrientations(graphWithPrior); @@ -300,7 +300,7 @@ TEST( Lago, largeGraphNoisy ) { // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = *g; - noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); + noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances(Vector3(1e-2, 1e-2, 1e-4)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); Values actual = lago::initialize(graphWithPrior); diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index cbe8efa30..9165ff17a 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((Vector(1) << 0.1)); -const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3)); +const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas((Vector(1) << 0.1).finished()); +const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas(Vector3(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((Vector(3) << 0.1, 0.2, 0.3)); +const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap(Vector3(0.1, 0.2, 0.3)); // Pose2 examples const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); @@ -63,9 +63,9 @@ TEST( testPoseRotationFactor, level3_error ) { Pose3RotationPrior factor(poseKey, rot3C, model3); Matrix actH1; #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT(assert_equal((Vector(3) << -0.1, -0.2,-0.3), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Vector3(-0.1, -0.2,-0.3), factor.evaluateError(pose1, actH1))); #else - EXPECT(assert_equal((Vector(3) << -0.1, -0.2, -0.3), factor.evaluateError(pose1, actH1),1e-2)); + EXPECT(assert_equal(Vector3(-0.1, -0.2, -0.3), factor.evaluateError(pose1, actH1),1e-2)); #endif Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); // the derivative is more complex, but is close to the identity for Rot3 around the origin @@ -88,7 +88,7 @@ TEST( testPoseRotationFactor, level2_error ) { Pose2 pose1(rot2A, point2A); Pose2RotationPrior factor(poseKey, rot2B, model1); Matrix actH1; - EXPECT(assert_equal((Vector(1) << -M_PI_2), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vector(1) << -M_PI_2).finished(), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -98,7 +98,7 @@ TEST( testPoseRotationFactor, level2_error_wrap ) { Pose2 pose1(rot2C, point2A); Pose2RotationPrior factor(poseKey, rot2D, model1); Matrix actH1; - EXPECT(assert_equal((Vector(1) << -0.02), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vector(1) << -0.02).finished(), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testPoseTranslationPrior.cpp b/gtsam/slam/tests/testPoseTranslationPrior.cpp index 47ff708d9..f7bd412fe 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((Vector(2) << 0.1, 0.2)); -const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3)); +const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); +const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)); typedef PoseTranslationPrior Pose2TranslationPrior; typedef PoseTranslationPrior Pose3TranslationPrior; @@ -58,7 +58,7 @@ TEST( testPoseTranslationFactor, level3_error ) { Pose3 pose1(rot3A, point3A); Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; - EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Vector3(-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -78,7 +78,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) { Pose3 pose1(rot3B, point3A); Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; - EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Vector3(-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -98,7 +98,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) { Pose3 pose1(rot3C, point3A); Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; - EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Vector3(-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -118,7 +118,7 @@ TEST( testPoseTranslationFactor, level2_error ) { Pose2 pose1(rot2A, point2A); Pose2TranslationPrior factor(poseKey, point2B, model2); Matrix actH1; - EXPECT(assert_equal((Vector(2) << -3.0,-4.0), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Vector2(-3.0,-4.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index b3454269e..1c8f3b8ed 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 = (Vector(2) << -3.0, 0.0); + Vector expectedError = Vector2(-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 = (Vector(2) << -3.0, 0.0); + Vector expectedError = Vector2(-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 = (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.); + Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.).finished(); + Matrix H2Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished(); // 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 = (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); + Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376).finished(); + Matrix H2Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished(); // 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 cba9300f5..6bffa3ce9 100644 --- a/gtsam/slam/tests/testRangeFactor.cpp +++ b/gtsam/slam/tests/testRangeFactor.cpp @@ -117,7 +117,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 = (Vector(1) << 0.295630141); + Vector expectedError = (Vector(1) << 0.295630141).finished(); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -142,7 +142,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 = (Vector(1) << 0.295630141); + Vector expectedError = (Vector(1) << 0.295630141).finished(); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -164,7 +164,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 = (Vector(1) << 0.295630141); + Vector expectedError = (Vector(1) << 0.295630141).finished(); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -189,7 +189,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 = (Vector(1) << 0.295630141); + Vector expectedError = (Vector(1) << 0.295630141).finished(); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index b9d3e996f..e96c8101c 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -34,7 +34,7 @@ const Matrix26 F3 = 3 * Matrix26::Ones(); const vector > Fblocks = list_of > // (make_pair(0, F0))(make_pair(1, F1))(make_pair(3, F3)); // RHS and sigmas -const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.); +const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished(); //************************************************************************************* TEST( regularImplicitSchurFactor, creation ) { diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index 15e8cf984..2a2e3b40f 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -58,11 +58,11 @@ TEST (RotateFactor, test) { RotateFactor f(1, i1Ri2, c1Zc2, model); EXPECT(assert_equal(zero(3), f.evaluateError(iRc), 1e-8)); - Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1)); + Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1)); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - Vector expectedE = (Vector(3) << -0.0248752, 0.202981, -0.0890529); + Vector expectedE = Vector3(-0.0248752, 0.202981, -0.0890529); #else - Vector expectedE = (Vector(3) << -0.0246305, 0.20197, -0.08867); + Vector expectedE = Vector3(-0.0246305, 0.20197, -0.08867); #endif EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5)); @@ -99,7 +99,7 @@ TEST (RotateFactor, minimization) { // Check error at initial estimate Values initial; double degree = M_PI / 180; - Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20)); + Rot3 initialE = iRc.retract(degree * Vector3(20, -20, 20)); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) @@ -128,12 +128,12 @@ TEST (RotateDirectionsFactor, test) { RotateDirectionsFactor f(1, p1, z1, model); EXPECT(assert_equal(zero(2), f.evaluateError(iRc), 1e-8)); - Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1)); + Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1)); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - Vector expectedE = (Vector(2) << -0.0890529, -0.202981); + Vector expectedE = Vector2(-0.0890529, -0.202981); #else - Vector expectedE = (Vector(2) << -0.08867, -0.20197); + Vector expectedE = Vector2(-0.08867, -0.20197); #endif EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5)); @@ -173,7 +173,7 @@ TEST (RotateDirectionsFactor, minimization) { // Check error at initial estimate Values initial; double degree = M_PI / 180; - Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20)); + Rot3 initialE = iRc.retract(degree * Vector3(20, -20, 20)); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 9411e3ccc..b6d2e2ef8 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -35,7 +35,7 @@ static Pose3 camera1((Matrix) (Matrix(3, 3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. - ), + ).finished(), Point3(0,0,6.25)); static boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); @@ -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 = (Vector(3) << -3.0, +2.0, -1.0); + Vector expectedError = Vector3(-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 = (Vector(3) << -3.0, +2.0, -1.0); + Vector expectedError = Vector3(-3.0, +2.0, -1.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -146,10 +146,10 @@ TEST( StereoFactor, Jacobian ) { // The expected Jacobians 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); + 625.0, 0.0, 0.0, 0.0, -100.0, 0.0).finished(); Matrix H2Expected = (Matrix(3, 3) << 100.0, 0.0, 0.0, 100.0, 0.0, 8.0, - 0.0, 100.0, 0.0); + 0.0, 100.0, 0.0).finished(); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); @@ -174,10 +174,10 @@ TEST( StereoFactor, JacobianWithTransform ) { // The expected Jacobians 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); + -10.0, -650.0, 0.0, 0.0, 0.0, 100.0).finished(); Matrix H2Expected = (Matrix(3, 3) << 0.0, -100.0, 0.0, 8.0, -100.0, 0.0, - 0.0, 0.0, -100.0); + 0.0, 0.0, -100.0).finished(); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); diff --git a/gtsam_unstable/base/tests/testFixedVector.cpp b/gtsam_unstable/base/tests/testFixedVector.cpp index e5e297b7c..1bae52a80 100644 --- a/gtsam_unstable/base/tests/testFixedVector.cpp +++ b/gtsam_unstable/base/tests/testFixedVector.cpp @@ -28,7 +28,7 @@ static const double tol = 1e-9; /* ************************************************************************* */ TEST( testFixedVector, conversions ) { double data1[] = {1.0, 2.0, 3.0}; - Vector v1 = (Vector(3) << data1[0], data1[1], data1[2]); + Vector v1 = (Vector(3) << data1[0], data1[1], data1[2]).finished(); TestVector3 fv1(v1), fv2(data1); Vector actFv2(fv2); @@ -37,7 +37,7 @@ TEST( testFixedVector, conversions ) { /* ************************************************************************* */ TEST( testFixedVector, variable_constructor ) { - TestVector3 act((Vector(3) << 1.0, 2.0, 3.0)); + TestVector3 act(Vector3(1.0, 2.0, 3.0)); EXPECT_DOUBLES_EQUAL(1.0, act(0), tol); EXPECT_DOUBLES_EQUAL(2.0, act(1), tol); EXPECT_DOUBLES_EQUAL(3.0, act(2), tol); @@ -45,9 +45,9 @@ TEST( testFixedVector, variable_constructor ) { /* ************************************************************************* */ TEST( testFixedVector, equals ) { - TestVector3 vec1((Vector(3) << 1.0, 2.0, 3.0)), vec2((Vector(3) << 1.0, 2.0, 3.0)), - vec3((Vector(3) << 2.0, 3.0, 4.0)); - TestVector5 vec4((Vector(5) << 1.0, 2.0, 3.0, 4.0, 5.0)); + TestVector3 vec1(Vector3(1.0, 2.0, 3.0)), vec2(Vector3(1.0, 2.0, 3.0)), + vec3(Vector3(2.0, 3.0, 4.0)); + TestVector5 vec4((Vector(5) << 1.0, 2.0, 3.0, 4.0, 5.0).finished()); EXPECT(assert_equal(vec1, vec1, tol)); EXPECT(assert_equal(vec1, vec2, tol)); @@ -61,23 +61,23 @@ TEST( testFixedVector, equals ) { /* ************************************************************************* */ TEST( testFixedVector, static_constructors ) { TestVector3 actZero = TestVector3::zero(); - TestVector3 expZero((Vector(3) << 0.0, 0.0, 0.0)); + TestVector3 expZero(Vector3(0.0, 0.0, 0.0)); EXPECT(assert_equal(expZero, actZero, tol)); TestVector3 actOnes = TestVector3::ones(); - TestVector3 expOnes((Vector(3) << 1.0, 1.0, 1.0)); + TestVector3 expOnes(Vector3(1.0, 1.0, 1.0)); EXPECT(assert_equal(expOnes, actOnes, tol)); TestVector3 actRepeat = TestVector3::repeat(2.3); - TestVector3 expRepeat((Vector(3) << 2.3, 2.3, 2.3)); + TestVector3 expRepeat(Vector3(2.3, 2.3, 2.3)); EXPECT(assert_equal(expRepeat, actRepeat, tol)); TestVector3 actBasis = TestVector3::basis(1); - TestVector3 expBasis((Vector(3) << 0.0, 1.0, 0.0)); + TestVector3 expBasis(Vector3(0.0, 1.0, 0.0)); EXPECT(assert_equal(expBasis, actBasis, tol)); TestVector3 actDelta = TestVector3::delta(1, 2.3); - TestVector3 expDelta((Vector(3) << 0.0, 2.3, 0.0)); + TestVector3 expDelta(Vector3(0.0, 2.3, 0.0)); EXPECT(assert_equal(expDelta, actDelta, tol)); } diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index 5fb4d77aa..73fc58fa6 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -68,7 +68,7 @@ public: // access const Vector3& gyro() const { return gyro_; } const Vector3& accel() const { return accel_; } - Vector6 z() const { return (Vector6() << accel_, gyro_);} + Vector6 z() const { return (Vector6() << accel_, gyro_).finished(); } /** * Error evaluation with optional derivatives - calculates diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h index 0100e17c7..ecc43ad79 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/gtsam_unstable/dynamics/Pendulum.h @@ -53,7 +53,7 @@ public: if (H1) *H1 = -eye(p); if (H2) *H2 = eye(p); if (H3) *H3 = eye(p)*h_; - return (Vector(1) << qk+v*h_-qk1); + return (Vector(1) << qk+v*h_-qk1).finished(); } }; // \PendulumFactor1 @@ -101,7 +101,7 @@ public: if (H1) *H1 = -eye(p); if (H2) *H2 = eye(p); if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q); - return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1); + return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1).finished(); } }; // \PendulumFactor2 @@ -158,7 +158,7 @@ public: if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid)); if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid)); - return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk); + return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk).finished(); } }; // \PendulumFactorPk @@ -214,7 +214,7 @@ public: if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid)); if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid)); - return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1); + return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1).finished(); } }; // \PendulumFactorPk1 diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index ed8d54696..06aa4ac24 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -67,7 +67,7 @@ PoseRTV PoseRTV::Expmap(const Vector9& v) { Vector9 PoseRTV::Logmap(const PoseRTV& p) { Vector6 Lx = Pose3::Logmap(p.Rt_); Vector3 Lv = Velocity3::Logmap(p.v_); - return (Vector9() << Lx, Lv); + return (Vector9() << Lx, Lv).finished(); } /* ************************************************************************* */ @@ -85,7 +85,7 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1) const { // First order approximation Vector6 poseLogmap = x0.localCoordinates(x1); Vector3 lv = rotation().unrotate(p1.velocity() - v_).vector(); - return (Vector(9) << poseLogmap, lv); + return (Vector(9) << poseLogmap, lv).finished(); } /* ************************************************************************* */ @@ -124,7 +124,7 @@ PoseRTV PoseRTV::planarDynamics(double vel_rate, double heading_rate, const Velocity3& v1 = v(); // Update vehicle heading - Rot3 r2 = r1.retract((Vector(3) << 0.0, 0.0, heading_rate * dt)); + Rot3 r2 = r1.retract((Vector(3) << 0.0, 0.0, heading_rate * dt).finished()); const double yaw2 = r2.ypr()(0); // Update vehicle position @@ -148,7 +148,7 @@ PoseRTV PoseRTV::flyingDynamics( const Velocity3& v1 = v(); // Update vehicle heading (and normalise yaw) - Vector rot_rates = (Vector(3) << 0.0, pitch_rate, heading_rate); + Vector rot_rates = (Vector(3) << 0.0, pitch_rate, heading_rate).finished(); Rot3 r2 = r1.retract(rot_rates*dt); // Work out dynamics on platform @@ -162,9 +162,9 @@ PoseRTV PoseRTV::flyingDynamics( Rot3 yaw_correction_bn = Rot3::yaw(yaw2); 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 * (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 + yaw_correction_bn.rotate(forward).vector() // applies locally forward force in the global frame + - drag * (Vector(3) << v1.x(), v1.y(), 0.0).finished() // drag term dependent on v1 + + delta(3, 2, loss_lift - lift_control); // falling due to lift lost from pitch // Update Vehicle Position and Velocity Velocity3 v2 = v1 + Velocity3(Acc_n * dt); diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index 06d485a88..b1c254a9e 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -125,7 +125,7 @@ public: Matrix D_gravityBody_gk; Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk, boost::none); - Vector f_ext = (Vector(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()).finished(); Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext; diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index 1a432ba1e..a92ae0426 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -44,7 +44,7 @@ public: if (H1) *H1 = eye(p); if (H2) *H2 = -eye(p); if (H3) *H3 = eye(p)*dt_; - return (Vector(1) << x1+v*dt_-x2); + return (Vector(1) << x1+v*dt_-x2).finished(); } private: diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 550b0e33c..56850a0fb 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)(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)); + pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), (Vector)Vector3(2.0, 2.0, 0.0)), + pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), (Vector)Vector3(0.0, 0.0, 0.0)), + pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), (Vector)Vector3(2.0, 2.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Unit::Create(6); @@ -102,9 +102,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)(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)); + pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)Vector3(1.0, 0.0, 0.0)), + pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)Vector3(1.0, 0.0, 0.0)), + pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)Vector3(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 e024ce884..0261257be 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 = (Vector(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).finished(); 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 = (Vector(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).finished(); 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 = (Vector(3) << 0.2, 0.0, 0.0), gyro = (Vector(3) << 0.0, 0.0, 0.2); + Vector accel = Vector3(0.2, 0.0, 0.0), gyro = Vector3(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((Vector(3) << 0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol)); + EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(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((Vector(3) << 0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol)); + EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(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 475d5285c..bdc71fea2 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -18,19 +18,19 @@ const double h = 0.01; //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)); -//Vector6 v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0)); -Vector6 V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0)); +//Vector6 v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0).finished()); +Vector6 V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0).finished()); Vector6 V1_g1 = g1.inverse().Adjoint(V1_w); Pose3 g2(g1.retract(h*V1_g1, Pose3::EXPMAP)); //Vector6 v2 = Pose3::Logmap(g1.between(g2)); double mass = 100.0; -Vector gamma2 = (Vector(2) << 0.0, 0.0); // no shape -Vector u2 = (Vector(2) << 0.0, 0.0); // no control at time 2 +Vector gamma2 = Vector2(0.0, 0.0); // no shape +Vector u2 = Vector2(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((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)); +Matrix Mass = diag((Vector(3) << mass, mass, mass).finished()); +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).finished()); Vector computeFu(const Vector& gamma, const Vector& control) { double gamma_r = gamma(0), gamma_p = gamma(1); @@ -41,7 +41,7 @@ Vector computeFu(const Vector& gamma, const Vector& control) { sin(gamma_p)*cos(gamma_r), 0.0, -sin(gamma_r), -1.0, cos(gamma_p)*sin(gamma_r), 0.0 - ); + ).finished(); return F*control; } @@ -104,7 +104,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((Vector(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.).finished()); 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 7188463b4..7e4802393 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)(Vector(3) << 1.0, 0.0, 0.0)), + pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), (Vector)Vector3(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)(Vector(3) << 1.0, 0.0, 0.0)); + pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), (Vector)Vector3(1.0, 0.0, 0.0)); /* ************************************************************************* */ TEST( testVelocityConstraint, trapezoidal ) { diff --git a/gtsam_unstable/examples/ConcurrentCalibration.cpp b/gtsam_unstable/examples/ConcurrentCalibration.cpp index 710dd4a10..b44b14b63 100644 --- a/gtsam_unstable/examples/ConcurrentCalibration.cpp +++ b/gtsam_unstable/examples/ConcurrentCalibration.cpp @@ -60,7 +60,7 @@ int main(int argc, char** argv){ initial_estimate.insert(Symbol('K', 0), *noisy_K); - noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100)); + noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished()); graph.push_back(PriorFactor(Symbol('K', 0), *noisy_K, calNoise)); diff --git a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp index 3c9b8d129..f2fa1b31b 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((Vector(3) << 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(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((Vector(3) << 0.1, 0.1, 0.05)); + noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(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((Vector(3) << 0.05, 0.05, 0.05)); + noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(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((Vector(3) << 0.5, 0.5, 0.25)); + noiseModel::Diagonal::shared_ptr loopNoise = noiseModel::Diagonal::Sigmas(Vector3(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((Vector(3) << 0.1, 0.1, 0.05)); + noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(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((Vector(3) << 0.05, 0.05, 0.05)); + noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(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((Vector(3) << 0.1, 0.1, 0.05)); + noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(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((Vector(3) << 0.05, 0.05, 0.05)); + noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(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 0980630ad..ea1381bab 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((Vector(3) << 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(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((Vector(3) << 0.1, 0.1, 0.05)); + noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(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((Vector(3) << 0.05, 0.05, 0.05)); + noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(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/examples/Pose2SLAMExampleExpressions.cpp b/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp index ac43fa428..3a3b97b77 100644 --- a/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp +++ b/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp @@ -41,11 +41,11 @@ int main(int argc, char** argv) { Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5); // 2a. Add a prior on the first pose, setting it to the origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.push_back(ExpressionFactor(priorNoise, Pose2(0, 0, 0), x1)); // For simplicity, we will use the same noise model for odometry and loop closures - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors graph.push_back(ExpressionFactor(model, Pose2(2, 0, 0 ), between(x1,x2))); diff --git a/gtsam_unstable/examples/SFMExampleExpressions.cpp b/gtsam_unstable/examples/SFMExampleExpressions.cpp index 1537af1d9..941f3dcb8 100644 --- a/gtsam_unstable/examples/SFMExampleExpressions.cpp +++ b/gtsam_unstable/examples/SFMExampleExpressions.cpp @@ -55,7 +55,7 @@ int main(int argc, char* argv[]) { NonlinearFactorGraph graph; // Specify uncertainty on first pose prior - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // Here we don't use a PriorFactor but directly the ExpressionFactor class // The object x0 is an Expression, and we create a factor wanting it to be equal to poses[0] diff --git a/gtsam_unstable/geometry/BearingS2.cpp b/gtsam_unstable/geometry/BearingS2.cpp index 3e1af8eb3..71278789c 100644 --- a/gtsam_unstable/geometry/BearingS2.cpp +++ b/gtsam_unstable/geometry/BearingS2.cpp @@ -32,7 +32,7 @@ BearingS2 BearingS2::fromDownwardsObservation(const Pose3& A, const Point3& B) { Matrix Cbc = (Matrix(3,3) << 0.,0.,1., 0.,1.,0., - -1.,0.,0.); + -1.,0.,0.).finished(); // p_rel_c = Cbc*Cnb*(PosObj - Pos); Vector p_rel_c = Cbc*Cnb*(B.vector() - A.translation().vector()); @@ -70,7 +70,7 @@ BearingS2 BearingS2::retract(const Vector& v) const { /* ************************************************************************* */ Vector BearingS2::localCoordinates(const BearingS2& x) const { return (Vector(2) << azimuth_.localCoordinates(x.azimuth_)(0), - elevation_.localCoordinates(x.elevation_)(0)); + elevation_.localCoordinates(x.elevation_)(0)).finished(); } } // \namespace gtsam diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 23d698d86..9347b9ffb 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -133,7 +133,7 @@ public: *H2 = J2 * (Matrix(3, 5) << H11, H12, H13, H14, H15, H21, H22, H23, H24, H25, - H31, H32, H33, H34, H35); + H31, H32, H33, H34, H35).finished(); } if(H3) { double H16 = -cos_phi*cos_theta/rho2; @@ -142,7 +142,7 @@ public: *H3 = J2 * (Matrix(3, 1) << H16, H26, - H36); + H36).finished(); } return uv; } @@ -162,7 +162,7 @@ public: gtsam::Point3 ray = pw - pt; double theta = atan2(ray.y(), ray.x()); // longitude double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); - return std::make_pair(Vector5((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)), + return std::make_pair((Vector5() << pt.x(),pt.y(),pt.z(), theta, phi).finished(), double(1./depth)); } diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index 15fd47236..f60ea107c 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -145,7 +145,7 @@ SimPolygon2D SimPolygon2D::randomTriangle( // extend line by random dist and angle to get BC double dAB = randomDistance(mean_side_len, sigma_side_len, min_side_len); double tABC = randomAngle().theta(); - Pose2 xB = xA.retract((Vector(3) << dAB, 0.0, tABC)); + Pose2 xB = xA.retract((Vector(3) << dAB, 0.0, tABC).finished()); // extend from B to find C double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len); diff --git a/gtsam_unstable/geometry/SimWall2D.cpp b/gtsam_unstable/geometry/SimWall2D.cpp index 894312556..eb8ddace6 100644 --- a/gtsam_unstable/geometry/SimWall2D.cpp +++ b/gtsam_unstable/geometry/SimWall2D.cpp @@ -128,7 +128,7 @@ std::pair moveWithBounce(const Pose2& cur_pose, double step_size, // calculate angle to change by Rot2 dtheta = Rot2::fromAngle(angle_drift.sample()(0) + bias.theta()); - Pose2 test_pose = cur_pose.retract((Vector(3) << step_size, 0.0, Rot2::Logmap(dtheta)(0))); + Pose2 test_pose = cur_pose.retract((Vector(3) << step_size, 0.0, Rot2::Logmap(dtheta)(0)).finished()); // create a segment to use for intersection checking // find the closest intersection diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index e72afdc25..2f3c763dd 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -29,7 +29,7 @@ TEST( InvDepthFactor, Project1) { Point2 expected_uv = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - Vector5 inv_landmark((Vector(5) << 1., 0., 1., 0., 0.)); + Vector5 inv_landmark((Vector(5) << 1., 0., 1., 0., 0.).finished()); double inv_depth(1./4); Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth); EXPECT(assert_equal(expected_uv, actual_uv,1e-8)); @@ -45,7 +45,7 @@ TEST( InvDepthFactor, Project2) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - Vector5 diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0)))); + Vector5 diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0))).finished()); double inv_depth(1/sqrt(3.0)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -60,7 +60,7 @@ TEST( InvDepthFactor, Project3) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - Vector5 diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0)))); + Vector5 diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0))).finished()); double inv_depth( 1./sqrt(1.0+1+4)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -75,7 +75,7 @@ TEST( InvDepthFactor, Project4) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - Vector5 diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.)))); + Vector5 diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.))).finished()); double inv_depth(1./sqrt(1.+16.+4.)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -88,7 +88,7 @@ Point2 project_(const Pose3& pose, const Vector5& landmark, const double& inv_de TEST( InvDepthFactor, Dproject_pose) { - Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); + Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2).finished()); double inv_depth(1./4); Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -100,7 +100,7 @@ TEST( InvDepthFactor, Dproject_pose) /* ************************************************************************* */ TEST( InvDepthFactor, Dproject_landmark) { - Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); + Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2).finished()); double inv_depth(1./4); Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -112,7 +112,7 @@ TEST( InvDepthFactor, Dproject_landmark) /* ************************************************************************* */ TEST( InvDepthFactor, Dproject_inv_depth) { - Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); + Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2).finished()); double inv_depth(1./4); Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -124,7 +124,7 @@ TEST( InvDepthFactor, Dproject_inv_depth) /* ************************************************************************* */ TEST(InvDepthFactor, backproject) { - Vector expected((Vector(5) << 0.,0.,1., 0.1,0.2)); + Vector expected((Vector(5) << 0.,0.,1., 0.1,0.2).finished()); double inv_depth(1./4); InvDepthCamera3 inv_camera(level_pose,K); Point2 z = inv_camera.project(expected, inv_depth); @@ -140,7 +140,7 @@ TEST(InvDepthFactor, backproject) TEST(InvDepthFactor, backproject2) { // backwards facing camera - Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1)); + Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1).finished()); double inv_depth(1./10); InvDepthCamera3 inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); Point2 z = inv_camera.project(expected, inv_depth); diff --git a/gtsam_unstable/geometry/tests/testPose3Upright.cpp b/gtsam_unstable/geometry/tests/testPose3Upright.cpp index dacf28992..0929035b6 100644 --- a/gtsam_unstable/geometry/tests/testPose3Upright.cpp +++ b/gtsam_unstable/geometry/tests/testPose3Upright.cpp @@ -72,7 +72,7 @@ TEST( testPose3Upright, manifold ) { EXPECT(assert_equal(x1, x1.retract(zero(4)), tol)); EXPECT(assert_equal(x2, x2.retract(zero(4)), tol)); - Vector delta12 = (Vector(4) << 3.0, 0.0, 4.0, 0.0), delta21 = -delta12; + Vector delta12 = (Vector(4) << 3.0, 0.0, 4.0, 0.0).finished(), delta21 = -delta12; EXPECT(assert_equal(x2, x1.retract(delta12), tol)); EXPECT(assert_equal(x1, x2.retract(delta21), tol)); diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp index d1a4b8125..25dd5fe98 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((Vector(2) << 0.1, 0.1)); - SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); + SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); + SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas(Vector2(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 92332238a..d359d0eff 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((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 poseOdometry( Rot3::RzRyRx(Vector3(0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx(Vector3(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((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)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished()); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0).finished()); /* ************************************************************************* */ 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 812c1d143..51c2a55a2 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((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 poseOdometry( Rot3::RzRyRx(Vector3(0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx(Vector3(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((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)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished()); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0).finished()); /* ************************************************************************* */ 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 34f768562..08dbc311c 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((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) ); +const Pose3 poseOdometry( Rot3::RzRyRx(Vector3(0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +//const Pose3 poseError( Rot3::RzRyRx(Vector3(0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseError( Rot3::RzRyRx(Vector3(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((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)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished()); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0).finished()); /* ************************************************************************* */ 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 51c61b8ec..5f91527e6 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((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 poseOdometry( Rot3::RzRyRx(Vector3(0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx(Vector3(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((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)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished()); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0).finished()); /* ************************************************************************* */ 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 1e3bc86c6..9708eedb5 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((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 poseOdometry( Rot3::RzRyRx(Vector3(0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx(Vector3(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((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)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished()); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0).finished()); /* ************************************************************************* */ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) { diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index c63bfeb6f..e609af953 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -110,8 +110,8 @@ TEST(ExpressionFactor, Unary) { values.insert(2, Point3(0, 0, 1)); JacobianFactor expected( // - 2, (Matrix(2, 3) << 1, 0, 0, 0, 1, 0), // - (Vector(2) << -17, 30)); + 2, (Matrix(2, 3) << 1, 0, 0, 0, 1, 0).finished(), // + Vector2(-17, 30)); // Create leaves Point3_ p(2); diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index afcd60ae3..832d37d14 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((Vector(2) << 0.1, 0.1)); - SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); + SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); + SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // Create a Fixed-Lag Smoother typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps; diff --git a/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp b/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp index cea3c0184..bfd3c9168 100644 --- a/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp @@ -91,7 +91,7 @@ TEST( particleFactor, eliminate) { /** Small 2D point class implemented as a Vector */ struct State: Vector { State(double x, double y) : - Vector((Vector(2) << x, y)) { + Vector((Vector(2) << x, y).finished()) { } }; @@ -128,7 +128,7 @@ TEST( ParticleFilter, linear1 ) { // Create the controls and measurement properties for our example Matrix F = eye(2, 2); Matrix B = eye(2, 2); - Vector u = (Vector(2) << 1.0, 0.0); + Vector u = Vector2(1.0, 0.0); SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1); Matrix Q = 0.01 * eye(2, 2); Matrix H = eye(2, 2); diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 1719abc86..2a756b5af 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_ = (Vector(3) << 0.0, 0.0, g_e); + n_g_ = (Vector(3) << 0.0, 0.0, g_e).finished(); 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 = (Matrix(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).finished(); // Calculate Jacobian of roll/pitch/yaw wrpt (g1,g2,g3), see doc/ypr.nb Vector b_g = mech0_.b_g(g_e); @@ -83,9 +83,9 @@ std::pair AHRS::initialize(double g_e) double g123 = g1 * g1 + g23; double f = 1 / (std::sqrt(g23) * g123); Matrix H_g = (Matrix(3, 3) << - 0.0, g3 / g23, -(g2 / g23), // roll + 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 + 0.0, 0.0, 0.0).finished(); // we don't know anything on yaw // Calculate the initial covariance matrix for the error state dx, Farrell08book eq. 10.66 Matrix Pa = 0.025 * 0.025 * eye(3); @@ -221,8 +221,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((Vector(3) << 1.0, 0.2, 1.0)); // good for L_twice - Matrix R = diag((Vector(3) << 0.01, 0.0001, 0.01)); +// Matrix R = diag(Vector3(1.0, 0.2, 1.0)); // good for L_twice + Matrix R = diag(Vector3(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/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 9082c0101..dcf43c569 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -281,7 +281,7 @@ public: } } - return (Vector(2) << p_inlier, p_outlier); + return (Vector(2) << p_inlier, p_outlier).finished(); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index f7130d611..614521e76 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -561,12 +561,12 @@ public: 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); + 0.0, 0.0, -1.0).finished(); 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); + 0.0, 0.0, -1.0).finished(); // Convert incoming parameters to ENU Vector Pos_ENU = NED_to_ENU * Pos_NED; @@ -614,7 +614,7 @@ public: Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); - Vector omega_earth_ECEF((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector omega_earth_ECEF(Vector3(0.0, 0.0, 7.292115e-5)); omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; // Calculating g @@ -628,7 +628,7 @@ public: double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); double g_calc( g0/( pow(1 + height/Ro, 2) ) ); - g_ENU = (Vector(3) << 0.0, 0.0, -g_calc); + g_ENU = (Vector(3) << 0.0, 0.0, -g_calc).finished(); // Calculate rho @@ -637,7 +637,7 @@ public: double rho_E = -Vn/(Rm + height); double rho_N = Ve/(Rp + height); double rho_U = Ve*tan(lat_new)/(Rp + height); - rho_ENU = (Vector(3) << rho_E, rho_N, rho_U); + rho_ENU = (Vector(3) << rho_E, rho_N, rho_U).finished(); } 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 a38525bd9..8cf22946a 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -483,12 +483,12 @@ public: 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); + 0.0, 0.0, -1.0).finished(); 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); + 0.0, 0.0, -1.0).finished(); // Convert incoming parameters to ENU Vector Pos_ENU = NED_to_ENU * Pos_NED; diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 908239d93..805c0a71a 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -294,12 +294,12 @@ public: 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); + 0.0, 0.0, -1.0).finished(); 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); + 0.0, 0.0, -1.0).finished(); // Convert incoming parameters to ENU Vector Pos_ENU = NED_to_ENU * Pos_NED; @@ -347,7 +347,7 @@ public: Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); - Vector omega_earth_ECEF((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector omega_earth_ECEF(Vector3(0.0, 0.0, 7.292115e-5)); omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; // Calculating g @@ -361,7 +361,7 @@ public: double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); double g_calc( g0/( pow(1 + height/Ro, 2) ) ); - g_ENU = (Vector(3) << 0.0, 0.0, -g_calc); + g_ENU = (Vector(3) << 0.0, 0.0, -g_calc).finished(); // Calculate rho @@ -370,7 +370,7 @@ public: double rho_E = -Vn/(Rm + height); double rho_N = Ve/(Rp + height); double rho_U = Ve*tan(lat_new)/(Rp + height); - rho_ENU = (Vector(3) << rho_E, rho_N, rho_U); + rho_ENU = (Vector(3) << rho_E, rho_N, rho_U).finished(); } 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/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index ae30a4a49..efa7f5f7d 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -96,7 +96,7 @@ public: " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return (gtsam::Vector(1) << 0.0); + return (gtsam::Vector(1) << 0.0).finished(); } /** return the measurement */ diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 6bf0722a5..2998fdc9b 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -95,7 +95,7 @@ public: << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return (gtsam::Vector(1) << 0.0); + return (gtsam::Vector(1) << 0.0).finished(); } /// Evaluate error h(x)-z and optionally derivatives diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index ae26b4240..154afbdff 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -98,7 +98,7 @@ public: << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return (gtsam::Vector(1) << 0.0); + return (gtsam::Vector(1) << 0.0).finished(); } /// Evaluate error h(x)-z and optionally derivatives diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index f805e26a4..2e2d1e310 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -98,7 +98,7 @@ public: << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return (Vector(1) << 0.0); + return (Vector(1) << 0.0).finished(); } /// Evaluate error h(x)-z and optionally derivatives @@ -217,7 +217,7 @@ public: << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return (Vector(1) << 0.0); + return (Vector(1) << 0.0).finished(); } /// 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 a7464d96f..c5b1fc2a4 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 = (Matrix(3, U.size()) << concatVectors(U)); - Matrix Fmat = (Matrix(3, F.size()) << concatVectors(F)); + Matrix Umat = (Matrix(3, U.size()) << concatVectors(U)).finished(); + Matrix Fmat = (Matrix(3, F.size()) << concatVectors(F)).finished(); 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 = (Vector(3) << 0.0, 0.0, norm_2(meanF)); + b_g = (Vector(3) << 0.0, 0.0, norm_2(meanF)).finished(); 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 = (Vector(3) << 0.0,0.0,g_e); + b_g = (Vector(3) << 0.0,0.0,g_e).finished(); else // normalize b_g and attribute remainder to biases b_g = - g_e * meanF/meanF.norm(); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index 76e4e9edd..dda267a59 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -41,7 +41,7 @@ public: /// gravity in the body frame Vector b_g(double g_e) const { - Vector n_g = (Vector(3) << 0, 0, g_e); + Vector n_g = (Vector(3) << 0, 0, g_e).finished(); return (bRn_ * n_g).vector(); } diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index 1925a3fe4..8d6fcc33b 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -70,7 +70,7 @@ namespace gtsam { /** Single Element Constructor: acts on a single parameter specified by idx */ PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : - Base(model, key), prior_((Vector(1) << prior)), mask_(1, idx), H_(zeros(1, T::Dim())) { + Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(zeros(1, T::Dim())) { assert(model->dim() == 1); this->fillH(); } diff --git a/gtsam_unstable/slam/RelativeElevationFactor.cpp b/gtsam_unstable/slam/RelativeElevationFactor.cpp index 292e3f68f..941a1db89 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/RelativeElevationFactor.cpp @@ -32,7 +32,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p *H2 = zeros(1, 3); (*H2)(0, 2) = -1.0; } - return (Vector(1) << hx - measured_); + return (Vector(1) << hx - measured_).finished(); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index 2f0b2c7a8..e3de43628 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -284,7 +284,7 @@ namespace gtsam { } } - return (Vector(2) << p_inlier, p_outlier); + return (Vector(2) << p_inlier, p_outlier).finished(); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testAHRS.cpp b/gtsam_unstable/slam/tests/testAHRS.cpp index a3c963a58..44f516ae9 100644 --- a/gtsam_unstable/slam/tests/testAHRS.cpp +++ b/gtsam_unstable/slam/tests/testAHRS.cpp @@ -16,8 +16,8 @@ using namespace std; using namespace gtsam; // stationary interval of gyro U and acc F -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)); +Matrix stationaryU = trans((Matrix(3, 3) << -0.0004,-0.0002,-0.0014,0.0006,-0.0003,0.0007,0.0006,-0.0002,-0.0003).finished()); +Matrix stationaryF = trans((Matrix(3, 3) << 0.1152,-0.0188,9.7419,-0.0163,0.0146,9.7753,-0.0283,-0.0428,9.9021).finished()); double g_e = 9.7963; // Atlanta /* ************************************************************************* */ @@ -28,13 +28,13 @@ TEST (AHRS, cov) { 1.0, 2.0, 3.0, 5.0, 7.0, 0.0, 9.0, 4.0, 7.0, - 6.0, 3.0, 2.0); + 6.0, 3.0, 2.0).finished(); Matrix actual = cov(trans(A)); Matrix expected = (Matrix(3, 3) << 10.9167, 2.3333, 5.0000, 2.3333, 4.6667, -2.6667, - 5.0000, -2.6667, 8.6667); + 5.0000, -2.6667, 8.6667).finished(); EXPECT(assert_equal(expected, actual, 1e-4)); } @@ -46,7 +46,7 @@ TEST (AHRS, covU) { Matrix expected = (Matrix(3, 3) << 33.3333333, -1.66666667, 53.3333333, -1.66666667, 0.333333333, -5.16666667, - 53.3333333, -5.16666667, 110.333333); + 53.3333333, -5.16666667, 110.333333).finished(); EXPECT(assert_equal(expected, actual, 1e-4)); } @@ -58,7 +58,7 @@ TEST (AHRS, covF) { Matrix expected = (Matrix(3, 3) << 63.3808333, -0.432166667, -48.1706667, -0.432166667, 8.31053333, -16.6792667, - -48.1706667, -16.6792667, 71.4297333); + -48.1706667, -16.6792667, 71.4297333).finished(); EXPECT(assert_equal(expected, actual, 1e-4)); } @@ -75,7 +75,7 @@ TEST (AHRS, Mechanization_integrate) { Mechanization_bRn2 mech; KalmanFilter::State state; // boost::tie(mech,state) = ahrs.initialize(g_e); -// Vector u = (Vector(3) << 0.05,0.0,0.0); +// Vector u = Vector3(0.05,0.0,0.0); // double dt = 2; // Rot3 expected; // Mechanization_bRn2 mech2 = mech.integrate(u,dt); diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index bd15b9203..5bb4dfd2e 100644 --- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp @@ -56,8 +56,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::(Vector(3) << 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(5, 5, 1.0))); double prior_outlier = 0.5; double prior_inlier = 0.5; @@ -85,8 +85,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::(Vector(3) << 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 50.0, 50.0, 10.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(50.0, 50.0, 10.0))); gtsam::Values values; values.insert(key1, p1); @@ -157,8 +157,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::(Vector(3) << 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 50.0, 50.0, 10.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(50.0, 50.0, 10.0))); gtsam::Values values; values.insert(key1, p1); @@ -220,8 +220,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::(Vector(3) << 0.4021, 0.286, 0.428))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 4.9821, 4.614, 1.8387))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.4021, 0.286, 0.428))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(4.9821, 4.614, 1.8387))); gtsam::Values values; values.insert(key1, p1); diff --git a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp index 38e8a1466..d8a3ba0c1 100644 --- a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp +++ b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp @@ -29,7 +29,7 @@ TEST(BiasedGPSFactor, errorNoiseless) { Point3 measured = t + noise; BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); - Vector expectedError = (Vector(3) << 0.0, 0.0, 0.0 ); + Vector expectedError = Vector3(0.0, 0.0, 0.0 ); Vector actualError = factor.evaluateError(pose,bias); EXPECT(assert_equal(expectedError,actualError, 1E-5)); } @@ -44,7 +44,7 @@ TEST(BiasedGPSFactor, errorNoisy) { Point3 measured = t - noise; BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); - Vector expectedError = (Vector(3) << 1.0, 2.0, 3.0 ); + Vector expectedError = Vector3(1.0, 2.0, 3.0 ); Vector actualError = factor.evaluateError(pose,bias); EXPECT(assert_equal(expectedError,actualError, 1E-5)); } diff --git a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp index c0da655c6..476447f8b 100644 --- a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp +++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp @@ -37,9 +37,9 @@ TEST( EquivInertialNavFactor_GlobalVel, Constructor) Key biasKey1(31); // IMU accumulation variables - Vector delta_pos_in_t0 = (Vector(3) << 0.0, 0.0, 0.0); - Vector delta_vel_in_t0 = (Vector(3) << 0.0, 0.0, 0.0); - Vector delta_angles = (Vector(3) << 0.0, 0.0, 0.0); + Vector delta_pos_in_t0 = Vector3(0.0, 0.0, 0.0); + Vector delta_vel_in_t0 = Vector3(0.0, 0.0, 0.0); + Vector delta_angles = Vector3(0.0, 0.0, 0.0); double delta_t = 0.0; Matrix EquivCov_Overall = zeros(15,15); Matrix Jacobian_wrt_t0_Overall = eye(15); diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp index 6cac34573..ff7307840 100644 --- a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp +++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -42,7 +42,7 @@ TEST( GaussMarkovFactor, equals ) Key x1(1); Key x2(2); double delta_t = 0.10; - Vector tau = (Vector(3) << 100.0, 150.0, 10.0); + Vector tau = Vector3(100.0, 150.0, 10.0); SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0); GaussMarkovFactor factor1(x1, x2, delta_t, tau, model); @@ -58,11 +58,11 @@ TEST( GaussMarkovFactor, error ) Key x1(1); Key x2(2); double delta_t = 0.10; - Vector tau = (Vector(3) << 100.0, 150.0, 10.0); + Vector tau = Vector3(100.0, 150.0, 10.0); SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0); - LieVector v1 = LieVector((Vector(3) << 10.0, 12.0, 13.0)); - LieVector v2 = LieVector((Vector(3) << 10.0, 15.0, 14.0)); + LieVector v1 = LieVector(Vector3(10.0, 12.0, 13.0)); + LieVector v2 = LieVector(Vector3(10.0, 15.0, 14.0)); // Create two nodes linPoint.insert(x1, v1); @@ -90,14 +90,14 @@ TEST (GaussMarkovFactor, jacobian ) { Key x1(1); Key x2(2); double delta_t = 0.10; - Vector tau = (Vector(3) << 100.0, 150.0, 10.0); + Vector tau = Vector3(100.0, 150.0, 10.0); SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0); GaussMarkovFactor factor(x1, x2, delta_t, tau, model); // Update the linearization point - LieVector v1_upd = LieVector((Vector(3) << 0.5, -0.7, 0.3)); - LieVector v2_upd = LieVector((Vector(3) << -0.7, 0.4, 0.9)); + LieVector v1_upd = LieVector(Vector3(0.5, -0.7, 0.3)); + LieVector v2_upd = LieVector(Vector3(-0.7, 0.4, 0.9)); // Calculate the Jacobian matrix using the factor Matrix computed_H1, computed_H2; diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index ad4efe26c..6cfcd93e6 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -104,8 +104,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // First test: zero angular motion, some acceleration - Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.3 - 9.81)); - Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); + Vector measurement_acc(Vector3(0.1, 0.2, 0.3 - 9.81)); + Vector measurement_gyro(Vector3(0.0, 0.0, 0.0)); InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -113,10 +113,10 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, model); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); - Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40)); + Vector3 Vel1(Vector3(0.50, -0.50, 0.40)); imuBias::ConstantBias Bias1; Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); - Vector3 expectedVel2((Vector(3) << 0.51, -0.48, 0.43)); + Vector3 expectedVel2(Vector3(0.51, -0.48, 0.43)); Pose3 actualPose2; Vector3 actualVel2; f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); @@ -137,8 +137,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // First test: zero angular motion, some acceleration - Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.3 - 9.81)); - Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); + Vector measurement_acc(Vector3(0.1, 0.2, 0.3 - 9.81)); + Vector measurement_gyro(Vector3(0.0, 0.0, 0.0)); InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -147,8 +147,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); - Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40)); - Vector3 Vel2((Vector(3) << 0.51, -0.48, 0.43)); + Vector3 Vel1(Vector3(0.50, -0.50, 0.40)); + Vector3 Vel2(Vector3(0.51, -0.48, 0.43)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -169,8 +169,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // Second test: zero angular motion, some acceleration - Vector measurement_acc((Vector(3) << 0.0, 0.0, 0.0 - 9.81)); - Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3)); + Vector measurement_acc(Vector3(0.0, 0.0, 0.0 - 9.81)); + Vector measurement_gyro(Vector3(0.1, 0.2, 0.3)); InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -180,8 +180,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Pose3 Pose1(Rot3(), Point3(2.0, 1.0, 3.0)); Pose3 Pose2(Rot3::Expmap(measurement_gyro * measurement_dt), Point3(2.0, 1.0, 3.0)); - Vector3 Vel1((Vector(3) << 0.0, 0.0, 0.0)); - Vector3 Vel2((Vector(3) << 0.0, 0.0, 0.0)); + Vector3 Vel1(Vector3(0.0, 0.0, 0.0)); + Vector3 Vel2(Vector3(0.0, 0.0, 0.0)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -203,8 +203,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, // Second test: zero angular motion, some acceleration - generated in matlab Vector measurement_acc( - (Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); - Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3)); + Vector3(6.501390843381716, -6.763926150509185, -2.300389940090343)); + Vector measurement_gyro(Vector3(0.1, 0.2, 0.3)); InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -215,7 +215,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, -0.427669306, -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0, 1.0, 3.0); Pose3 Pose1(R1, t1); - Vector3 Vel1((Vector(3) << 0.5, -0.5, 0.4)); + Vector3 Vel1(Vector3(0.5, -0.5, 0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); Point3 t2 = t1.compose(Point3(Vel1 * measurement_dt)); @@ -237,9 +237,9 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, //} // //TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) { -// Vector3 angles((Vector(3) << 3.001, -1.0004, 2.0005)); +// Vector3 angles(Vector3(3.001, -1.0004, 2.0005)); // Rot3 R1(Rot3().RzRyRx(angles)); -// Vector3 q((Vector(3) << 5.8, -2.2, 4.105)); +// Vector3 q(Vector3(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); @@ -271,8 +271,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); Vector measurement_acc( - (Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); - Vector measurement_gyro((Vector(3) << 3.14, 3.14 / 2, -3.14)); + Vector3(6.501390843381716, -6.763926150509185, -2.300389940090343)); + Vector measurement_gyro((Vector(3) << 3.14, 3.14 / 2, -3.14).finished()); InertialNavFactor_GlobalVelocity factor( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -283,13 +283,13 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, -0.427669306, -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0, 1.0, 3.0); Pose3 Pose1(R1, t1); - Vector3 Vel1((Vector(3) << 0.5, -0.5, 0.4)); + Vector3 Vel1(Vector3(0.5, -0.5, 0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); Pose3 Pose2(R2, t2); Vector3 Vel2( - (Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000)); + Vector3(0.510000000000000, -0.480000000000000, 0.430000000000000)); imuBias::ConstantBias Bias1; Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; @@ -374,8 +374,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Key Vel2(22); Key Bias1(31); - Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4)); - Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); + Vector measurement_acc(Vector3(0.1, 0.2, 0.4)); + Vector measurement_gyro(Vector3(-0.2, 0.5, 0.03)); double measurement_dt(0.1); @@ -396,8 +396,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Key Vel2(22); Key Bias1(31); - Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4)); - Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); + Vector measurement_acc(Vector3(0.1, 0.2, 0.4)); + Vector measurement_gyro(Vector3(-0.2, 0.5, 0.03)); double measurement_dt(0.1); @@ -430,9 +430,9 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, 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 // First test: zero angular motion, some acceleration - Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation + Vector measurement_gyro(Vector3(0.0, 0.0, 0.0)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = (Vector(3) << 0.2, 0.1, -0.3 + 9.81) + Vector measurement_acc = Vector3(0.2, 0.1, -0.3 + 9.81) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() * body_P_sensor.translation().vector(); // Measured in ENU orientation @@ -443,10 +443,10 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, model, body_P_sensor); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); - Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40)); + Vector3 Vel1(Vector3(0.50, -0.50, 0.40)); imuBias::ConstantBias Bias1; Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); - Vector3 expectedVel2((Vector(3) << 0.51, -0.48, 0.43)); + Vector3 expectedVel2(Vector3(0.51, -0.48, 0.43)); Pose3 actualPose2; Vector3 actualVel2; f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); @@ -469,9 +469,9 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, 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 // First test: zero angular motion, some acceleration - Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation + Vector measurement_gyro(Vector3(0.0, 0.0, 0.0)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = (Vector(3) << 0.2, 0.1, -0.3 + 9.81) + Vector measurement_acc = Vector3(0.2, 0.1, -0.3 + 9.81) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() * body_P_sensor.translation().vector(); // Measured in ENU orientation @@ -483,8 +483,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); - Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40)); - Vector3 Vel2((Vector(3) << 0.51, -0.48, 0.43)); + Vector3 Vel1(Vector3(0.50, -0.50, 0.40)); + Vector3 Vel2(Vector3(0.51, -0.48, 0.43)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -507,9 +507,9 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, 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 // Second test: zero angular motion, some acceleration - Vector measurement_gyro((Vector(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation + Vector measurement_gyro(Vector3(0.2, 0.1, -0.3)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = (Vector(3) << 0.0, 0.0, 0.0 + 9.81) + Vector measurement_acc = Vector3(0.0, 0.0, 0.0 + 9.81) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() * body_P_sensor.translation().vector(); // Measured in ENU orientation @@ -524,8 +524,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Rot3::Expmap( body_P_sensor.rotation().matrix() * measurement_gyro * measurement_dt), Point3(2.0, 1.0, 3.0)); - Vector3 Vel1((Vector(3) << 0.0, 0.0, 0.0)); - Vector3 Vel2((Vector(3) << 0.0, 0.0, 0.0)); + Vector3 Vel1(Vector3(0.0, 0.0, 0.0)); + Vector3 Vel2(Vector3(0.0, 0.0, 0.0)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -548,10 +548,10 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, 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 // Second test: zero angular motion, some acceleration - generated in matlab - Vector measurement_gyro((Vector(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation + Vector measurement_gyro(Vector3(0.2, 0.1, -0.3)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); Vector measurement_acc = - (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() * body_P_sensor.translation().vector(); // Measured in ENU orientation @@ -565,7 +565,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, -0.427669306, -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0, 1.0, 3.0); Pose3 Pose1(R1, t1); - Vector3 Vel1((Vector(3) << 0.5, -0.5, 0.4)); + Vector3 Vel1(Vector3(0.5, -0.5, 0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); Point3 t2 = t1.compose(Point3(Vel1 * measurement_dt)); @@ -573,7 +573,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector dv = measurement_dt * (R1.matrix() * body_P_sensor.rotation().matrix() - * (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + * Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343) + world_g); Vector3 Vel2 = Vel1 + dv; imuBias::ConstantBias Bias1; @@ -599,10 +599,10 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, 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((Vector(3) << 3.14 / 2, 3.14, +3.14)); // Measured in ENU orientation + Vector measurement_gyro((Vector(3) << 3.14 / 2, 3.14, +3.14).finished()); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); Vector measurement_acc = - (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() * body_P_sensor.translation().vector(); // Measured in ENU orientation diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index bf15c7322..407c9a345 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp @@ -38,7 +38,7 @@ TEST( InvDepthFactor, optimize) { Point2 expected_uv = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - Vector5 inv_landmark((Vector(5) << 0., 0., 1., 0., 0.)); + Vector5 inv_landmark((Vector(5) << 0., 0., 1., 0., 0.).finished()); // initialize inverse depth with "incorrect" depth of 1/4 // in reality this is 1/5, but initial depth is guessed double inv_depth(1./4); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index bb3b81481..20e96e6bf 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -45,7 +45,7 @@ TEST( InvDepthFactorVariant1, optimize) { double theta = atan2(ray.y(), ray.x()); double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); double rho = 1./ray.norm(); - Vector6 expected((Vector(6) << x, y, z, theta, phi, rho)); + Vector6 expected((Vector(6) << x, y, z, theta, phi, rho).finished()); @@ -66,9 +66,9 @@ TEST( InvDepthFactorVariant1, optimize) { // Create a values with slightly incorrect initial conditions Values values; - 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, Vector6(expected + (Vector(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).finished())); + values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30).finished())); + values.insert(landmarkKey, Vector6(expected + (Vector(6) << -0.20, +0.20, -0.35, +0.02, -0.04, +0.05).finished())); // 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 23f642a13..2c85b3dad 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant2, optimize) { double theta = atan2(ray.y(), ray.x()); double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); double rho = 1./ray.norm(); - Vector3 expected((Vector(3) << theta, phi, rho)); + Vector3 expected((Vector(3) << theta, phi, rho).finished()); @@ -64,9 +64,9 @@ TEST( InvDepthFactorVariant2, optimize) { // Create a values with slightly incorrect initial conditions Values values; - 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, Vector3(expected + (Vector(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).finished())); + values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30).finished())); + values.insert(landmarkKey, Vector3(expected + Vector3(+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 e0be9c79c..ec9aa90c3 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant3, optimize) { double theta = atan2(landmark_p1.x(), landmark_p1.z()); double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z())); double rho = 1./landmark_p1.norm(); - Vector3 expected((Vector(3) << theta, phi, rho)); + Vector3 expected((Vector(3) << theta, phi, rho).finished()); @@ -64,9 +64,9 @@ TEST( InvDepthFactorVariant3, optimize) { // Create a values with slightly incorrect initial conditions Values values; - 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, Vector3(expected + (Vector(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).finished())); + values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30).finished())); + values.insert(landmarkKey, Vector3(expected + Vector3(+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 cbc1022fc..2794e5707 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 = (Vector(2) << -3.0, 0.0); +// Vector expectedError = Vector2(-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 = (Vector(2) << -3.0, 0.0); +// Vector expectedError = Vector2(-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 = (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.); +// Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.).finished(); +// Matrix H2Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished(); // // // 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 = (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); +// Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376).finished(); +// Matrix H2Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished(); // // // Verify the Jacobians are correct // CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp index b732b8088..0e5f6421f 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp @@ -114,7 +114,7 @@ TEST( ProjectionFactorPPP, Error ) { Vector actualError(factor.evaluateError(pose, Pose3(), point)); // The expected error is (-3.0, 0.0) pixels / UnitCovariance - Vector expectedError = (Vector(2) << -3.0, 0.0); + Vector expectedError = Vector2(-3.0, 0.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -138,7 +138,7 @@ TEST( ProjectionFactorPPP, ErrorWithTransform ) { Vector actualError(factor.evaluateError(pose, transform, point)); // The expected error is (-3.0, 0.0) pixels / UnitCovariance - Vector expectedError = (Vector(2) << -3.0, 0.0); + Vector expectedError = Vector2(-3.0, 0.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -162,8 +162,8 @@ TEST( ProjectionFactorPPP, Jacobian ) { factor.evaluateError(pose, Pose3(), point, H1Actual, H2Actual, H3Actual); // The expected Jacobians - Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.); - Matrix H3Expected = (Matrix(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.).finished(); + Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished(); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); @@ -197,8 +197,8 @@ TEST( ProjectionFactorPPP, JacobianWithTransform ) { factor.evaluateError(pose, body_P_sensor, point, H1Actual, H2Actual, H3Actual); // The expected Jacobians - Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376); - Matrix H3Expected = (Matrix(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).finished(); + Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished(); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp index cda588d90..e878ea5d9 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp @@ -86,7 +86,7 @@ TEST( ProjectionFactorPPPC, Error ) { Vector actualError(factor.evaluateError(pose, Pose3(), point, *K1)); // The expected error is (-3.0, 0.0) pixels / UnitCovariance - Vector expectedError = (Vector(2) << -3.0, 0.0); + Vector expectedError = Vector2(-3.0, 0.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -107,7 +107,7 @@ TEST( ProjectionFactorPPPC, ErrorWithTransform ) { Vector actualError(factor.evaluateError(pose, transform, point, *K1)); // The expected error is (-3.0, 0.0) pixels / UnitCovariance - Vector expectedError = (Vector(2) << -3.0, 0.0); + Vector expectedError = Vector2(-3.0, 0.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -128,8 +128,8 @@ TEST( ProjectionFactorPPPC, Jacobian ) { factor.evaluateError(pose, Pose3(), point, *K1, H1Actual, H2Actual, H3Actual, H4Actual); // The expected Jacobians - Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.); - Matrix H3Expected = (Matrix(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.).finished(); + Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished(); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); @@ -164,8 +164,8 @@ TEST( ProjectionFactorPPPC, JacobianWithTransform ) { factor.evaluateError(pose, body_P_sensor, point, *K1, H1Actual, H2Actual, H3Actual, H4Actual); // The expected Jacobians - Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376); - Matrix H3Expected = (Matrix(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).finished(); + Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished(); // 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 0d5cb2e36..c6ad9a38b 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((Vector(1) << 2.0), factor.evaluateError(pose1, point1, actH1, actH2))); + EXPECT(assert_equal((Vector(1) << 2.0).finished(), 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((Vector(1) << 3.0), factor.evaluateError(pose1, point1, actH1, actH2))); + EXPECT(assert_equal((Vector(1) << 3.0).finished(), 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((Vector(1) << 2.0), factor.evaluateError(pose2, point1, actH1, actH2))); + EXPECT(assert_equal((Vector(1) << 2.0).finished(), 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((Vector(1) << 3.0), factor.evaluateError(pose2, point1, actH1, actH2))); + EXPECT(assert_equal((Vector(1) << 3.0).finished(), 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((Vector(1) << 3.0), factor.evaluateError(pose3, point1, actH1, actH2))); + EXPECT(assert_equal((Vector(1) << 3.0).finished(), 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 f645f5086..ee06dbce2 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((Vector(1) << 0.0), actual1)); + EXPECT(assert_equal((Vector(1) << 0.0).finished(), actual1)); f.addRange(2, r2); Vector actual2 = f.unwhitenedError(values); - EXPECT(assert_equal((Vector(1) << 0.0), actual2)); + EXPECT(assert_equal((Vector(1) << 0.0).finished(), actual2)); f.addRange(3, r3); vector H(3); Vector actual3 = f.unwhitenedError(values); EXPECT_LONGS_EQUAL(3, f.keys().size()); - EXPECT(assert_equal((Vector(1) << 0.0), actual3)); + EXPECT(assert_equal((Vector(1) << 0.0).finished(), actual3)); // Check keys and Jacobian Vector actual4 = f.unwhitenedError(values, H); // with H now ! - 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())); + EXPECT(assert_equal((Vector(1) << 0.0).finished(), actual4)); + CHECK(assert_equal((Matrix(1, 3) << 0.0,-1.0,0.0).finished(), H.front())); + CHECK(assert_equal((Matrix(1, 3) << sqrt(2.0)/2,-sqrt(2.0)/2,0.0).finished(), 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((Vector(1) << sqrt(25.0+16.0)-sqrt(50.0)), actual5)); + EXPECT(assert_equal((Vector(1) << sqrt(25.0+16.0)-sqrt(50.0)).finished(), actual5)); // Create Factor graph NonlinearFactorGraph graph; diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp index 7e2aa507e..d8d720e83 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp @@ -52,7 +52,7 @@ TEST( TransformBtwRobotsUnaryFactor, ConstructorAndEquals) gtsam::Pose2 rel_pose_ideal = p1.between(p2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); - SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); gtsam::Values valA, valB; valA.insert(keyA, p1); @@ -83,7 +83,7 @@ TEST( TransformBtwRobotsUnaryFactor, unwhitenedError) gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal; - SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); gtsam::Values valA, valB; valA.insert(keyA, orgA_T_1); @@ -117,7 +117,7 @@ TEST( TransformBtwRobotsUnaryFactor, unwhitenedError2) gtsam::Pose2 rel_pose_ideal = orgA_T_currA.between(orgA_T_currB); gtsam::Pose2 rel_pose_msr = rel_pose_ideal; - SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); gtsam::Values valA, valB; valA.insert(keyA, orgA_T_currA); @@ -160,7 +160,7 @@ TEST( TransformBtwRobotsUnaryFactor, Optimize) gtsam::Pose2 currA_Tmsr_currB3 = currA_Tmsr_currB1; gtsam::Pose2 currA_Tmsr_currB4 = currA_Tmsr_currB1; - SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); gtsam::Values valA, valB; valA.insert(keyA, orgA_T_currA); @@ -213,7 +213,7 @@ TEST( TransformBtwRobotsUnaryFactor, 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(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); + SharedGaussian model(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); gtsam::Values valA, valB; valA.insert(keyA, orgA_T_1); @@ -251,8 +251,8 @@ TEST( TransformBtwRobotsUnaryFactor, 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::(Vector(3) << 0.5, 0.5, 0.05))); -// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 50.0, 50.0, 10.0))); +// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05))); +// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(50.0, 50.0, 10.0))); // // gtsam::Values values; // values.insert(keyA, p1); diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp index 146cdc06f..dbd90f3a3 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp @@ -52,8 +52,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((Vector(3) << 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(Vector3(5, 5, 1.0))); double prior_outlier = 0.5; double prior_inlier = 0.5; @@ -89,8 +89,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((Vector(3) << 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(Vector3(5, 5, 1.0))); double prior_outlier = 0.01; double prior_inlier = 0.99; @@ -128,8 +128,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((Vector(3) << 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(Vector3(5, 5, 1.0))); double prior_outlier = 0.01; double prior_inlier = 0.99; @@ -176,8 +176,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((Vector(3) << 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(Vector3(5, 5, 1.0))); double prior_outlier = 0.01; double prior_inlier = 0.99; @@ -237,8 +237,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((Vector(3) << 0.5, 0.5, 0.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(Vector3(5, 5, 1.0))); double prior_outlier = 0.5; double prior_inlier = 0.5; @@ -276,8 +276,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::(Vector(3) << 0.5, 0.5, 0.05))); -// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 50.0, 50.0, 10.0))); +// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05))); +// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(50.0, 50.0, 10.0))); // // gtsam::Values values; // values.insert(keyA, p1); diff --git a/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp index dabf283c6..39481f929 100644 --- a/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp @@ -31,7 +31,7 @@ gtsam::Rot3 world_R_ECEF( 0.85173, -0.52399, 0, 0.41733, 0.67835, -0.60471); -gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); +gtsam::Vector ECEF_omega_earth(Vector3(0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); /* ************************************************************************* */ @@ -53,16 +53,16 @@ int main() { gtsam::Key BiasKey1(31); double measurement_dt(0.1); - 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)); + Vector world_g(Vector3(0.0, 0.0, 9.81)); + Vector world_rho(Vector3(0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth(Vector3(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((Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); - Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3)); + Vector measurement_acc(Vector3(6.501390843381716, -6.763926150509185, -2.300389940090343)); + Vector measurement_gyro(Vector3(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); @@ -71,7 +71,7 @@ int main() { -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - Vector3 Vel1 = Vector((Vector(3) << 0.5,-0.5,0.4)); + Vector3 Vel1 = Vector(Vector3(0.5,-0.5,0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); diff --git a/tests/smallExample.h b/tests/smallExample.h index 7de553a68..b7cb7aefe 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -222,9 +222,9 @@ inline Values createValues() { inline VectorValues createVectorValues() { using namespace impl; VectorValues c = boost::assign::pair_list_of - (_l1_, (Vector(2) << 0.0, -1.0)) - (_x1_, (Vector(2) << 0.0, 0.0)) - (_x2_, (Vector(2) << 1.5, 0.0)); + (_l1_, Vector2(0.0, -1.0)) + (_x1_, Vector2(0.0, 0.0)) + (_x2_, Vector2(1.5, 0.0)); return c; } @@ -249,9 +249,9 @@ inline VectorValues createCorrectDelta() { using symbol_shorthand::X; using symbol_shorthand::L; VectorValues c; - 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)); + c.insert(L(1), Vector2(-0.1, 0.1)); + c.insert(X(1), Vector2(-0.1, -0.1)); + c.insert(X(2), Vector2(0.1, -0.2)); return c; } @@ -277,13 +277,13 @@ inline 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), (Vector(2) << 2.0, -1.0)); + fg += JacobianFactor(X(1), -10*eye(2), X(2), 10*eye(2), Vector2(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), (Vector(2) << 0.0, 1.0)); + fg += JacobianFactor(X(1), -5*eye(2), L(1), 5*eye(2), Vector2(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), (Vector(2) << -1.0, 1.5)); + fg += JacobianFactor(X(2), -5*eye(2), L(1), 5*eye(2), Vector2(-1.0, 1.5)); return fg; } @@ -296,8 +296,8 @@ inline GaussianFactorGraph createGaussianFactorGraph() { */ inline GaussianBayesNet createSmallGaussianBayesNet() { using namespace impl; - Matrix R11 = (Matrix(1, 1) << 1.0), S12 = (Matrix(1, 1) << 1.0); - Matrix R22 = (Matrix(1, 1) << 1.0); + Matrix R11 = (Matrix(1, 1) << 1.0).finished(), S12 = (Matrix(1, 1) << 1.0).finished(); + Matrix R22 = (Matrix(1, 1) << 1.0).finished(); Vector d1(1), d2(1); d1(0) = 9; d2(0) = 5; @@ -324,7 +324,7 @@ inline Point2 h(const Point2& v) { inline Matrix H(const Point2& v) { return (Matrix(2, 2) << -sin(v.x()), 0.0, - 0.0, cos(v.y())); + 0.0, cos(v.y())).finished(); } struct UnaryFactor: public gtsam::NoiseModelFactor1 { @@ -349,7 +349,7 @@ inline boost::shared_ptr sharedReallyNonlinearFactor using symbol_shorthand::X; using symbol_shorthand::L; boost::shared_ptr fg(new NonlinearFactorGraph); - Vector z = (Vector(2) << 1.0, 0.0); + Vector z = Vector2(1.0, 0.0); double sigma = 0.1; boost::shared_ptr factor( new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); @@ -421,7 +421,7 @@ inline GaussianFactorGraph createSimpleConstraintGraph() { // |0 1||x_2| | 0 -1||y_2| |0| Matrix Ax1 = eye(2); Matrix Ay1 = eye(2) * -1; - Vector b2 = (Vector(2) << 0.0, 0.0); + Vector b2 = Vector2(0.0, 0.0); JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, constraintModel)); @@ -439,7 +439,7 @@ inline VectorValues createSimpleConstraintValues() { using symbol_shorthand::X; using symbol_shorthand::L; VectorValues config; - Vector v = (Vector(2) << 1.0, -1.0); + Vector v = Vector2(1.0, -1.0); config.insert(_x_, v); config.insert(_y_, v); return config; @@ -467,7 +467,7 @@ inline GaussianFactorGraph createSingleConstraintGraph() { Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0; Matrix Ay1 = eye(2) * 10; - Vector b2 = (Vector(2) << 1.0, 2.0); + Vector b2 = Vector2(1.0, 2.0); JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, constraintModel)); @@ -483,8 +483,8 @@ inline GaussianFactorGraph createSingleConstraintGraph() { inline VectorValues createSingleConstraintValues() { using namespace impl; VectorValues config = boost::assign::pair_list_of - (_x_, (Vector(2) << 1.0, -1.0)) - (_y_, (Vector(2) << 0.2, 0.1)); + (_x_, Vector2(1.0, -1.0)) + (_y_, Vector2(0.2, 0.1)); return config; } @@ -493,7 +493,7 @@ inline GaussianFactorGraph createMultiConstraintGraph() { using namespace impl; // unary factor 1 Matrix A = eye(2); - Vector b = (Vector(2) << -2.0, 2.0); + Vector b = Vector2(-2.0, 2.0); JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1)); // constraint 1 @@ -547,9 +547,9 @@ inline GaussianFactorGraph createMultiConstraintGraph() { inline VectorValues createMultiConstraintValues() { using namespace impl; VectorValues config = boost::assign::pair_list_of - (_x_, (Vector(2) << -2.0, 2.0)) - (_y_, (Vector(2) << -0.1, 0.4)) - (_z_, (Vector(2) <<-4.0, 5.0)); + (_x_, Vector2(-2.0, 2.0)) + (_y_, Vector2(-0.1, 0.4)) + (_z_, Vector2(-4.0, 5.0)); return config; } diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 7b5f31660..7c1e20a1a 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, (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); + JacobianFactor expected1(key, (Matrix(1, 2) << 1.0, 0.0).finished(), repeat(1, 3.0), hard_model1); + JacobianFactor expected2(key, (Matrix(1, 2) << 0.0, 1.0).finished(), repeat(1, 5.0), hard_model1); EXPECT(assert_equal((const GaussianFactor&)expected1, *actual1, tol)); EXPECT(assert_equal((const GaussianFactor&)expected2, *actual2, tol)); } @@ -187,7 +187,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) { EXPECT(!rangeBound.isGreaterThan()); EXPECT(rangeBound.dim() == 1); - EXPECT(assert_equal(((Vector)Vector(1) << 2.0), rangeBound.evaluateError(pt1, pt1))); + EXPECT(assert_equal((Vector(1) << 2.0).finished(), rangeBound.evaluateError(pt1, pt1))); EXPECT(assert_equal(ones(1), rangeBound.evaluateError(pt1, pt2))); EXPECT(assert_equal(zero(1), rangeBound.evaluateError(pt1, pt3))); EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4))); diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index c4bf0480c..a2bb57e1c 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, (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))); + 0, Vector2(1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(), + 3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0).finished(), + 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 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))); + 1, Vector2(15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(), + 2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0).finished(), + 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 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))); + 2, Vector2(29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(), + 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 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))); + 3, Vector2(39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(), + 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0))); + 4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished())); // 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, (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))); + 0, Vector2(1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(), + 3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0).finished(), + 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 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))); + 1, Vector2(15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(), + 2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0).finished(), + 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 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))); + 2, Vector2(29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(), + 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 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))); + 3, Vector2(39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(), + 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0))); + 4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished())); // Compute dogleg point for different deltas diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 7b27293df..8748e7464 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((Vector(2) << 0.1, 0.1)); + SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector2(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 = (Vector(2) << 1.0, 0.0); + Vector u = Vector2(1.0, 0.0); Point2 difference(u*dt); - SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1), true); + SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector2(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((Vector(2) << 0.25, 0.25), true); + SharedDiagonal R = noiseModel::Diagonal::Sigmas(Vector2(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((Vector(2) << 1.0, 1.0)), TestKey1, TestKey2), Q_(2,2) { + Base(noiseModel::Diagonal::Sigmas(Vector2(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' @@ -403,7 +403,7 @@ TEST( ExtendedKalmanFilter, nonlinear ) { // Create the Kalman Filter initialization point Point2 x_initial(0.90, 1.10); - SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); + SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // Create an ExtendedKalmanFilter object ExtendedKalmanFilter ekf(X(0), x_initial, P_initial); @@ -416,7 +416,7 @@ TEST( ExtendedKalmanFilter, nonlinear ) { x_predict = ekf.predict(motionFactor); // Create a measurement factor - NonlinearMeasurementModel measurementFactor(X(i+1), (Vector(1) << z[i])); + NonlinearMeasurementModel measurementFactor(X(i+1), (Vector(1) << z[i]).finished()); x_update = ekf.update(measurementFactor); EXPECT(assert_equal(expected_predict[i],x_predict, 1e-6)); diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index 97bd5bd57..f7f5467af 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -77,7 +77,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) // Check the conditional P(C3|Root) double sigma3 = 0.61808; - Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022); + Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022).finished(); 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 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067); + Matrix A46 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067).finished(); 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, (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); + fg.add(1, (Matrix(1, 1) << 1.0).finished(), 3, (Matrix(1, 1) << 2.0).finished(), 5, (Matrix(1, 1) << 3.0).finished(), (Vector(1) << 4.0).finished(), model); + fg.add(1, (Matrix(1, 1) << 5.0).finished(), (Vector(1) << 6.0).finished(), model); + fg.add(2, (Matrix(1, 1) << 7.0).finished(), 4, (Matrix(1, 1) << 8.0).finished(), 5, (Matrix(1, 1) << 9.0).finished(), (Vector(1) << 10.0).finished(), model); + fg.add(2, (Matrix(1, 1) << 11.0).finished(), (Vector(1) << 12.0).finished(), model); + fg.add(5, (Matrix(1, 1) << 13.0).finished(), 6, (Matrix(1, 1) << 14.0).finished(), (Vector(1) << 15.0).finished(), model); + fg.add(6, (Matrix(1, 1) << 17.0).finished(), 7, (Matrix(1, 1) << 18.0).finished(), (Vector(1) << 19.0).finished(), model); + fg.add(7, (Matrix(1, 1) << 20.0).finished(), (Vector(1) << 21.0).finished(), model); // Eliminate into BayesTree // c(6,7) diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 962d8b893..3a0081bdb 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 = (Vector(2) << -0.133333, -0.0222222); + Vector d = Vector2(-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 = (Vector(2) << 0.2, -0.14)/sig, sigma = ones(2); + Vector d = Vector2(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 = (Vector(2) << -0.1, 0.25)/sig, sigma = ones(2); + Vector d = Vector2(-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,7 +130,7 @@ 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 = (Vector(2) << -0.133333, -0.0222222), sigma = ones(2); + Vector d = Vector2(-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 @@ -138,13 +138,13 @@ TEST( GaussianFactorGraph, eliminateOne_x1_fast ) 4.714045207910318, 0., 0., 4.714045207910318, 0., 0., - 0., 0.), + 0., 0.).finished(), 2, (Matrix(4,2) << -2.357022603955159, 0., 0., -2.357022603955159, 7.071067811865475, 0., - 0., 7.071067811865475), - (Vector(4) << -0.707106781186547, 0.942809041582063, 0.707106781186547, -1.414213562373094), noiseModel::Unit::Create(4)); + 0., 7.071067811865475).finished(), + (Vector(4) << -0.707106781186547, 0.942809041582063, 0.707106781186547, -1.414213562373094).finished(), 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 = (Vector(2) << 0.2, -0.14)/sig, sigma = ones(2); + Vector d = Vector2(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 = (Vector(2) << -0.1, 0.25)/sig, sigma = ones(2); + Vector d = Vector2(-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 = (Vector(2) << -0.1,-0.1); + Vector d1 = Vector2(-0.1,-0.1); GaussianBayesNet expected = simpleGaussian(ordering[X(1)],d1,0.1); double sig1 = 0.149071; - Vector d2 = (Vector(2) << 0.0, 0.2)/sig1, sigma2 = ones(2); + Vector d2 = Vector2(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 = (Vector(2) << 0.2, -0.14)/sig2, sigma3 = ones(2); + Vector d3 = Vector2(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 += (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); + expected += Vector2(-1.0,-1.0); + expected += Vector2(2.0,-1.0); + expected += Vector2(0.0, 1.0); + expected += Vector2(-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 = (Vector(1) << 0.0); + Vector b = (Vector(1) << 0.0).finished(); 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; @@ -407,10 +407,10 @@ TEST( GaussianFactorGraph, elimination ) boost::tie(R,d) = matrix(bayesNet); Matrix expected = (Matrix(2, 2) << 0.707107, -0.353553, - 0.0, 0.612372); + 0.0, 0.612372).finished(); Matrix expected2 = (Matrix(2, 2) << 0.707107, -0.353553, - 0.0, -0.612372); + 0.0, -0.612372).finished(); 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((Vector(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).finished()); 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 553a7fd5f..b43f0d3ef 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -43,8 +43,8 @@ static const LieScalar Zero(0); // SETDEBUG("ISAM2 recalculate", true); // Set up parameters -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)); +SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, M_PI/100.0).finished()); +SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, 0.1).finished()); ISAM2 createSlamlikeISAM2( boost::optional init_values = boost::none, @@ -172,10 +172,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((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)); +// values.push_back_preallocated(Vector2(0.09, 0.09)); +// values.push_back_preallocated(Vector3(0.11, 0.11, 0.09)); +// values.push_back_preallocated(Vector3(0.09, 0.09, 0.09)); +// values.push_back_preallocated(Vector2(0.11, 0.11)); // // // Create a permutation // Permutation permutation(4); diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index 0b84f137d..cdb1509b6 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 = (Vector(2) << 0., 0.); + Vector v = Vector2(0., 0.); for (int i=1; i<=7; i++) expected[ordering[X(i)]] = v; EXPECT(assert_equal(expected,actual)); @@ -199,7 +199,7 @@ TEST(GaussianJunctionTreeB, simpleMarginal) { // Create a simple graph NonlinearFactorGraph fg; fg.add(PriorFactor(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0))); - fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas((Vector(3) << 10.0, 1.0, 1.0)))); + fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector3(10.0, 1.0, 1.0)))); Values init; init.insert(X(0), Pose2()); diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index 2a84248a3..81bcac344 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((Vector(3) << 0.3, 0.3, 0.1)); + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph += PriorFactor(1, priorMean, priorNoise); // 2b. Add odometry factors - SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); + SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(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((Vector(3) << 0.2, 0.2, 0.1)); + SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector3(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/testGraph.cpp b/tests/testGraph.cpp index a1d2d8c41..6c1fd7dd5 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -114,7 +114,7 @@ TEST( GaussianFactorGraph, findMinimumSpanningTree ) GaussianFactorGraph g; Matrix I = eye(2); Vector2 b(0, 0); - const SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(2) << 0.5, 0.5)); + const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5)); using namespace symbol_shorthand; g += JacobianFactor(X(1), I, X(2), I, b, model); g += JacobianFactor(X(1), I, X(3), I, b, model); diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index bb50d2afa..071b9d12d 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 = (Vector(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).finished(); // 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), (Vector(3) << -0.5,0.,0.)); + expected.insert(X(2), Vector3(-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), (Vector(3) << -0.5,0.,0.)); + expected.insert(X(2), Vector3(-0.5,0.,0.)); CHECK(assert_equal(expected, actual)); } diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index 5478ce38e..dd22ae738 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((Vector(3) << 0.3, 0.3, 0.1)); + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(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((Vector(3) << 0.2, 0.2, 0.1)); + SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(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((Vector(2) << 0.1, 0.2)); + SharedDiagonal measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(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 b55f6f144..6366d9fa5 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 = (Vector(3) << -0.989992, -0.14112, 0.0); + Vector expVec = Vector3(-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((Vector(2) << 1.0, 0.0), constraint.evaluateError(ptBad1), tol)); - EXPECT(assert_equal((Vector(2) << 1.0, 0.0), constraint.unwhitenedError(config2), tol)); + EXPECT(assert_equal(Vector2(1.0, 0.0), constraint.evaluateError(ptBad1), tol)); + EXPECT(assert_equal(Vector2(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), (Vector(2) <<-1.0,0.0), hard_model)); + GaussianFactor::shared_ptr expected2(new JacobianFactor(key, eye(2,2), Vector2(-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((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(assert_equal(Vector2(-1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol)); + EXPECT(assert_equal(Vector2(-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), (Vector(2) << 1.0, 1.0), hard_model)); + eye(2,2), Vector2(1.0, 1.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } @@ -523,7 +523,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { Rot3 faceDownY((Matrix)(Matrix(3,3) << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, - 0.0, -1.0, 0.0)); + 0.0, -1.0, 0.0).finished()); Pose3 pose1(faceDownY, Point3()); // origin, left camera SimpleCamera camera1(pose1, K); Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 025a3c12e..05cb9c4ad 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -207,7 +207,7 @@ TEST( NonlinearFactor, linearize_constraint1 ) // create expected Vector2 b(0., -3.); - JacobianFactor expected(X(1), (Matrix(2, 2) << 5.0, 0.0, 0.0, 1.0), b, + JacobianFactor expected(X(1), (Matrix(2, 2) << 5.0, 0.0, 0.0, 1.0).finished(), b, noiseModel::Constrained::MixedSigmas(Vector2(1,0))); CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } @@ -237,7 +237,7 @@ TEST( NonlinearFactor, linearize_constraint2 ) class TestFactor4 : public NoiseModelFactor4 { public: typedef NoiseModelFactor4 Base; - TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4)) {} + TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {} virtual Vector evaluateError(const double& x1, const double& x2, const double& x3, const double& x4, @@ -246,10 +246,10 @@ public: boost::optional H3 = boost::none, boost::optional H4 = boost::none) const { if(H1) { - *H1 = (Matrix(1, 1) << 1.0); - *H2 = (Matrix(1, 1) << 2.0); - *H3 = (Matrix(1, 1) << 3.0); - *H4 = (Matrix(1, 1) << 4.0); + *H1 = (Matrix(1, 1) << 1.0).finished(); + *H2 = (Matrix(1, 1) << 2.0).finished(); + *H3 = (Matrix(1, 1) << 3.0).finished(); + *H4 = (Matrix(1, 1) << 4.0).finished(); } return (Vector(1) << x1 + x2 + x3 + x4).finished(); } @@ -267,25 +267,25 @@ TEST(NonlinearFactor, NoiseModelFactor4) { tv.insert(X(2), double((2.0))); tv.insert(X(3), double((3.0))); tv.insert(X(4), double((4.0))); - EXPECT(assert_equal((Vector(1) << 10.0), tf.unwhitenedError(tv))); + EXPECT(assert_equal((Vector(1) << 10.0).finished(), 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)(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())); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), jf.getA(jf.begin()))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0).finished(), jf.getA(jf.begin()+1))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3))); + EXPECT(assert_equal((Vector)(Vector(1) << -5.0).finished(), jf.getb())); } /* ************************************************************************* */ class TestFactor5 : public NoiseModelFactor5 { public: typedef NoiseModelFactor5 Base; - TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4), X(5)) {} + TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), 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, @@ -295,11 +295,11 @@ public: boost::optional H4 = boost::none, boost::optional H5 = boost::none) const { if(H1) { - *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); + *H1 = (Matrix(1, 1) << 1.0).finished(); + *H2 = (Matrix(1, 1) << 2.0).finished(); + *H3 = (Matrix(1, 1) << 3.0).finished(); + *H4 = (Matrix(1, 1) << 4.0).finished(); + *H5 = (Matrix(1, 1) << 5.0).finished(); } return (Vector(1) << x1 + x2 + x3 + x4 + x5).finished(); } @@ -314,7 +314,7 @@ TEST(NonlinearFactor, NoiseModelFactor5) { tv.insert(X(3), double((3.0))); tv.insert(X(4), double((4.0))); tv.insert(X(5), double((5.0))); - EXPECT(assert_equal((Vector(1) << 15.0), tf.unwhitenedError(tv))); + EXPECT(assert_equal((Vector(1) << 15.0).finished(), 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]); @@ -322,19 +322,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)(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())); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), jf.getA(jf.begin()))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0).finished(), jf.getA(jf.begin()+1))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.5).finished(), jf.getA(jf.begin()+4))); + EXPECT(assert_equal((Vector)(Vector(1) << -7.5).finished(), jf.getb())); } /* ************************************************************************* */ class TestFactor6 : public NoiseModelFactor6 { public: typedef NoiseModelFactor6 Base; - TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4), X(5), X(6)) {} + TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), 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, @@ -345,12 +345,12 @@ public: boost::optional H5 = boost::none, boost::optional H6 = boost::none) const { if(H1) { - *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); + *H1 = (Matrix(1, 1) << 1.0).finished(); + *H2 = (Matrix(1, 1) << 2.0).finished(); + *H3 = (Matrix(1, 1) << 3.0).finished(); + *H4 = (Matrix(1, 1) << 4.0).finished(); + *H5 = (Matrix(1, 1) << 5.0).finished(); + *H6 = (Matrix(1, 1) << 6.0).finished(); } return (Vector(1) << x1 + x2 + x3 + x4 + x5 + x6).finished(); } @@ -367,7 +367,7 @@ TEST(NonlinearFactor, NoiseModelFactor6) { tv.insert(X(4), double((4.0))); tv.insert(X(5), double((5.0))); tv.insert(X(6), double((6.0))); - EXPECT(assert_equal((Vector(1) << 21.0), tf.unwhitenedError(tv))); + EXPECT(assert_equal((Vector(1) << 21.0).finished(), 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]); @@ -376,13 +376,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)(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())); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), jf.getA(jf.begin()))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0).finished(), jf.getA(jf.begin()+1))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.5).finished(), jf.getA(jf.begin()+4))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 3.0).finished(), jf.getA(jf.begin()+5))); + EXPECT(assert_equal((Vector)(Vector(1) << -10.5).finished(), jf.getb())); } diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 51aa21773..e9e266bb3 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((Vector(3) << 3.0, 3.0, 0.5)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(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((Vector(3) << 3.0, 3.0, 0.5)); - SharedDiagonal model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 2.0, 2.0)); + SharedDiagonal model3 = noiseModel::Diagonal::Sigmas(Vector3(3.0, 3.0, 0.5)); + SharedDiagonal model2 = noiseModel::Diagonal::Sigmas(Vector2(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((Vector(3) << 3.0, 3.0, 0.5)); - SharedDiagonal model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 2.0, 2.0)); + SharedDiagonal model3 = noiseModel::Diagonal::Sigmas(Vector3(3.0, 3.0, 0.5)); + SharedDiagonal model2 = noiseModel::Diagonal::Sigmas(Vector2(2.0, 2.0)); Sampler sampler(model3, 42u); // create initial graph diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index 04c4e9d52..da3462b57 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -57,19 +57,19 @@ TEST( PCGSolver, llt ) { Matrix R = (Matrix(3,3) << 1., -1., -1., 0., 2., -1., - 0., 0., 1.); + 0., 0., 1.).finished(); Matrix AtA = R.transpose() * R; Vector Rvector = (Vector(9) << 1., -1., -1., 0., 2., -1., - 0., 0., 1.); + 0., 0., 1.).finished(); // Vector Rvector = (Vector(6) << 1., -1., -1., // 2., -1., -// 1.); +// 1.).finished(); - Vector b = (Vector(3) << 1., 2., 3.); + Vector b = Vector3(1., 2., 3.); - Vector x = (Vector(3) << 6.5, 2.5, 3.) ; + Vector x = Vector3(6.5, 2.5, 3.) ; /* test cholesky */ Matrix Rhat = AtA.llt().matrixL().transpose(); diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 40d49a934..ad2c0767a 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -34,11 +34,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), (Vector(2) << 2.0, -1.0), unit2); + fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), Vector2(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), (Vector(2) << 0.0, 1.0), unit2); + fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), Vector2(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), (Vector(2) << -1.0, 1.5), unit2); + fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), Vector2(-1.0, 1.5), unit2); return fg; } diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index c9f434da2..ffde900e9 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -287,8 +287,8 @@ TEST (testSerializationSLAM, smallExample_nonlinear) { /* ************************************************************************* */ TEST (testSerializationSLAM, factors) { - LieVector lieVector((Vector(4) << 1.0, 2.0, 3.0, 4.0)); - LieMatrix lieMatrix((Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0)); + LieVector lieVector((Vector(4) << 1.0, 2.0, 3.0, 4.0).finished()); + LieMatrix lieMatrix((Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0).finished()); Point2 point2(1.0, 2.0); StereoPoint2 stereoPoint2(1.0, 2.0, 3.0); Point3 point3(1.0, 2.0, 3.0); diff --git a/tests/testSimulated2DOriented.cpp b/tests/testSimulated2DOriented.cpp index dcc31e0ec..d411e7d5e 100644 --- a/tests/testSimulated2DOriented.cpp +++ b/tests/testSimulated2DOriented.cpp @@ -58,7 +58,7 @@ TEST( simulated2DOriented, Dprior ) TEST( simulated2DOriented, constructor ) { Pose2 measurement(0.2, 0.3, 0.1); - SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1., 1., 1.)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1., 1., 1.)); simulated2DOriented::Odometry factor(measurement, model, X(1), X(2)); simulated2DOriented::Values config; diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index 765c1e1ca..6a49f66d3 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -131,12 +131,12 @@ TEST( SubgraphPreconditioner, system ) // y1 = perturbed y0 VectorValues y1 = zeros; - y1[1] = (Vector(2) << 1.0, -1.0); + y1[1] = Vector2(1.0, -1.0); // Check corresponding x values VectorValues expected_x1 = xtrue, x1 = system.x(y1); - expected_x1[1] = (Vector(2) << 2.01, 2.99); - expected_x1[0] = (Vector(2) << 3.01, 2.99); + expected_x1[1] = Vector2(2.01, 2.99); + expected_x1[0] = Vector2(3.01, 2.99); CHECK(assert_equal(xtrue, system.x(y0))); CHECK(assert_equal(expected_x1,system.x(y1))); @@ -150,21 +150,21 @@ TEST( SubgraphPreconditioner, system ) VectorValues expected_gx0 = zeros; VectorValues expected_gx1 = zeros; CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue))); - 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.); + expected_gx1[2] = Vector2(-100., 100.); + expected_gx1[4] = Vector2(-100., 100.); + expected_gx1[1] = Vector2(200., -200.); + expected_gx1[3] = Vector2(-100., 100.); + expected_gx1[0] = Vector2(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] = (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.); + expected_gy1[2] = Vector2(2., -2.); + expected_gy1[4] = Vector2(-2., 2.); + expected_gy1[1] = Vector2(3., -3.); + expected_gy1[3] = Vector2(-1., 1.); + expected_gy1[0] = Vector2(1., -1.); CHECK(assert_equal(expected_gy0,gradient(system,y0))); CHECK(assert_equal(expected_gy1,gradient(system,y1))); @@ -204,7 +204,7 @@ TEST( SubgraphPreconditioner, conjugateGradients ) VectorValues y0 = VectorValues::Zero(xbar); VectorValues y1 = y0; - y1[1] = (Vector(2) << 1.0, -1.0); + y1[1] = Vector2(1.0, -1.0); VectorValues x1 = system.x(y1); // Solve for the remaining constraints using PCG diff --git a/timing/timeCalibratedCamera.cpp b/timing/timeCalibratedCamera.cpp index 76813c455..0a003a4c7 100644 --- a/timing/timeCalibratedCamera.cpp +++ b/timing/timeCalibratedCamera.cpp @@ -31,7 +31,7 @@ int main() 1., 0., 0., 0.,-1., 0., 0., 0.,-1. - )), + ).finished()), Point3(0,0,0.5)); const CalibratedCamera camera(pose1); diff --git a/timing/timeGaussianFactor.cpp b/timing/timeGaussianFactor.cpp index 261fbac48..e6ae51aa4 100644 --- a/timing/timeGaussianFactor.cpp +++ b/timing/timeGaussianFactor.cpp @@ -63,7 +63,7 @@ int main() +0.,-5., 10., 0., +0.,10. - ); + ).finished(); Matrix Al1 = (Matrix(8, 10) << // l1 @@ -75,7 +75,7 @@ int main() 0., 5.,1.,2.,3.,4.,5.,6.,7.,8., 0., 0.,1.,2.,3.,4.,5.,6.,7.,8., 0., 0.,1.,2.,3.,4.,5.,6.,7.,8. - ); + ).finished(); Matrix Ax1 = (Matrix(8, 2) << // x1 @@ -87,7 +87,7 @@ int main() 0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8., -10., 0.,1.,2.,3.,4.,5.,6.,7.,8., 0.00,-10.,1.,2.,3.,4.,5.,6.,7.,8. - ); + ).finished(); // and a RHS Vector b2(8); diff --git a/timing/timeLago.cpp b/timing/timeLago.cpp index ff60c4a27..931e2498f 100644 --- a/timing/timeLago.cpp +++ b/timing/timeLago.cpp @@ -36,14 +36,14 @@ int main(int argc, char *argv[]) { Values::shared_ptr solution; NonlinearFactorGraph::shared_ptr g; string inputFile = findExampleDataFile("w10000"); - SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(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).finished()); boost::tie(g, solution) = load2D(inputFile, model); // add noise to create initial estimate Values initial; Sampler sampler(42u); Values::ConstFiltered poses = solution->filter(); - SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0)); + SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished()); BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& it, poses) initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise))); diff --git a/timing/timeMatrix.cpp b/timing/timeMatrix.cpp index 7e1c1b5be..d51b0abd2 100644 --- a/timing/timeMatrix.cpp +++ b/timing/timeMatrix.cpp @@ -193,7 +193,7 @@ double timeHouseholder(size_t reps) { -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); + 00, 10, 0, 0, 0,-10, -1).finished(); // perform timing double elapsed; diff --git a/timing/timePinholeCamera.cpp b/timing/timePinholeCamera.cpp index bd39589f4..6f7e5e972 100644 --- a/timing/timePinholeCamera.cpp +++ b/timing/timePinholeCamera.cpp @@ -32,7 +32,7 @@ int main() 1., 0., 0., 0.,-1., 0., 0., 0.,-1. - ), + ).finished(), Point3(0,0,0.5)); static Cal3Bundler K(500, 1e-3, 2.0*1e-3); diff --git a/timing/timePose2.cpp b/timing/timePose2.cpp index 94bd78ef9..06ab633c6 100644 --- a/timing/timePose2.cpp +++ b/timing/timePose2.cpp @@ -61,7 +61,7 @@ Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2, *H1 = (Matrix(3,3) << -c, -s, dt1, s, -c, dt2, - 0.0, 0.0,-1.0); + 0.0, 0.0,-1.0).finished(); } if (H2) *H2 = Matrix::Identity(3,3); @@ -138,7 +138,7 @@ int main() // create a random pose: double x = 4.0, y = 2.0, r = 0.3; - Vector v = (Vector(3) << x, y, r); + Vector v = (Vector(3) << x, y, r).finished(); Pose2 X = Pose2(3,2,0.4), X2 = X.retract(v), X3(5,6,0.3); TEST(Expmap, Pose2::Expmap(v)); diff --git a/timing/timePose3.cpp b/timing/timePose3.cpp index fa8ef7b6c..713d99d09 100644 --- a/timing/timePose3.cpp +++ b/timing/timePose3.cpp @@ -37,8 +37,8 @@ int main() double norm=sqrt(1.0+16.0+4.0); double x=1.0/norm, y=4.0/norm, z=2.0/norm; - Vector v = (Vector(6) << x, y, z, 0.1, 0.2, -0.1); - Pose3 T = Pose3::Expmap((Vector(6) << 0.1, 0.1, 0.2, 0.1, 0.4, 0.2)), T2 = T.retract(v); + Vector v = (Vector(6) << x, y, z, 0.1, 0.2, -0.1).finished(); + Pose3 T = Pose3::Expmap((Vector(6) << 0.1, 0.1, 0.2, 0.1, 0.4, 0.2).finished()), T2 = T.retract(v); Matrix H1,H2; TEST(retract, T.retract(v)) diff --git a/timing/timeRot2.cpp b/timing/timeRot2.cpp index 86dda2b8c..6860914ef 100644 --- a/timing/timeRot2.cpp +++ b/timing/timeRot2.cpp @@ -95,7 +95,7 @@ int main() // create a random direction: double norm=sqrt(16.0+4.0); double x=4.0/norm, y=2.0/norm; - Vector v = (Vector(2) << x, y); + Vector v = (Vector(2) << x, y).finished(); Rot2 R = Rot2(0.4), R2 = R.retract(v), R3(0.6); TEST(Rot2_Expmap, Rot2::Expmap(v)); diff --git a/timing/timeRot3.cpp b/timing/timeRot3.cpp index dd98465ed..4977fba86 100644 --- a/timing/timeRot3.cpp +++ b/timing/timeRot3.cpp @@ -39,7 +39,7 @@ int main() // create a random direction: double norm=sqrt(1.0+16.0+4.0); double x=1.0/norm, y=4.0/norm, z=2.0/norm; - Vector v = (Vector(3) << x, y, z); + Vector v = (Vector(3) << x, y, z).finished(); Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2), R2 = R.retract(v); TEST("Rodriguez formula given axis angle", Rot3::rodriguez(v,0.001)) diff --git a/timing/timeStereoCamera.cpp b/timing/timeStereoCamera.cpp index dce622720..ab8e2fff9 100644 --- a/timing/timeStereoCamera.cpp +++ b/timing/timeStereoCamera.cpp @@ -31,7 +31,7 @@ int main() 1., 0., 0., 0.,-1., 0., 0., 0.,-1. - ), + ).finished(), Point3(0,0,0.5)); const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));