diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 511e7eb3f..957931af4 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -26,28 +26,25 @@ using namespace gtsam; -typedef TypedSymbol PoseKey; - /** * Unary factor for the pose. */ -class ResectioningFactor: public NonlinearFactor1 { - typedef NonlinearFactor1 Base; +class ResectioningFactor: public NonlinearFactor1 { + typedef NonlinearFactor1 Base; shared_ptrK K_; // camera's intrinsic parameters Point3 P_; // 3D point on the calibration rig Point2 p_; // 2D measurement of the 3D point public: - ResectioningFactor(const SharedNoiseModel& model, const PoseKey& key, + ResectioningFactor(const SharedNoiseModel& model, const Symbol& key, const shared_ptrK& calib, const Point2& p, const Point3& P) : Base(model, key), K_(calib), P_(P), p_(p) { } virtual ~ResectioningFactor() {} - virtual Vector evaluateError(const Pose3& X, boost::optional H = - boost::none) const { + virtual Vector evaluateError(const Pose3& X, boost::optional H = boost::none) const { SimpleCamera camera(*K_, X); Point2 reprojectionError(camera.project(P_, H) - p_); return reprojectionError.vector(); @@ -69,7 +66,7 @@ int main(int argc, char* argv[]) { /* create keys for variables */ // we have only 1 variable to solve: the camera pose - PoseKey X(1); + Symbol X('x',1); /* 1. create graph */ NonlinearFactorGraph graph; diff --git a/examples/PlanarSLAMExample_easy.cpp b/examples/PlanarSLAMExample_easy.cpp index d3aecb4b6..1ff3a8677 100644 --- a/examples/PlanarSLAMExample_easy.cpp +++ b/examples/PlanarSLAMExample_easy.cpp @@ -36,25 +36,22 @@ using namespace planarSLAM; * - Landmarks are 2 meters away from the robot trajectory */ int main(int argc, char** argv) { - // create keys for variables - PoseKey x1(1), x2(2), x3(3); - PointKey l1(1), l2(2); - // create graph container and add factors to it + // create graph container and add factors to it Graph graph; /* add prior */ // gaussian for prior SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin - graph.addPrior(x1, prior_measurement, prior_model); // add directly to graph + graph.addPrior(1, prior_measurement, prior_model); // add directly to graph /* add odometry */ // general noisemodel for odometry SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - graph.addOdometry(x1, x2, odom_measurement, odom_model); - graph.addOdometry(x2, x3, odom_measurement, odom_model); + graph.addOdometry(1, 2, odom_measurement, odom_model); + graph.addOdometry(2, 3, odom_measurement, odom_model); /* add measurements */ // general noisemodel for measurements @@ -69,24 +66,24 @@ int main(int argc, char** argv) { range32 = 2.0; // create bearing/range factors and add them - graph.addBearingRange(x1, l1, bearing11, range11, meas_model); - graph.addBearingRange(x2, l1, bearing21, range21, meas_model); - graph.addBearingRange(x3, l2, bearing32, range32, meas_model); + graph.addBearingRange(1, 1, bearing11, range11, meas_model); + graph.addBearingRange(2, 1, bearing21, range21, meas_model); + graph.addBearingRange(3, 2, bearing32, range32, meas_model); graph.print("full graph"); // initialize to noisy points planarSLAM::Values initialEstimate; - initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); - initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); - initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); - initialEstimate.insert(l1, Point2(1.8, 2.1)); - initialEstimate.insert(l2, Point2(4.1, 1.8)); + initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); + initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2)); + initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1)); + initialEstimate.insertPoint(1, Point2(1.8, 2.1)); + initialEstimate.insertPoint(2, Point2(4.1, 1.8)); initialEstimate.print("initial estimate"); // optimize using Levenberg-Marquardt optimization with an ordering from colamd - planarSLAM::Values result = optimize(graph, initialEstimate); + planarSLAM::Values result = optimize(graph, initialEstimate); result.print("final result"); return 0; diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp index fd6d2cbfe..9dc908bc0 100644 --- a/examples/PlanarSLAMSelfContained_advanced.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -40,11 +40,8 @@ #include // Main typedefs -typedef gtsam::TypedSymbol PoseKey; // Key for poses, with type included -typedef gtsam::TypedSymbol PointKey; // Key for points, with type included -typedef gtsam::NonlinearFactorGraph Graph; // graph structure -typedef gtsam::NonlinearOptimizer OptimizerSeqential; // optimization engine for this domain -typedef gtsam::NonlinearOptimizer OptimizerMultifrontal; // optimization engine for this domain +typedef gtsam::NonlinearOptimizer OptimizerSeqential; // optimization engine for this domain +typedef gtsam::NonlinearOptimizer OptimizerMultifrontal; // optimization engine for this domain using namespace std; using namespace gtsam; @@ -60,17 +57,17 @@ using namespace gtsam; */ int main(int argc, char** argv) { // create keys for variables - PoseKey x1(1), x2(2), x3(3); - PointKey l1(1), l2(2); + Symbol x1('x',1), x2('x',2), x3('x',3); + Symbol l1('l',1), l2('l',2); // create graph container and add factors to it - Graph::shared_ptr graph(new Graph); + NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); /* add prior */ // gaussian for prior SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin - PriorFactor posePrior(x1, prior_measurement, prior_model); // create the factor + PriorFactor posePrior(x1, prior_measurement, prior_model); // create the factor graph->add(posePrior); // add the factor to the graph /* add odometry */ @@ -78,8 +75,8 @@ int main(int argc, char** argv) { SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) // create between factors to represent odometry - BetweenFactor odom12(x1, x2, odom_measurement, odom_model); - BetweenFactor odom23(x2, x3, odom_measurement, odom_model); + BetweenFactor odom12(x1, x2, odom_measurement, odom_model); + BetweenFactor odom23(x2, x3, odom_measurement, odom_model); graph->add(odom12); // add both to graph graph->add(odom23); @@ -96,9 +93,9 @@ int main(int argc, char** argv) { range32 = 2.0; // create bearing/range factors - BearingRangeFactor meas11(x1, l1, bearing11, range11, meas_model); - BearingRangeFactor meas21(x2, l1, bearing21, range21, meas_model); - BearingRangeFactor meas32(x3, l2, bearing32, range32, meas_model); + BearingRangeFactor meas11(x1, l1, bearing11, range11, meas_model); + BearingRangeFactor meas21(x2, l1, bearing21, range21, meas_model); + BearingRangeFactor meas32(x3, l2, bearing32, range32, meas_model); // add the factors graph->add(meas11); diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index cab263047..1c196488f 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -33,9 +33,6 @@ using namespace boost; using namespace pose2SLAM; int main(int argc, char** argv) { - // create keys for robot positions - PoseKey x1(1), x2(2), x3(3); - /* 1. create graph container and add factors to it */ shared_ptr graph(new Graph); @@ -43,7 +40,7 @@ int main(int argc, char** argv) { // gaussian for prior SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin - graph->addPrior(x1, prior_measurement, prior_model); // add directly to graph + graph->addPrior(1, prior_measurement, prior_model); // add directly to graph /* 2.b add odometry */ // general noisemodel for odometry @@ -51,16 +48,16 @@ int main(int argc, char** argv) { /* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/ Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - graph->addOdometry(x1, x2, odom_measurement, odom_model); - graph->addOdometry(x2, x3, odom_measurement, odom_model); + graph->addOdometry(1, 2, odom_measurement, odom_model); + graph->addOdometry(2, 3, odom_measurement, odom_model); graph->print("full graph"); /* 3. Create the data structure to hold the initial estimate to the solution * initialize to noisy points */ shared_ptr initial(new pose2SLAM::Values); - initial->insert(x1, Pose2(0.5, 0.0, 0.2)); - initial->insert(x2, Pose2(2.3, 0.1,-0.2)); - initial->insert(x3, Pose2(4.1, 0.1, 0.1)); + initial->insertPose(1, Pose2(0.5, 0.0, 0.2)); + initial->insertPose(2, Pose2(2.3, 0.1,-0.2)); + initial->insertPose(3, Pose2(4.1, 0.1, 0.1)); initial->print("initial estimate"); /* 4.2.1 Alternatively, you can go through the process step by step @@ -76,8 +73,8 @@ int main(int argc, char** argv) { result.print("final result"); /* Get covariances */ - Matrix covariance1 = optimizer_result.marginalCovariance(x1); - Matrix covariance2 = optimizer_result.marginalCovariance(x2); + Matrix covariance1 = optimizer_result.marginalCovariance(PoseKey(1)); + Matrix covariance2 = optimizer_result.marginalCovariance(PoseKey(1)); print(covariance1, "Covariance1"); print(covariance2, "Covariance2"); diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp index 43a648398..2593c043c 100644 --- a/examples/Pose2SLAMExample_easy.cpp +++ b/examples/Pose2SLAMExample_easy.cpp @@ -31,8 +31,6 @@ using namespace gtsam; using namespace pose2SLAM; int main(int argc, char** argv) { - // create keys for robot positions - PoseKey x1(1), x2(2), x3(3); /* 1. create graph container and add factors to it */ Graph graph ; @@ -41,7 +39,7 @@ int main(int argc, char** argv) { // gaussian for prior SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin - graph.addPrior(x1, prior_measurement, prior_model); // add directly to graph + graph.addPrior(1, prior_measurement, prior_model); // add directly to graph /* 2.b add odometry */ // general noisemodel for odometry @@ -49,16 +47,16 @@ int main(int argc, char** argv) { /* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/ Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - graph.addOdometry(x1, x2, odom_measurement, odom_model); - graph.addOdometry(x2, x3, odom_measurement, odom_model); + graph.addOdometry(1, 2, odom_measurement, odom_model); + graph.addOdometry(2, 3, odom_measurement, odom_model); graph.print("full graph"); /* 3. Create the data structure to hold the initial estinmate to the solution * initialize to noisy points */ pose2SLAM::Values initial; - initial.insert(x1, Pose2(0.5, 0.0, 0.2)); - initial.insert(x2, Pose2(2.3, 0.1,-0.2)); - initial.insert(x3, Pose2(4.1, 0.1, 0.1)); + initial.insertPose(1, Pose2(0.5, 0.0, 0.2)); + initial.insertPose(2, Pose2(2.3, 0.1,-0.2)); + initial.insertPose(3, Pose2(4.1, 0.1, 0.1)); initial.print("initial estimate"); /* 4 Single Step Optimization diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index 821b177dc..29bab981a 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -35,25 +35,6 @@ using namespace std; using namespace gtsam; -/** - * Step 1: Setup basic types for optimization of a single variable type - * This can be considered to be specifying the domain of the problem we wish - * to solve. In this case, we will create a very simple domain that operates - * on variables of a specific type, in this case, Rot2. - * - * To create a domain: - * - variable types need to have a key defined to act as a label in graphs - * - a "RotValues" structure needs to be defined to store the system state - * - a graph container acting on a given RotValues - * - * In a typical scenario, these typedefs could be placed in a header - * file and reused between projects. Also, RotValues can be combined to - * form a "TupleValues" to enable multiple variable types, such as 2D points - * and 2D poses. - */ -typedef TypedSymbol Key; /// Variable labels for a specific type -typedef NonlinearFactorGraph Graph; /// Graph container for constraints - needs to know type of variables - const double degree = M_PI / 180; int main() { @@ -64,7 +45,7 @@ int main() { */ /** - * Step 2: create a factor on to express a unary constraint + * Step 1: create a factor on to express a unary constraint * The "prior" in this case is the measurement from a sensor, * with a model of the noise on the measurement. * @@ -80,11 +61,11 @@ int main() { Rot2 prior = Rot2::fromAngle(30 * degree); prior.print("goal angle"); SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree); - Key key(1); - PriorFactor factor(key, prior, model); + Symbol key('x',1); + PriorFactor factor(key, prior, model); /** - * Step 3: create a graph container and add the factor to it + * Step 2: create a graph container and add the factor to it * Before optimizing, all factors need to be added to a Graph container, * which provides the necessary top-level functionality for defining a * system of constraints. @@ -92,12 +73,12 @@ int main() { * In this case, there is only one factor, but in a practical scenario, * many more factors would be added. */ - Graph graph; + NonlinearFactorGraph graph; graph.add(factor); graph.print("full graph"); /** - * Step 4: create an initial estimate + * Step 3: create an initial estimate * An initial estimate of the solution for the system is necessary to * start optimization. This system state is the "RotValues" structure, * which is similar in structure to a STL map, in that it maps @@ -117,14 +98,14 @@ int main() { initial.print("initial estimate"); /** - * Step 5: optimize + * Step 4: optimize * After formulating the problem with a graph of constraints * and an initial estimate, executing optimization is as simple * as calling a general optimization function with the graph and * initial estimate. This will yield a new RotValues structure * with the final state of the optimization. */ - Values result = optimize(graph, initial); + Values result = optimize(graph, initial); result.print("final result"); return 0; diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index d190c38dc..b13058a07 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -30,7 +30,6 @@ using namespace std; using namespace gtsam; // Define Types for Linear System Test -typedef TypedSymbol LinearKey; typedef Point2 LinearMeasurement; int main() { @@ -40,7 +39,7 @@ int main() { SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // Create an ExtendedKalmanFilter object - ExtendedKalmanFilter ekf(x_initial, P_initial); + ExtendedKalmanFilter ekf(x_initial, P_initial); @@ -61,11 +60,11 @@ int main() { // This simple motion can be modeled with a BetweenFactor // Create Keys - LinearKey x0(0), x1(1); + Symbol x0('x',0), x1('x',1); // Predict delta based on controls Point2 difference(1,0); // Create Factor - BetweenFactor factor1(x0, x1, difference, Q); + BetweenFactor factor1(x0, x1, difference, Q); // Predict the new value with the EKF class Point2 x1_predict = ekf.predict(factor1); @@ -86,7 +85,7 @@ int main() { // This simple measurement can be modeled with a PriorFactor Point2 z1(1.0, 0.0); - PriorFactor factor2(x1, z1, R); + PriorFactor factor2(x1, z1, R); // Update the Kalman Filter with the measurement Point2 x1_update = ekf.update(factor2); @@ -96,15 +95,15 @@ int main() { // Do the same thing two more times... // Predict - LinearKey x2(2); + Symbol x2('x',2); difference = Point2(1,0); - BetweenFactor factor3(x1, x2, difference, Q); + BetweenFactor factor3(x1, x2, difference, Q); Point2 x2_predict = ekf.predict(factor1); x2_predict.print("X2 Predict"); // Update Point2 z2(2.0, 0.0); - PriorFactor factor4(x2, z2, R); + PriorFactor factor4(x2, z2, R); Point2 x2_update = ekf.update(factor4); x2_update.print("X2 Update"); @@ -112,15 +111,15 @@ int main() { // Do the same thing one more time... // Predict - LinearKey x3(3); + Symbol x3('x',3); difference = Point2(1,0); - BetweenFactor factor5(x2, x3, difference, Q); + BetweenFactor factor5(x2, x3, difference, Q); Point2 x3_predict = ekf.predict(factor5); x3_predict.print("X3 Predict"); // Update Point2 z3(3.0, 0.0); - PriorFactor factor6(x3, z3, R); + PriorFactor factor6(x3, z3, R); Point2 x3_update = ekf.update(factor6); x3_update.print("X3 Update"); diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index 30d77aedc..1c3415e61 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -33,8 +33,6 @@ using namespace std; using namespace gtsam; -typedef TypedSymbol Key; /// Variable labels for a specific type - int main() { // [code below basically does SRIF with LDL] @@ -55,14 +53,14 @@ int main() { // i.e., we should get 0,0 0,1 0,2 if there is no noise // Create new state variable - Key x0(0); + Symbol x0('x',0); ordering->insert(x0, 0); // Initialize state x0 (2D point) at origin by adding a prior factor, i.e., Bayes net P(x0) // This is equivalent to x_0 and P_0 Point2 x_initial(0,0); SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - PriorFactor factor1(x0, x_initial, P_initial); + PriorFactor factor1(x0, x_initial, P_initial); // Linearize the factor and add it to the linear factor graph linearizationPoints.insert(x0, x_initial); linearFactorGraph->push_back(factor1.linearize(linearizationPoints, *ordering)); @@ -88,12 +86,12 @@ int main() { // so, difference = x_{t+1} - x_{t} = F*x_{t} + B*u_{t} - I*x_{t} // = (F - I)*x_{t} + B*u_{t} // = B*u_{t} (for our example) - Key x1(1); + Symbol x1('x',1); ordering->insert(x1, 1); Point2 difference(1,0); SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - BetweenFactor factor2(x0, x1, difference, Q); + BetweenFactor factor2(x0, x1, difference, Q); // Linearize the factor and add it to the linear factor graph linearizationPoints.insert(x1, x_initial); linearFactorGraph->push_back(factor2.linearize(linearizationPoints, *ordering)); @@ -116,7 +114,7 @@ int main() { // Extract the current estimate of x1,P1 from the Bayes Network VectorValues result = optimize(*linearBayesNet); - Point2 x1_predict = linearizationPoints[x1].retract(result[ordering->at(x1)]); + Point2 x1_predict = linearizationPoints.at(x1).retract(result[ordering->at(x1)]); x1_predict.print("X1 Predict"); // Update the new linearization point to the new estimate @@ -171,7 +169,7 @@ int main() { // This can be modeled using the PriorFactor, where the mean is z_{t} and the covariance is R. Point2 z1(1.0, 0.0); SharedDiagonal R1 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); - PriorFactor factor4(x1, z1, R1); + PriorFactor factor4(x1, z1, R1); // Linearize the factor and add it to the linear factor graph linearFactorGraph->push_back(factor4.linearize(linearizationPoints, *ordering)); @@ -188,7 +186,7 @@ int main() { // Extract the current estimate of x1 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x1_update = linearizationPoints[x1].retract(result[ordering->at(x1)]); + Point2 x1_update = linearizationPoints.at(x1).retract(result[ordering->at(x1)]); x1_update.print("X1 Update"); // Update the linearization point to the new estimate @@ -213,7 +211,7 @@ int main() { linearFactorGraph->add(0, tmpPrior1.getA(tmpPrior1.begin()), tmpPrior1.getb() - tmpPrior1.getA(tmpPrior1.begin()) * result[ordering->at(x1)], tmpPrior1.get_model()); // Create a key for the new state - Key x2(2); + Symbol x2('x',2); // Create the desired ordering ordering = Ordering::shared_ptr(new Ordering); @@ -223,7 +221,7 @@ int main() { // Create a nonlinear factor describing the motion model difference = Point2(1,0); Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - BetweenFactor factor6(x1, x2, difference, Q); + BetweenFactor factor6(x1, x2, difference, Q); // Linearize the factor and add it to the linear factor graph linearizationPoints.insert(x2, x1_update); @@ -235,7 +233,7 @@ int main() { // Extract the current estimate of x2 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x2_predict = linearizationPoints[x2].retract(result[ordering->at(x2)]); + Point2 x2_predict = linearizationPoints.at(x2).retract(result[ordering->at(x2)]); x2_predict.print("X2 Predict"); // Update the linearization point to the new estimate @@ -261,7 +259,7 @@ int main() { // And update using z2 ... Point2 z2(2.0, 0.0); SharedDiagonal R2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); - PriorFactor factor8(x2, z2, R2); + PriorFactor factor8(x2, z2, R2); // Linearize the factor and add it to the linear factor graph linearFactorGraph->push_back(factor8.linearize(linearizationPoints, *ordering)); @@ -279,7 +277,7 @@ int main() { // Extract the current estimate of x2 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x2_update = linearizationPoints[x2].retract(result[ordering->at(x2)]); + Point2 x2_update = linearizationPoints.at(x2).retract(result[ordering->at(x2)]); x2_update.print("X2 Update"); // Update the linearization point to the new estimate @@ -302,7 +300,7 @@ int main() { linearFactorGraph->add(0, tmpPrior3.getA(tmpPrior3.begin()), tmpPrior3.getb() - tmpPrior3.getA(tmpPrior3.begin()) * result[ordering->at(x2)], tmpPrior3.get_model()); // Create a key for the new state - Key x3(3); + Symbol x3('x',3); // Create the desired ordering ordering = Ordering::shared_ptr(new Ordering); @@ -312,7 +310,7 @@ int main() { // Create a nonlinear factor describing the motion model difference = Point2(1,0); Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - BetweenFactor factor10(x2, x3, difference, Q); + BetweenFactor factor10(x2, x3, difference, Q); // Linearize the factor and add it to the linear factor graph linearizationPoints.insert(x3, x2_update); @@ -324,7 +322,7 @@ int main() { // Extract the current estimate of x3 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x3_predict = linearizationPoints[x3].retract(result[ordering->at(x3)]); + Point2 x3_predict = linearizationPoints.at(x3).retract(result[ordering->at(x3)]); x3_predict.print("X3 Predict"); // Update the linearization point to the new estimate @@ -350,7 +348,7 @@ int main() { // And update using z3 ... Point2 z3(3.0, 0.0); SharedDiagonal R3 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); - PriorFactor factor12(x3, z3, R3); + PriorFactor factor12(x3, z3, R3); // Linearize the factor and add it to the linear factor graph linearFactorGraph->push_back(factor12.linearize(linearizationPoints, *ordering)); @@ -368,7 +366,7 @@ int main() { // Extract the current estimate of x2 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x3_update = linearizationPoints[x3].retract(result[ordering->at(x3)]); + Point2 x3_update = linearizationPoints.at(x3).retract(result[ordering->at(x3)]); x3_update.print("X3 Update"); // Update the linearization point to the new estimate diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index be4ddf840..d8395942a 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -153,7 +153,7 @@ public: KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g)); KEY key_to = boost::get(boost::vertex_name, g, boost::target(edge, g)); POSE relativePose = boost::get(boost::edge_weight, g, edge); - config_->insert(key_to, (*config_)[key_from].compose(relativePose)); + config_->insert(key_to, config_->at(key_from).compose(relativePose)); } }; diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index 2c4e382a1..7cd67f6d2 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -27,10 +27,10 @@ namespace gtsam { /* ************************************************************************* */ - template - typename ExtendedKalmanFilter::T ExtendedKalmanFilter::solve_( + template + typename ExtendedKalmanFilter::T ExtendedKalmanFilter::solve_( const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering, - const Values& linearizationPoints, const KEY& lastKey, + const Values& linearizationPoints, const Symbol& lastKey, JacobianFactor::shared_ptr& newPrior) const { // Extract the Index of the provided last key @@ -43,7 +43,7 @@ namespace gtsam { // Extract the current estimate of x1,P1 from the Bayes Network VectorValues result = optimize(*linearBayesNet); - T x = linearizationPoints[lastKey].retract(result[lastIndex]); + T x = linearizationPoints.at(lastKey).retract(result[lastIndex]); // Create a Jacobian Factor from the root node of the produced Bayes Net. // This will act as a prior for the next iteration. @@ -66,8 +66,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - ExtendedKalmanFilter::ExtendedKalmanFilter(T x_initial, + template + ExtendedKalmanFilter::ExtendedKalmanFilter(T x_initial, noiseModel::Gaussian::shared_ptr P_initial) { // Set the initial linearization point to the provided mean @@ -82,8 +82,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename ExtendedKalmanFilter::T ExtendedKalmanFilter::predict( + template + typename ExtendedKalmanFilter::T ExtendedKalmanFilter::predict( const MotionFactor& motionFactor) { // TODO: This implementation largely ignores the actual factor symbols. @@ -91,8 +91,8 @@ namespace gtsam { // different keys will still compute as if a common key-set was used // Create Keys - KEY x0 = motionFactor.key1(); - KEY x1 = motionFactor.key2(); + Symbol x0 = motionFactor.key1(); + Symbol x1 = motionFactor.key2(); // Create an elimination ordering Ordering ordering; @@ -122,8 +122,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename ExtendedKalmanFilter::T ExtendedKalmanFilter::update( + template + typename ExtendedKalmanFilter::T ExtendedKalmanFilter::update( const MeasurementFactor& measurementFactor) { // TODO: This implementation largely ignores the actual factor symbols. @@ -131,7 +131,7 @@ namespace gtsam { // different keys will still compute as if a common key-set was used // Create Keys - KEY x0 = measurementFactor.key(); + Symbol x0 = measurementFactor.key(); // Create an elimination ordering Ordering ordering; diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index 6e85d6e27..bdfaa7651 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -40,14 +40,14 @@ namespace gtsam { * \nosubgrouping */ - template + template class ExtendedKalmanFilter { public: - typedef boost::shared_ptr > shared_ptr; - typedef typename KEY::Value T; - typedef NonlinearFactor2 MotionFactor; - typedef NonlinearFactor1 MeasurementFactor; + typedef boost::shared_ptr > shared_ptr; + typedef VALUE T; + typedef NonlinearFactor2 MotionFactor; + typedef NonlinearFactor1 MeasurementFactor; protected: T x_; // linearization point @@ -55,7 +55,7 @@ namespace gtsam { T solve_(const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering, const Values& linearizationPoints, - const KEY& x, JacobianFactor::shared_ptr& newPrior) const; + const Symbol& x, JacobianFactor::shared_ptr& newPrior) const; public: diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index a95044dee..1a1fce839 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -219,7 +219,7 @@ namespace gtsam { /** Print */ virtual void print(const std::string& s = "") const { std::cout << s << ": NonlinearEquality1(" - << (std::string) this->key_ << "),"<< "\n"; + << (std::string) this->key() << "),"<< "\n"; this->noiseModel_->print(); value_.print("Value"); } diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index 3c28afa07..42f42e429 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -95,18 +95,18 @@ private: * Binary scalar inequality constraint, with a similar value() function * to implement for specific systems */ -template -struct BoundingConstraint2: public NonlinearFactor2 { - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; +template +struct BoundingConstraint2: public NonlinearFactor2 { + typedef VALUE1 X1; + typedef VALUE2 X2; - typedef NonlinearFactor2 Base; - typedef boost::shared_ptr > shared_ptr; + typedef NonlinearFactor2 Base; + typedef boost::shared_ptr > shared_ptr; double threshold_; bool isGreaterThan_; /// flag for greater/less than - BoundingConstraint2(const KEY1& key1, const KEY2& key2, double threshold, + BoundingConstraint2(const Symbol& key1, const Symbol& key2, double threshold, bool isGreaterThan, double mu = 1000.0) : Base(noiseModel::Constrained::All(1, mu), key1, key2), threshold_(threshold), isGreaterThan_(isGreaterThan) {} @@ -127,7 +127,7 @@ struct BoundingConstraint2: public NonlinearFactor2 { /** active when constraint *NOT* met */ bool active(const Values& c) const { // note: still active at equality to avoid zigzagging - double x = value(c[this->key1_], c[this->key2_]); + double x = value(c[this->key1()], c[this->key2()]); return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; } diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 60092c15d..868b064d7 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -53,7 +53,7 @@ namespace gtsam { * @param j is the index of the landmark */ GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, const Symbol& cameraKey, const Symbol& landmarkKey) : - Base(model, cameraKey, cameraKey), measured_(measured) {} + Base(model, cameraKey, landmarkKey), measured_(measured) {} GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2 diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp index f7e562640..db3d6b146 100644 --- a/gtsam/slam/planarSLAM.cpp +++ b/gtsam/slam/planarSLAM.cpp @@ -37,7 +37,7 @@ namespace planarSLAM { } void Graph::addOdometry(Index i, Index j, const Pose2& odometry, const SharedNoiseModel& model) { - sharedFactor factor(new Odometry(PoseKey(i), PointKey(j), odometry, model)); + sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), odometry, model)); push_back(factor); } diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 5c9c62d46..33dc97a8c 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -97,7 +97,7 @@ namespace planarSLAM { void addPoseConstraint(Index poseKey, const Pose2& pose); /// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement) - void addOdometry(Index poseKey, Index pointKey, const Pose2& odometry, const SharedNoiseModel& model); + void addOdometry(Index poseKey1, Index poseKey2, const Pose2& odometry, const SharedNoiseModel& model); /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation void addBearing(Index poseKey, Index pointKey, const Rot2& bearing, const SharedNoiseModel& model); diff --git a/gtsam/slam/simulated2DConstraints.h b/gtsam/slam/simulated2DConstraints.h index 99475ce91..5a6b39861 100644 --- a/gtsam/slam/simulated2DConstraints.h +++ b/gtsam/slam/simulated2DConstraints.h @@ -147,7 +147,7 @@ namespace simulated2D { } }; - typedef MaxDistanceConstraint PoseMaxDistConstraint; ///< Simulated2D domain example factor + typedef MaxDistanceConstraint PoseMaxDistConstraint; ///< Simulated2D domain example factor /** * Binary inequality constraint forcing a minimum range diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 022a76960..8c59d63ac 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -44,7 +44,7 @@ public: } void addPoint3Constraint(int j, const Point3& p) { - boost::shared_ptr factor(new Point3Constraint(Symbol('x',j), p)); + boost::shared_ptr factor(new Point3Constraint(Symbol('l',j), p)); push_back(factor); } @@ -89,8 +89,7 @@ TEST( GeneralSFMFactor, equals ) TEST( GeneralSFMFactor, error ) { Point2 z(3.,0.); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr - factor(new Projection(z, sigma, "x1", "l1")); + boost::shared_ptr factor(new Projection(z, sigma, "x1", "l1")); // For the following configuration, the factor predicts 320,240 Values values; Rot3 R; @@ -167,13 +166,13 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { const double noise = baseline*0.1; boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(PointKey(i), pt) ; + values->insert(Symbol('l',i), pt) ; } graph->addCameraConstraint(0, X[0]); @@ -206,7 +205,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { const double noise = baseline*0.1; boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; // add noise only to the first landmark for ( size_t i = 0 ; i < L.size() ; ++i ) { @@ -214,10 +213,10 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(PointKey(i), pt) ; + values->insert(Symbol('l',i), pt) ; } else { - values->insert(PointKey(i), L[i]) ; + values->insert(Symbol('l',i), L[i]) ; } } @@ -252,14 +251,14 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); //Point3 pt(L[i].x(), L[i].y(), L[i].z()); - values->insert(PointKey(i), pt) ; + values->insert(Symbol('l',i), pt) ; } for ( size_t i = 0 ; i < X.size() ; ++i ) @@ -301,7 +300,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { focal_noise = 1, skew_noise = 1e-5; if ( i == 0 ) { - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; } else { @@ -312,12 +311,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { skew_noise, // s trans_noise, trans_noise // ux, uy ) ; - values->insert(CameraKey((int)i), X[i].retract(delta)) ; + values->insert(Symbol('x',i), X[i].retract(delta)) ; } } for ( size_t i = 0 ; i < L.size() ; ++i ) { - values->insert(PointKey(i), L[i]) ; + values->insert(Symbol('l',i), L[i]) ; } // fix X0 and all landmarks, allow only the X[1] to move @@ -357,14 +356,14 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { const double noise = baseline*0.1; boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; // add noise only to the first landmark for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(PointKey(i), pt) ; + values->insert(Symbol('l',i), pt) ; } graph->addCameraConstraint(0, X[0]); diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 7c1d395ba..6b1ace287 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -207,7 +207,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { const double noise = baseline*0.1; boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; // add noise only to the first landmark for ( size_t i = 0 ; i < L.size() ; ++i ) { @@ -215,10 +215,10 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(PointKey(i), pt) ; + values->insert(Symbol('l',i), pt) ; } else { - values->insert(PointKey(i), L[i]) ; + values->insert(Symbol('l',i), L[i]) ; } } @@ -253,14 +253,14 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); //Point3 pt(L[i].x(), L[i].y(), L[i].z()); - values->insert(PointKey(i), pt) ; + values->insert(Symbol('l',i), pt) ; } for ( size_t i = 0 ; i < X.size() ; ++i ) @@ -300,7 +300,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1, distort_noise = 1e-3; if ( i == 0 ) { - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; } else { @@ -309,12 +309,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { trans_noise, trans_noise, trans_noise, // translation focal_noise, distort_noise, distort_noise // f, k1, k2 ) ; - values->insert(CameraKey((int)i), X[i].retract(delta)) ; + values->insert(Symbol('x',i), X[i].retract(delta)) ; } } for ( size_t i = 0 ; i < L.size() ; ++i ) { - values->insert(PointKey(i), L[i]) ; + values->insert(Symbol('l',i), L[i]) ; } // fix X0 and all landmarks, allow only the X[1] to move @@ -353,14 +353,14 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { const double noise = baseline*0.1; boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; // add noise only to the first landmark for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(PointKey(i), pt) ; + values->insert(Symbol('l',i), pt) ; } graph->addCameraConstraint(0, X[0]); diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index a9b743145..21c0fd458 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -131,12 +131,12 @@ TEST( Pose3Factor, error ) // Create factor SharedNoiseModel I6(noiseModel::Unit::Create(6)); - Pose3Factor factor(1,2, z, I6); + Pose3Factor factor(PoseKey(1), PoseKey(2), z, I6); // Create config Values x; - x.insert(Key(1),t1); - x.insert(Key(2),t2); + x.insert(PoseKey(1),t1); + x.insert(PoseKey(2),t2); // Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2)) Vector actual = factor.unwhitenedError(x); @@ -146,10 +146,10 @@ TEST( Pose3Factor, error ) /* ************************************************************************* */ TEST(Pose3Graph, partial_prior_xy) { - typedef PartialPriorFactor Partial; + typedef PartialPriorFactor Partial; // XY prior - full mask interface - pose3SLAM::Key key(1); + Symbol key = PoseKey(1); Vector exp_xy = Vector_(2, 3.0, 4.0); SharedDiagonal model = noiseModel::Unit::Create(2); Pose3 init(Rot3(), Point3(1.0,-2.0, 3.0)), expected(Rot3(), Point3(3.0, 4.0, 3.0)); @@ -169,7 +169,7 @@ TEST(Pose3Graph, partial_prior_xy) { values.insert(key, init); Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values); - EXPECT(assert_equal(expected, actual[key], tol)); + EXPECT(assert_equal(expected, actual.at(key), tol)); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } @@ -185,10 +185,10 @@ TEST( Values, pose3Circle ) { // expected is 4 poses tangent to circle with radius 1m Values expected; - expected.insert(Key(0), Pose3(R1, Point3( 1, 0, 0))); - expected.insert(Key(1), Pose3(R2, Point3( 0, 1, 0))); - expected.insert(Key(2), Pose3(R3, Point3(-1, 0, 0))); - expected.insert(Key(3), Pose3(R4, Point3( 0,-1, 0))); + expected.insert(PoseKey(0), Pose3(R1, Point3( 1, 0, 0))); + expected.insert(PoseKey(1), Pose3(R2, Point3( 0, 1, 0))); + expected.insert(PoseKey(2), Pose3(R3, Point3(-1, 0, 0))); + expected.insert(PoseKey(3), Pose3(R4, Point3( 0,-1, 0))); Values actual = pose3SLAM::circle(4,1.0); CHECK(assert_equal(expected,actual)); @@ -198,10 +198,10 @@ TEST( Values, pose3Circle ) TEST( Values, expmap ) { Values expected; - expected.insert(Key(0), Pose3(R1, Point3( 1.0, 0.1, 0))); - expected.insert(Key(1), Pose3(R2, Point3(-0.1, 1.0, 0))); - expected.insert(Key(2), Pose3(R3, Point3(-1.0,-0.1, 0))); - expected.insert(Key(3), Pose3(R4, Point3( 0.1,-1.0, 0))); + expected.insert(PoseKey(0), Pose3(R1, Point3( 1.0, 0.1, 0))); + expected.insert(PoseKey(1), Pose3(R2, Point3(-0.1, 1.0, 0))); + expected.insert(PoseKey(2), Pose3(R3, Point3(-1.0,-0.1, 0))); + expected.insert(PoseKey(3), Pose3(R4, Point3( 0.1,-1.0, 0))); Ordering ordering(*expected.orderingArbitrary()); VectorValues delta(expected.dims(ordering)); diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index 4ecbf2e43..df137ad47 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -85,8 +85,8 @@ namespace visualSLAM { * @param K shared pointer to calibration object */ void addMeasurement(const Point2& measured, const SharedNoiseModel& model, - const Symbol& poseKey, const Symbol& pointKey, const shared_ptrK& K) { - boost::shared_ptr factor(new ProjectionFactor(measured, model, poseKey, pointKey, K)); + Index poseKey, Index pointKey, const shared_ptrK& K) { + boost::shared_ptr factor(new ProjectionFactor(measured, model, PoseKey(poseKey), PointKey(pointKey), K)); push_back(factor); } @@ -95,8 +95,8 @@ namespace visualSLAM { * @param key variable key of the camera pose * @param p to which pose to constrain it to */ - void addPoseConstraint(const Symbol& key, const Pose3& p = Pose3()) { - boost::shared_ptr factor(new PoseConstraint(key, p)); + void addPoseConstraint(Index poseKey, const Pose3& p = Pose3()) { + boost::shared_ptr factor(new PoseConstraint(PoseKey(poseKey), p)); push_back(factor); } @@ -105,8 +105,8 @@ namespace visualSLAM { * @param key variable key of the landmark * @param p point around which soft prior is defined */ - void addPointConstraint(const Symbol& key, const Point3& p = Point3()) { - boost::shared_ptr factor(new PointConstraint(key, p)); + void addPointConstraint(Index pointKey, const Point3& p = Point3()) { + boost::shared_ptr factor(new PointConstraint(PointKey(pointKey), p)); push_back(factor); } @@ -116,8 +116,8 @@ namespace visualSLAM { * @param p around which soft prior is defined * @param model uncertainty model of this prior */ - void addPosePrior(const Symbol& key, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) { - boost::shared_ptr factor(new PosePrior(key, p, model)); + void addPosePrior(Index poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) { + boost::shared_ptr factor(new PosePrior(PoseKey(poseKey), p, model)); push_back(factor); } @@ -127,8 +127,8 @@ namespace visualSLAM { * @param p to which point to constrain it to * @param model uncertainty model of this prior */ - void addPointPrior(const Symbol& key, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) { - boost::shared_ptr factor(new PointPrior(key, p, model)); + void addPointPrior(Index pointKey, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) { + boost::shared_ptr factor(new PointPrior(PointKey(pointKey), p, model)); push_back(factor); } @@ -139,8 +139,8 @@ namespace visualSLAM { * @param range approximate range to landmark * @param model uncertainty model of this prior */ - void addRangeFactor(const Symbol& poseKey, const Symbol& pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) { - push_back(boost::shared_ptr(new RangeFactor(poseKey, pointKey, range, model))); + void addRangeFactor(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) { + push_back(boost::shared_ptr(new RangeFactor(PoseKey(poseKey), PointKey(pointKey), range, model))); } }; // Graph diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index c15070b4c..4fddf1ba0 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -17,6 +17,8 @@ #include +#define GTSAM_MAGIC_KEY + #include #include #include @@ -37,7 +39,7 @@ typedef boost::shared_ptr shared_values; typedef NonlinearOptimizer Optimizer; // some simple inequality constraints -simulated2D::PoseKey key(1); +Symbol key(simulated2D::PoseKey(1)); double mu = 10.0; // greater than iq2D::PoseXInequality constraint1(key, 1.0, true, mu); @@ -151,7 +153,7 @@ TEST( testBoundingConstraint, unary_simple_optimization1) { Point2 start_pt(0.0, 1.0); shared_graph graph(new Graph()); - simulated2D::PoseKey x1(1); + Symbol x1("x1"); graph->add(iq2D::PoseXInequality(x1, 1.0, true)); graph->add(iq2D::PoseYInequality(x1, 2.0, true)); graph->add(simulated2D::Prior(start_pt, soft_model2, x1)); @@ -173,7 +175,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) { Point2 start_pt(2.0, 3.0); shared_graph graph(new Graph()); - simulated2D::PoseKey x1(1); + Symbol x1("x1"); graph->add(iq2D::PoseXInequality(x1, 1.0, false)); graph->add(iq2D::PoseYInequality(x1, 2.0, false)); graph->add(simulated2D::Prior(start_pt, soft_model2, x1)); @@ -189,7 +191,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) { /* ************************************************************************* */ TEST( testBoundingConstraint, MaxDistance_basics) { - simulated2D::PoseKey key1(1), key2(2); + Symbol key1("x1"), key2("x2"); Point2 pt1, pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0); iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu); EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol); @@ -231,7 +233,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) { TEST( testBoundingConstraint, MaxDistance_simple_optimization) { Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0); - simulated2D::PoseKey x1(1), x2(2); + Symbol x1("x1"), x2("x2"); Graph graph; graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1)); @@ -254,8 +256,7 @@ TEST( testBoundingConstraint, MaxDistance_simple_optimization) { /* ************************************************************************* */ TEST( testBoundingConstraint, avoid_demo) { - simulated2D::PoseKey x1(1), x2(2), x3(3); - simulated2D::PointKey l1(1); + Symbol x1("x1"), x2("x2"), x3("x3"), l1("l1"); double radius = 1.0; Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0); Point2 odo(2.0, 0.0); diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 5a87950d0..532305f24 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -16,6 +16,8 @@ #include +#define GTSAM_MAGIC_KEY + #include #include #include @@ -24,9 +26,6 @@ using namespace gtsam; -// Define Types for System Test -typedef TypedSymbol TestKey; - /* ************************************************************************* */ TEST( ExtendedKalmanFilter, linear ) { @@ -35,10 +34,10 @@ TEST( ExtendedKalmanFilter, linear ) { SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // Create an ExtendedKalmanFilter object - ExtendedKalmanFilter ekf(x_initial, P_initial); + ExtendedKalmanFilter ekf(x_initial, P_initial); // Create the TestKeys for our example - TestKey x0(0), x1(1), x2(2), x3(3); + Symbol x0("x0"), x1("x1"), x2("x2"), x3("x3"); // Create the controls and measurement properties for our example double dt = 1.0; @@ -57,30 +56,30 @@ TEST( ExtendedKalmanFilter, linear ) { // Run iteration 1 // Create motion factor - BetweenFactor factor1(x0, x1, difference, Q); + BetweenFactor factor1(x0, x1, difference, Q); Point2 predict1 = ekf.predict(factor1); EXPECT(assert_equal(expected1,predict1)); // Create the measurement factor - PriorFactor factor2(x1, z1, R); + PriorFactor factor2(x1, z1, R); Point2 update1 = ekf.update(factor2); EXPECT(assert_equal(expected1,update1)); // Run iteration 2 - BetweenFactor factor3(x1, x2, difference, Q); + BetweenFactor factor3(x1, x2, difference, Q); Point2 predict2 = ekf.predict(factor3); EXPECT(assert_equal(expected2,predict2)); - PriorFactor factor4(x2, z2, R); + PriorFactor factor4(x2, z2, R); Point2 update2 = ekf.update(factor4); EXPECT(assert_equal(expected2,update2)); // Run iteration 3 - BetweenFactor factor5(x2, x3, difference, Q); + BetweenFactor factor5(x2, x3, difference, Q); Point2 predict3 = ekf.predict(factor5); EXPECT(assert_equal(expected3,predict3)); - PriorFactor factor6(x3, z3, R); + PriorFactor factor6(x3, z3, R); Point2 update3 = ekf.update(factor6); EXPECT(assert_equal(expected3,update3)); @@ -89,12 +88,12 @@ TEST( ExtendedKalmanFilter, linear ) { // Create Motion Model Factor -class NonlinearMotionModel : public NonlinearFactor2 { +class NonlinearMotionModel : public NonlinearFactor2 { public: - typedef TestKey::Value T; + typedef Point2 T; private: - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; typedef NonlinearMotionModel This; protected: @@ -104,7 +103,7 @@ protected: public: NonlinearMotionModel(){} - NonlinearMotionModel(const TestKey& TestKey1, const TestKey& TestKey2) : + NonlinearMotionModel(const Symbol& TestKey1, const Symbol& TestKey2) : Base(noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)), TestKey1, TestKey2), Q_(2,2) { // Initialize motion model parameters: @@ -156,14 +155,14 @@ public: /* print */ virtual void print(const std::string& s = "") const { std::cout << s << ": NonlinearMotionModel\n"; - std::cout << " TestKey1: " << (std::string) key1_ << std::endl; - std::cout << " TestKey2: " << (std::string) key2_ << std::endl; + std::cout << " TestKey1: " << (std::string) key1() << std::endl; + std::cout << " TestKey2: " << (std::string) key2() << std::endl; } /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ - virtual bool equals(const NonlinearFactor2& f, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const This *e = dynamic_cast (&f); - return (e != NULL) && (key1_ == e->key1_) && (key2_ == e->key2_); + return (e != NULL) && (key1() == e->key1()) && (key2() == e->key2()); } /** @@ -182,7 +181,7 @@ public: /** Vector of errors, whitened ! */ Vector whitenedError(const Values& c) const { - return QInvSqrt(c[key1_])*unwhitenedError(c); + return QInvSqrt(c[key1()])*unwhitenedError(c); } /** @@ -191,11 +190,11 @@ public: * Hence b = z - h(x1,x2) = - error_vector(x) */ boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { - const X1& x1 = c[key1_]; - const X2& x2 = c[key2_]; + const X1& x1 = c[key1()]; + const X2& x2 = c[key2()]; Matrix A1, A2; Vector b = -evaluateError(x1, x2, A1, A2); - const Index var1 = ordering[key1_], var2 = ordering[key2_]; + const Index var1 = ordering[key1()], var2 = ordering[key2()]; SharedDiagonal constrained = boost::shared_dynamic_cast(this->noiseModel_); if (constrained.get() != NULL) { @@ -236,13 +235,13 @@ public: }; // Create Measurement Model Factor -class NonlinearMeasurementModel : public NonlinearFactor1 { +class NonlinearMeasurementModel : public NonlinearFactor1 { public: - typedef TestKey::Value T; + typedef Point2 T; private: - typedef NonlinearFactor1 Base; + typedef NonlinearFactor1 Base; typedef NonlinearMeasurementModel This; protected: @@ -253,7 +252,7 @@ protected: public: NonlinearMeasurementModel(){} - NonlinearMeasurementModel(const TestKey& TestKey, Vector z) : + NonlinearMeasurementModel(const Symbol& TestKey, Vector z) : Base(noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)), TestKey), z_(z), R_(1,1) { // Initialize nonlinear measurement model parameters: @@ -294,13 +293,13 @@ public: /* print */ virtual void print(const std::string& s = "") const { std::cout << s << ": NonlinearMeasurementModel\n"; - std::cout << " TestKey: " << (std::string) key_ << std::endl; + std::cout << " TestKey: " << (std::string) key() << std::endl; } /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ - virtual bool equals(const NonlinearFactor1& f, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const This *e = dynamic_cast (&f); - return (e != NULL) && (key_ == e->key_); + return (e != NULL) && Base::equals(f); } /** @@ -319,7 +318,7 @@ public: /** Vector of errors, whitened ! */ Vector whitenedError(const Values& c) const { - return RInvSqrt(c[key_])*unwhitenedError(c); + return RInvSqrt(c[key()])*unwhitenedError(c); } /** @@ -328,10 +327,10 @@ public: * Hence b = z - h(x1) = - error_vector(x) */ boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { - const X& x1 = c[key_]; + const X& x1 = c[key()]; Matrix A1; Vector b = -evaluateError(x1, A1); - const Index var1 = ordering[key_]; + const Index var1 = ordering[key()]; SharedDiagonal constrained = boost::shared_dynamic_cast(this->noiseModel_); if (constrained.get() != NULL) { @@ -345,7 +344,7 @@ public: } /** vector of errors */ - Vector evaluateError(const TestKey::Value& p, boost::optional H1 = boost::none) const { + Vector evaluateError(const T& p, boost::optional H1 = boost::none) const { // error = z - h(p) // H = d error / d p = -d h/ d p = -H Vector z_hat = h(p); @@ -406,17 +405,17 @@ TEST( ExtendedKalmanFilter, nonlinear ) { SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // Create an ExtendedKalmanFilter object - ExtendedKalmanFilter ekf(x_initial, P_initial); + ExtendedKalmanFilter ekf(x_initial, P_initial); // Enter Predict-Update Loop Point2 x_predict, x_update; for(unsigned int i = 0; i < 10; ++i){ // Create motion factor - NonlinearMotionModel motionFactor(TestKey(i-1), TestKey(i)); + NonlinearMotionModel motionFactor(Symbol('x',i-1), Symbol('x',i)); x_predict = ekf.predict(motionFactor); // Create a measurement factor - NonlinearMeasurementModel measurementFactor(TestKey(i), Vector_(1, z[i])); + NonlinearMeasurementModel measurementFactor(Symbol('x',i), Vector_(1, z[i])); x_update = ekf.update(measurementFactor); EXPECT(assert_equal(expected_predict[i],x_predict, 1e-6)); diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index c4f8c69fc..b0a955484 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -202,8 +202,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton) // SETDEBUG("ISAM2 recalculate", true); // Pose and landmark key types from planarSLAM - typedef planarSLAM::PoseKey PoseKey; - typedef planarSLAM::PointKey PointKey; + using planarSLAM::PoseKey; + using planarSLAM::PointKey; // Set up parameters SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); @@ -335,8 +335,8 @@ TEST(ISAM2, slamlike_solution_dogleg) // SETDEBUG("ISAM2 recalculate", true); // Pose and landmark key types from planarSLAM - typedef planarSLAM::PoseKey PoseKey; - typedef planarSLAM::PointKey PointKey; + using planarSLAM::PoseKey; + using planarSLAM::PointKey; // Set up parameters SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); @@ -463,8 +463,8 @@ TEST(ISAM2, slamlike_solution_dogleg) TEST(ISAM2, clone) { // Pose and landmark key types from planarSLAM - typedef planarSLAM::PoseKey PoseKey; - typedef planarSLAM::PointKey PointKey; + using planarSLAM::PoseKey; + using planarSLAM::PointKey; // Set up parameters SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); @@ -636,8 +636,8 @@ TEST(ISAM2, removeFactors) // then removes the 2nd-to-last landmark measurement // Pose and landmark key types from planarSLAM - typedef planarSLAM::PoseKey PoseKey; - typedef planarSLAM::PointKey PointKey; + using planarSLAM::PoseKey; + using planarSLAM::PointKey; // Set up parameters SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); @@ -775,8 +775,8 @@ TEST(ISAM2, constrained_ordering) // SETDEBUG("ISAM2 recalculate", true); // Pose and landmark key types from planarSLAM - typedef planarSLAM::PoseKey PoseKey; - typedef planarSLAM::PointKey PointKey; + using planarSLAM::PoseKey; + using planarSLAM::PointKey; // Set up parameters SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp index 271f625b6..ff1732ec5 100644 --- a/tests/testGaussianJunctionTree.cpp +++ b/tests/testGaussianJunctionTree.cpp @@ -125,8 +125,8 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2) /* ************************************************************************* */ TEST(GaussianJunctionTree, slamlike) { - typedef planarSLAM::PoseKey PoseKey; - typedef planarSLAM::PointKey PointKey; + using planarSLAM::PoseKey; + using planarSLAM::PointKey; Values init; planarSLAM::Graph newfactors; @@ -195,7 +195,7 @@ TEST(GaussianJunctionTree, simpleMarginal) { // Create a simple graph pose2SLAM::Graph fg; - fg.addPrior(pose2SLAM::PoseKey(0), Pose2(), sharedSigma(3, 10.0)); + fg.addPrior(0, Pose2(), sharedSigma(3, 10.0)); fg.addOdometry(0, 1, Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0))); Values init; diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index b41da9286..34439b73a 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -86,16 +86,16 @@ TEST( Graph, composePoses ) graph.addOdometry(2,3, p23, cov); graph.addOdometry(4,3, p43, cov); - PredecessorMap tree; - tree.insert(pose2SLAM::PoseKey(1),2); - tree.insert(pose2SLAM::PoseKey(2),2); - tree.insert(pose2SLAM::PoseKey(3),2); - tree.insert(pose2SLAM::PoseKey(4),3); + PredecessorMap tree; + tree.insert(pose2SLAM::PoseKey(1),pose2SLAM::PoseKey(2)); + tree.insert(pose2SLAM::PoseKey(2),pose2SLAM::PoseKey(2)); + tree.insert(pose2SLAM::PoseKey(3),pose2SLAM::PoseKey(2)); + tree.insert(pose2SLAM::PoseKey(4),pose2SLAM::PoseKey(3)); Pose2 rootPose = p2; boost::shared_ptr actual = composePoses (graph, tree, rootPose); + Pose2, Symbol> (graph, tree, rootPose); Values expected; expected.insert(pose2SLAM::PoseKey(1), p1); diff --git a/tests/testInference.cpp b/tests/testInference.cpp index e530393f8..5c60f08e7 100644 --- a/tests/testInference.cpp +++ b/tests/testInference.cpp @@ -54,12 +54,12 @@ TEST( Inference, marginals2) SharedDiagonal poseModel(sharedSigma(3, 0.1)); SharedDiagonal pointModel(sharedSigma(3, 0.1)); - fg.addPrior(planarSLAM::PoseKey(0), Pose2(), poseModel); - fg.addOdometry(planarSLAM::PoseKey(0), planarSLAM::PoseKey(1), Pose2(1.0,0.0,0.0), poseModel); - fg.addOdometry(planarSLAM::PoseKey(1), planarSLAM::PoseKey(2), Pose2(1.0,0.0,0.0), poseModel); - fg.addBearingRange(planarSLAM::PoseKey(0), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel); - fg.addBearingRange(planarSLAM::PoseKey(1), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel); - fg.addBearingRange(planarSLAM::PoseKey(2), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel); + fg.addPrior(0, Pose2(), poseModel); + fg.addOdometry(0, 1, Pose2(1.0,0.0,0.0), poseModel); + fg.addOdometry(1, 2, Pose2(1.0,0.0,0.0), poseModel); + fg.addBearingRange(0, 0, Rot2(), 1.0, pointModel); + fg.addBearingRange(1, 0, Rot2(), 1.0, pointModel); + fg.addBearingRange(2, 0, Rot2(), 1.0, pointModel); Values init; init.insert(planarSLAM::PoseKey(0), Pose2(0.0,0.0,0.0)); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index bdbb3fdd2..2ceb066e2 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -31,15 +31,14 @@ namespace eq2D = simulated2D::equality_constraints; static const double tol = 1e-5; -typedef TypedSymbol PoseKey; -typedef PriorFactor PosePrior; -typedef NonlinearEquality PoseNLE; +typedef PriorFactor PosePrior; +typedef NonlinearEquality PoseNLE; typedef boost::shared_ptr shared_poseNLE; typedef NonlinearFactorGraph PoseGraph; typedef NonlinearOptimizer PoseOptimizer; -PoseKey key(1); +Symbol key('x',1); /* ************************************************************************* */ TEST ( NonlinearEquality, linearization ) { @@ -60,7 +59,7 @@ TEST ( NonlinearEquality, linearization ) { /* ********************************************************************** */ TEST ( NonlinearEquality, linearization_pose ) { - PoseKey key(1); + Symbol key('x',1); Pose2 value; Values config; config.insert(key, value); @@ -89,7 +88,7 @@ TEST ( NonlinearEquality, linearization_fail ) { /* ********************************************************************** */ TEST ( NonlinearEquality, linearization_fail_pose ) { - PoseKey key(1); + Symbol key('x',1); Pose2 value(2.0, 1.0, 2.0), wrong(2.0, 3.0, 4.0); Values bad_linearize; @@ -105,7 +104,7 @@ TEST ( NonlinearEquality, linearization_fail_pose ) { /* ********************************************************************** */ TEST ( NonlinearEquality, linearization_fail_pose_origin ) { - PoseKey key(1); + Symbol key('x',1); Pose2 value, wrong(2.0, 3.0, 4.0); Values bad_linearize; @@ -155,7 +154,7 @@ TEST ( NonlinearEquality, equals ) { /* ************************************************************************* */ TEST ( NonlinearEquality, allow_error_pose ) { - PoseKey key1(1); + Symbol key1('x',1); Pose2 feasible1(1.0, 2.0, 3.0); double error_gain = 500.0; PoseNLE nle(key1, feasible1, error_gain); @@ -183,7 +182,7 @@ TEST ( NonlinearEquality, allow_error_pose ) { /* ************************************************************************* */ TEST ( NonlinearEquality, allow_error_optimize ) { - PoseKey key1(1); + Symbol key1('x',1); Pose2 feasible1(1.0, 2.0, 3.0); double error_gain = 500.0; PoseNLE nle(key1, feasible1, error_gain); @@ -200,7 +199,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) { // optimize boost::shared_ptr ord(new Ordering()); ord->push_back(key1); - NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-5, 1e-5); + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-5, 1e-5); PoseOptimizer optimizer(graph, init, ord, params); PoseOptimizer result = optimizer.levenbergMarquardt(); @@ -214,7 +213,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) { TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { // create a hard constraint - PoseKey key1(1); + Symbol key1('x',1); Pose2 feasible1(1.0, 2.0, 3.0); // initialize away from the ideal @@ -236,7 +235,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { // optimize boost::shared_ptr ord(new Ordering()); ord->push_back(key1); - NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-5, 1e-5); + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-5, 1e-5); PoseOptimizer optimizer(graph, init, ord, params); PoseOptimizer result = optimizer.levenbergMarquardt(); @@ -258,7 +257,7 @@ typedef NonlinearOptimizer Optimizer; /* ************************************************************************* */ TEST( testNonlinearEqualityConstraint, unary_basics ) { Point2 pt(1.0, 2.0); - simulated2D::PoseKey key(1); + Symbol key1('x',1); double mu = 1000.0; eq2D::UnaryEqualityConstraint constraint(pt, key, mu); @@ -281,7 +280,7 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) { /* ************************************************************************* */ TEST( testNonlinearEqualityConstraint, unary_linearization ) { Point2 pt(1.0, 2.0); - simulated2D::PoseKey key(1); + Symbol key1('x',1); double mu = 1000.0; Ordering ordering; ordering += key; @@ -306,7 +305,7 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { // create a single-node graph with a soft and hard constraint to // ensure that the hard constraint overrides the soft constraint Point2 truth_pt(1.0, 2.0); - simulated2D::PoseKey key(1); + Symbol key('x',1); double mu = 10.0; eq2D::UnaryEqualityConstraint::shared_ptr constraint( new eq2D::UnaryEqualityConstraint(truth_pt, key, mu)); @@ -337,7 +336,7 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { /* ************************************************************************* */ TEST( testNonlinearEqualityConstraint, odo_basics ) { Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); - simulated2D::PoseKey key1(1), key2(2); + Symbol key1('x',1), key2('x',2); double mu = 1000.0; eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); @@ -363,7 +362,7 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) { /* ************************************************************************* */ TEST( testNonlinearEqualityConstraint, odo_linearization ) { Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); - simulated2D::PoseKey key1(1), key2(2); + Symbol key1('x',1), key2('x',2); double mu = 1000.0; Ordering ordering; ordering += key1, key2; @@ -396,7 +395,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { // a hard prior on one variable, and a conflicting soft prior // on the other variable - the constraints should override the soft constraint Point2 truth_pt1(1.0, 2.0), truth_pt2(3.0, 2.0); - simulated2D::PoseKey key1(1), key2(2); + Symbol key1('x',1), key2('x',2); // hard prior on x1 eq2D::UnaryEqualityConstraint::shared_ptr constraint1( @@ -438,8 +437,8 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { shared_graph graph(new Graph()); - simulated2D::PoseKey x1(1), x2(2); - simulated2D::PointKey l1(1), l2(2); + Symbol x1('x',1), x2('x',2); + Symbol l1('l',1), l2('l',2); Point2 pt_x1(1.0, 1.0), pt_x2(5.0, 6.0); graph->add(eq2D::UnaryEqualityConstraint(pt_x1, x1)); @@ -476,8 +475,8 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { shared_graph graph(new Graph()); // keys - simulated2D::PoseKey x1(1), x2(2); - simulated2D::PointKey l1(1), l2(2); + Symbol x1('x',1), x2('x',2); + Symbol l1('l',1), l2('l',2); // constant constraint on x1 Point2 pose1(1.0, 1.0); @@ -526,7 +525,7 @@ typedef visualSLAM::Graph VGraph; typedef NonlinearOptimizer VOptimizer; // factors for visual slam -typedef NonlinearEquality2 Point3Equality; +typedef NonlinearEquality2 Point3Equality; /* ********************************************************************* */ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { @@ -543,8 +542,8 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away // keys - visualSLAM::PoseKey x1(1), x2(2); - visualSLAM::PointKey l1(1), l2(2); + Symbol x1('x',1), x2('x',2); + Symbol l1('l',1), l2('l',2); // create graph VGraph::shared_graph graph(new VGraph()); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 53a2af385..488d406dc 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -40,6 +40,9 @@ using namespace std; using namespace gtsam; using namespace example; +using simulated2D::PoseKey; +using simulated2D::PointKey; + typedef boost::shared_ptr shared_nlf; /* ************************************************************************* */ @@ -49,11 +52,11 @@ TEST( NonlinearFactor, equals ) // create two nonlinear2 factors Point2 z3(0.,-1.); - simulated2D::Measurement f0(z3, sigma, 1,1); + simulated2D::Measurement f0(z3, sigma, PoseKey(1),PointKey(1)); // measurement between x2 and l1 Point2 z4(-1.5, -1.); - simulated2D::Measurement f1(z4, sigma, 2,1); + simulated2D::Measurement f1(z4, sigma, PoseKey(2),PointKey(1)); CHECK(assert_equal(f0,f0)); CHECK(f0.equals(f0)); @@ -199,7 +202,7 @@ TEST( NonlinearFactor, linearize_constraint1 ) SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas); Point2 mu(1., -1.); - Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, 1)); + Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, PoseKey(1))); Values config; config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0)); @@ -219,7 +222,7 @@ TEST( NonlinearFactor, linearize_constraint2 ) SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas); Point2 z3(1.,-1.); - simulated2D::Measurement f0(z3, constraint, 1,1); + simulated2D::Measurement f0(z3, constraint, PoseKey(1),PointKey(1)); Values config; config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0)); @@ -283,7 +286,7 @@ TEST(NonlinearFactor, NonlinearFactor4) { class TestFactor5 : public NonlinearFactor5 { public: typedef NonlinearFactor5 Base; - TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5) {} + TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4", "x5") {} virtual Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, @@ -333,7 +336,7 @@ TEST(NonlinearFactor, NonlinearFactor5) { class TestFactor6 : public NonlinearFactor6 { public: typedef NonlinearFactor6 Base; - TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5, 6) {} + TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4", "x5", "x6") {} virtual Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 2316c5652..09a3d4db0 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -25,15 +25,14 @@ TEST(testNonlinearISAM, markov_chain ) { Sampler sampler(model, 42u); // create initial graph - PoseKey key(0); Pose2 cur_pose; // start at origin Graph start_factors; - start_factors.addPoseConstraint(key, cur_pose); + start_factors.addPoseConstraint(0, cur_pose); planarSLAM::Values init; planarSLAM::Values expected; - init.insert(key, cur_pose); - expected.insert(key, cur_pose); + init.insertPose(0, cur_pose); + expected.insertPose(0, cur_pose); isam.update(start_factors, init); // loop for a period of time to verify memory usage @@ -41,8 +40,7 @@ TEST(testNonlinearISAM, markov_chain ) { Pose2 z(1.0, 2.0, 0.1); for (size_t i=1; i<=nrPoses; ++i) { Graph new_factors; - PoseKey key1(i-1), key2(i); - new_factors.addOdometry(key1, key2, z, model); + new_factors.addOdometry(i-1, i, z, model); planarSLAM::Values new_init; // perform a check on changing orderings @@ -61,16 +59,15 @@ TEST(testNonlinearISAM, markov_chain ) { } cur_pose = cur_pose.compose(z); - new_init.insert(key2, cur_pose.retract(sampler.sample())); - expected.insert(key2, cur_pose); + new_init.insertPose(i, cur_pose.retract(sampler.sample())); + expected.insertPose(i, cur_pose); isam.update(new_factors, new_init); } // verify values - all but the last one should be very close planarSLAM::Values actual = isam.estimate(); for (size_t i=0; i Key; -typedef PriorFactor Prior; -typedef BetweenFactor Between; +typedef PriorFactor Prior; +typedef BetweenFactor Between; typedef NonlinearFactorGraph Graph; /* ************************************************************************* */ @@ -41,11 +40,11 @@ TEST(Rot3, optimize) { Values truth; Values initial; Graph fg; - fg.add(Prior(0, Rot3(), sharedSigma(3, 0.01))); + fg.add(Prior(Symbol('r',0), Rot3(), sharedSigma(3, 0.01))); for(int j=0; j<6; ++j) { - truth.insert(Key(j), Rot3::Rz(M_PI/3.0 * double(j))); - initial.insert(Key(j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2))); - fg.add(Between(Key(j), Key((j+1)%6), Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01))); + truth.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j))); + initial.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2))); + fg.add(Between(Symbol('r',j), Symbol('r',(j+1)%6), Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01))); } NonlinearOptimizationParameters params; diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp index 23551bbae..6ab1ad929 100644 --- a/tests/testSerialization.cpp +++ b/tests/testSerialization.cpp @@ -525,8 +525,8 @@ TEST (Serialization, planar_system) { graph.add(constraint); // text - EXPECT(equalsObj(PoseKey(2))); - EXPECT(equalsObj(PointKey(3))); + EXPECT(equalsObj(PoseKey(2))); + EXPECT(equalsObj(PointKey(3))); EXPECT(equalsObj(values)); EXPECT(equalsObj(prior)); EXPECT(equalsObj(bearing)); @@ -537,8 +537,8 @@ TEST (Serialization, planar_system) { EXPECT(equalsObj(graph)); // xml - EXPECT(equalsXML(PoseKey(2))); - EXPECT(equalsXML(PointKey(3))); + EXPECT(equalsXML(PoseKey(2))); + EXPECT(equalsXML(PointKey(3))); EXPECT(equalsXML(values)); EXPECT(equalsXML(prior)); EXPECT(equalsXML(bearing)); @@ -562,8 +562,8 @@ BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoF TEST (Serialization, visual_system) { using namespace visualSLAM; Values values; - PoseKey x1(1), x2(2); - PointKey l1(1), l2(2); + Symbol x1('x',1), x2('x',2); + Symbol l1('l',1), l2('l',2); Pose3 pose1 = pose3, pose2 = pose3.inverse(); Point3 pt1(1.0, 2.0, 3.0), pt2(4.0, 5.0, 6.0); values.insert(x1, pose1); @@ -574,7 +574,7 @@ TEST (Serialization, visual_system) { boost::shared_ptr K(new Cal3_S2(cal1)); Graph graph; - graph.addMeasurement(Point2(1.0, 2.0), model2, x1, l1, K); + graph.addMeasurement(Point2(1.0, 2.0), model2, 1, 1, K); graph.addPointConstraint(1, pt1); graph.addPointPrior(1, pt2, model3); graph.addPoseConstraint(1, pose1);