diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 07dc69dc7..d26b8b3af 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include @@ -28,7 +28,7 @@ using namespace gtsam; typedef TypedSymbol PoseKey; -typedef LieValues PoseValues; +typedef Values PoseValues; /** * Unary factor for the pose. diff --git a/examples/PlanarSLAMExample_easy.cpp b/examples/PlanarSLAMExample_easy.cpp index f840c443d..d697ae261 100644 --- a/examples/PlanarSLAMExample_easy.cpp +++ b/examples/PlanarSLAMExample_easy.cpp @@ -76,7 +76,7 @@ int main(int argc, char** argv) { graph.print("full graph"); // initialize to noisy points - Values initialEstimate; + 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)); @@ -86,7 +86,7 @@ int main(int argc, char** argv) { initialEstimate.print("initial estimate"); // optimize using Levenberg-Marquardt optimization with an ordering from colamd - 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 df58e557c..549acabae 100644 --- a/examples/PlanarSLAMSelfContained_advanced.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -43,12 +43,12 @@ // Main typedefs typedef gtsam::TypedSymbol PoseKey; // Key for poses, with type included typedef gtsam::TypedSymbol PointKey; // Key for points, with type included -typedef gtsam::LieValues PoseValues; // config type for poses -typedef gtsam::LieValues PointValues; // config type for points -typedef gtsam::TupleValues2 Values; // main config with two variable classes -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::Values PoseValues; // config type for poses +typedef gtsam::Values PointValues; // config type for points +typedef gtsam::TupleValues2 PlanarValues; // main config with two variable classes +typedef gtsam::NonlinearFactorGraph Graph; // graph structure +typedef gtsam::NonlinearOptimizer OptimizerSeqential; // optimization engine for this domain +typedef gtsam::NonlinearOptimizer OptimizerMultifrontal; // optimization engine for this domain using namespace std; using namespace gtsam; @@ -74,7 +74,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 - 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 */ @@ -82,8 +82,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); @@ -100,9 +100,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); @@ -112,7 +112,7 @@ int main(int argc, char** argv) { graph->print("Full Graph"); // initialize to noisy points - boost::shared_ptr initial(new Values); + boost::shared_ptr initial(new PlanarValues); 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)); diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index 2b54b3403..13e419def 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -57,7 +57,7 @@ int main(int argc, char** argv) { /* 3. Create the data structure to hold the initial estimate to the solution * initialize to noisy points */ - shared_ptr initial(new Values); + 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)); @@ -72,7 +72,7 @@ int main(int argc, char** argv) { Optimizer optimizer(graph, initial, ordering, params); Optimizer optimizer_result = optimizer.levenbergMarquardt(); - Values result = *optimizer_result.values(); + pose2SLAM::Values result = *optimizer_result.values(); result.print("final result"); /* Get covariances */ diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp index cf6a5613c..1e48157f0 100644 --- a/examples/Pose2SLAMExample_easy.cpp +++ b/examples/Pose2SLAMExample_easy.cpp @@ -55,7 +55,7 @@ int main(int argc, char** argv) { /* 3. Create the data structure to hold the initial estinmate to the solution * initialize to noisy points */ - Values initial; + 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)); @@ -63,7 +63,7 @@ int main(int argc, char** argv) { /* 4 Single Step Optimization * optimize using Levenberg-Marquardt optimization with an ordering from colamd */ - Values result = optimize(graph, initial); + pose2SLAM::Values result = optimize(graph, initial); result.print("final result"); diff --git a/examples/Pose2SLAMwSPCG_advanced.cpp b/examples/Pose2SLAMwSPCG_advanced.cpp index c63b249dc..4b91d625c 100644 --- a/examples/Pose2SLAMwSPCG_advanced.cpp +++ b/examples/Pose2SLAMwSPCG_advanced.cpp @@ -27,17 +27,17 @@ using namespace gtsam; using namespace pose2SLAM; typedef boost::shared_ptr sharedGraph ; -typedef boost::shared_ptr sharedValue ; +typedef boost::shared_ptr sharedValue ; //typedef NonlinearOptimizer > SPCGOptimizer; -typedef SubgraphSolver Solver; +typedef SubgraphSolver Solver; typedef boost::shared_ptr sharedSolver ; -typedef NonlinearOptimizer SPCGOptimizer; +typedef NonlinearOptimizer SPCGOptimizer; sharedGraph graph; sharedValue initial; -Values result; +pose2SLAM::Values result; /* ************************************************************************* */ int main(void) { @@ -47,7 +47,7 @@ int main(void) { Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9); graph = boost::make_shared() ; - initial = boost::make_shared() ; + initial = boost::make_shared() ; // create a 3 by 3 grid // x3 x6 x9 diff --git a/examples/Pose2SLAMwSPCG_easy.cpp b/examples/Pose2SLAMwSPCG_easy.cpp index a1c1cb57e..d1a33678c 100644 --- a/examples/Pose2SLAMwSPCG_easy.cpp +++ b/examples/Pose2SLAMwSPCG_easy.cpp @@ -27,8 +27,7 @@ using namespace gtsam; using namespace pose2SLAM; Graph graph; -Values initial; -Values result; +pose2SLAM::Values initial, result; /* ************************************************************************* */ int main(void) { diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index 0429a701d..c31e91b5f 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -23,12 +23,12 @@ #include #include #include -#include +#include #include #include /* - * TODO: make factors independent of Values + * TODO: make factors independent of RotValues * TODO: make toplevel documentation * TODO: Clean up nonlinear optimization API */ @@ -44,17 +44,17 @@ using namespace gtsam; * * To create a domain: * - variable types need to have a key defined to act as a label in graphs - * - a "Values" structure needs to be defined to store the system state - * - a graph container acting on a given Values + * - 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, LieValues can be combined to + * 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 LieValues Values; /// Class to store values - acts as a state for the system -typedef NonlinearFactorGraph Graph; /// Graph container for constraints - needs to know type of variables +typedef Values RotValues; /// Class to store values - acts as a state for the system +typedef NonlinearFactorGraph Graph; /// Graph container for constraints - needs to know type of variables const double degree = M_PI / 180; @@ -71,7 +71,7 @@ int main() { * with a model of the noise on the measurement. * * The "Key" created here is a label used to associate parts of the - * state (stored in "Values") with particular factors. They require + * state (stored in "RotValues") with particular factors. They require * an index to allow for lookup, and should be unique. * * In general, creating a factor requires: @@ -83,7 +83,7 @@ int main() { prior.print("goal angle"); SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree); Key key(1); - PriorFactor factor(key, prior, model); + PriorFactor factor(key, prior, model); /** * Step 3: create a graph container and add the factor to it @@ -101,7 +101,7 @@ int main() { /** * Step 4: create an initial estimate * An initial estimate of the solution for the system is necessary to - * start optimization. This system state is the "Values" structure, + * start optimization. This system state is the "RotValues" structure, * which is similar in structure to a STL map, in that it maps * keys (the label created in step 1) to specific values. * @@ -110,11 +110,11 @@ int main() { * all of the variables in the graph have a corresponding value in * this structure. * - * The interface to all Values types is the same, it only depends + * The interface to all RotValues types is the same, it only depends * on the type of key used to find the appropriate value map if there * are multiple types of variables. */ - Values initial; + RotValues initial; initial.insert(key, Rot2::fromAngle(20 * degree)); initial.print("initial estimate"); @@ -123,10 +123,10 @@ int main() { * 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 Values structure + * initial estimate. This will yield a new RotValues structure * with the final state of the optimization. */ - Values result = optimize(graph, initial); + RotValues result = optimize(graph, initial); result.print("final result"); return 0; diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index 5081f6356..3b8ee2980 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include using namespace std; @@ -32,7 +32,7 @@ using namespace gtsam; // Define Types for Linear System Test typedef TypedSymbol LinearKey; -typedef LieValues LinearValues; +typedef Values LinearValues; typedef Point2 LinearMeasurement; int main() { diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index 6b6e4fb30..73efc653c 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include #include @@ -35,7 +35,7 @@ using namespace std; using namespace gtsam; typedef TypedSymbol Key; /// Variable labels for a specific type -typedef LieValues Values; /// Class to store values - acts as a state for the system +typedef Values KalmanValues; /// Class to store values - acts as a state for the system int main() { @@ -48,7 +48,7 @@ int main() { Ordering::shared_ptr ordering(new Ordering); // Create a structure to hold the linearization points - Values linearizationPoints; + KalmanValues linearizationPoints; // Ground truth example // Start at origin, move to the right (x-axis): 0,0 0,1 0,2 @@ -64,7 +64,7 @@ int main() { // 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)); @@ -95,7 +95,7 @@ int main() { 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)); @@ -173,7 +173,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)); @@ -225,7 +225,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); @@ -263,7 +263,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)); @@ -314,7 +314,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); @@ -352,7 +352,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)); diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp index 1e5040509..300124c4e 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -81,7 +81,7 @@ void readAllDataISAM() { /** * Setup newFactors and initialValues for each new pose and set of measurements at each frame. */ -void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& initialValues, +void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& initialValues, int pose_id, Pose3& pose, std::vector& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { // Create a graph of newFactors with new measurements @@ -102,7 +102,7 @@ void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& } // Create initial values for all nodes in the newFactors - initialValues = shared_ptr (new Values()); + initialValues = shared_ptr (new visualSLAM::Values()); initialValues->insert(pose_id, pose); for (size_t i = 0; i < measurements.size(); i++) { initialValues->insert(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]); @@ -125,17 +125,17 @@ int main(int argc, char* argv[]) { // Create an NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates int relinearizeInterval = 3; - NonlinearISAM isam(relinearizeInterval); + NonlinearISAM isam(relinearizeInterval); // At each frame i with new camera pose and new set of measurements associated with it, // create a graph of new factors and update ISAM for (size_t i = 0; i < g_measurements.size(); i++) { shared_ptr newFactors; - shared_ptr initialValues; + shared_ptr initialValues; createNewFactors(newFactors, initialValues, i, g_poses[i], g_measurements[i], measurementSigma, g_calib); isam.update(*newFactors, *initialValues); - Values currentEstimate = isam.estimate(); + visualSLAM::Values currentEstimate = isam.estimate(); cout << "****************************************************" << endl; currentEstimate.print("Current estimate: "); } diff --git a/examples/vSLAMexample/vSFMexample.cpp b/examples/vSLAMexample/vSFMexample.cpp index 8027fa571..6aaf04442 100644 --- a/examples/vSLAMexample/vSFMexample.cpp +++ b/examples/vSLAMexample/vSFMexample.cpp @@ -103,9 +103,9 @@ Graph setupGraph(std::vector& measurements, SharedNoiseModel measurem * Create a structure of initial estimates for all nodes (landmarks and poses) in the graph. * The returned Values structure contains all initial values for all nodes. */ -Values initialize(std::map landmarks, std::map poses) { +visualSLAM::Values initialize(std::map landmarks, std::map poses) { - Values initValues; + visualSLAM::Values initValues; // Initialize landmarks 3D positions. for (map::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++) @@ -136,7 +136,7 @@ int main(int argc, char* argv[]) { boost::shared_ptr graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib))); // Create an initial Values structure using groundtruth values as the initial estimates - boost::shared_ptr initialEstimates(new Values(initialize(g_landmarks, g_poses))); + boost::shared_ptr initialEstimates(new visualSLAM::Values(initialize(g_landmarks, g_poses))); cout << "*******************************************************" << endl; initialEstimates->print("INITIAL ESTIMATES: "); @@ -148,7 +148,7 @@ int main(int argc, char* argv[]) { // Optimize the graph cout << "*******************************************************" << endl; NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newVerbosity(Optimizer::Parameters::DAMPED); - Optimizer::shared_values result = Optimizer::optimizeGN(graph, initialEstimates, params); + visualSLAM::Optimizer::shared_values result = visualSLAM::Optimizer::optimizeGN(graph, initialEstimates, params); // Print final results cout << "*******************************************************" << endl; diff --git a/gtsam/nonlinear/DoglegOptimizer.h b/gtsam/nonlinear/DoglegOptimizer.h index b4ea90f3d..62ff72b05 100644 --- a/gtsam/nonlinear/DoglegOptimizer.h +++ b/gtsam/nonlinear/DoglegOptimizer.h @@ -15,7 +15,7 @@ namespace gtsam { * This class is functional, meaning every method is \c const, and returns a new * copy of the class. * - * \tparam VALUES The LieValues or TupleValues type to hold the values to be + * \tparam VALUES The Values or TupleValues type to hold the values to be * estimated. * * \tparam GAUSSIAN_SOLVER The linear solver to use at each iteration, diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index 1f23352b3..ca43b25bc 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -18,7 +18,7 @@ namespace gtsam { * here matches that in "trustregion.pdf" in gtsam_experimental/doc, see this * file for further explanation of the computations performed by this class. * - * \tparam VALUES The LieValues or TupleValues type to hold the values to be + * \tparam VALUES The Values or TupleValues type to hold the values to be * estimated. * * \tparam GAUSSIAN_SOLVER The linear solver to use at each iteration, @@ -60,7 +60,7 @@ struct DoglegOptimizerImpl { * @tparam M The type of the Bayes' net or tree, currently * either BayesNet (or GaussianBayesNet) or BayesTree. * @tparam F For normal usage this will be NonlinearFactorGraph. - * @tparam VALUES The LieValues or TupleValues to pass to F::error() to evaluate + * @tparam VALUES The Values or TupleValues to pass to F::error() to evaluate * the error function. * @param initialDelta The initial trust region radius. * @param Rd The Bayes' net or tree as described above. diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index 00f637d1e..943e53aa1 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -24,7 +24,7 @@ namespace gtsam { * GaussianConditional, and the values on which that GaussianISAM2 is * templated. * - * @tparam VALUES The LieValues or TupleValues\Emph{N} that contains the + * @tparam VALUES The Values or TupleValues\Emph{N} that contains the * variables. */ template diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index c187fd6f8..8629379fc 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -16,8 +16,8 @@ check_PROGRAMS = #---------------------------------------------------------------------------------------------------- # Lie Groups -headers += LieValues.h LieValues-inl.h TupleValues.h TupleValues-inl.h -check_PROGRAMS += tests/testLieValues tests/testKey tests/testOrdering +headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h +check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering # Nonlinear nonlinear headers += Key.h diff --git a/gtsam/nonlinear/TupleValues-inl.h b/gtsam/nonlinear/TupleValues-inl.h index f398cd799..09e510093 100644 --- a/gtsam/nonlinear/TupleValues-inl.h +++ b/gtsam/nonlinear/TupleValues-inl.h @@ -18,7 +18,7 @@ #pragma once -#include +#include #include // TupleValues instantiations for N = 1-6 diff --git a/gtsam/nonlinear/TupleValues.h b/gtsam/nonlinear/TupleValues.h index c4d701c35..a809c360f 100644 --- a/gtsam/nonlinear/TupleValues.h +++ b/gtsam/nonlinear/TupleValues.h @@ -16,7 +16,7 @@ * @author Alex Cunningham */ -#include +#include #include #pragma once @@ -24,11 +24,11 @@ namespace gtsam { /** - * TupleValues are a structure to manage heterogenous LieValues, so as to + * TupleValues are a structure to manage heterogenous Values, so as to * enable different types of variables/keys to be used simultaneously. We * do this with recursive templates (instead of blind pointer casting) to * reduce run-time overhead and keep static type checking. The interface - * mimics that of a single LieValues. + * mimics that of a single Values. * * This uses a recursive structure of values pairs to form a lisp-like * list, with a special case (TupleValuesEnd) that contains only one values diff --git a/gtsam/nonlinear/LieValues-inl.h b/gtsam/nonlinear/Values-inl.h similarity index 75% rename from gtsam/nonlinear/LieValues-inl.h rename to gtsam/nonlinear/Values-inl.h index b5975e352..4a4e1898f 100644 --- a/gtsam/nonlinear/LieValues-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file LieValues.cpp + * @file Values.cpp * @author Richard Roberts */ @@ -28,9 +28,9 @@ #include #include -#include +#include -#define INSTANTIATE_LIE_VALUES(J) template class LieValues; +#define INSTANTIATE_VALUES(J) template class Values; using namespace std; @@ -38,8 +38,8 @@ namespace gtsam { /* ************************************************************************* */ template - void LieValues::print(const string &s) const { - cout << "LieValues " << s << ", size " << values_.size() << "\n"; + void Values::print(const string &s) const { + cout << "Values " << s << ", size " << values_.size() << "\n"; BOOST_FOREACH(const KeyValuePair& v, values_) { gtsam::print(v.second, (string)(v.first)); } @@ -47,7 +47,7 @@ namespace gtsam { /* ************************************************************************* */ template - bool LieValues::equals(const LieValues& expected, double tol) const { + bool Values::equals(const Values& expected, double tol) const { if (values_.size() != expected.values_.size()) return false; BOOST_FOREACH(const KeyValuePair& v, values_) { if (!expected.exists(v.first)) return false; @@ -59,15 +59,15 @@ namespace gtsam { /* ************************************************************************* */ template - const typename J::Value& LieValues::at(const J& j) const { + const typename J::Value& Values::at(const J& j) const { const_iterator it = values_.find(j); - if (it == values_.end()) throw std::invalid_argument("LieValues::at() invalid j: " + (string)j); + if (it == values_.end()) throw std::invalid_argument("Values::at() invalid j: " + (string)j); else return it->second; } /* ************************************************************************* */ template - size_t LieValues::dim() const { + size_t Values::dim() const { size_t n = 0; BOOST_FOREACH(const KeyValuePair& value, values_) n += value.second.dim(); @@ -76,26 +76,26 @@ namespace gtsam { /* ************************************************************************* */ template - VectorValues LieValues::zero(const Ordering& ordering) const { + VectorValues Values::zero(const Ordering& ordering) const { return VectorValues::Zero(this->dims(ordering)); } /* ************************************************************************* */ template - void LieValues::insert(const J& name, const typename J::Value& val) { + void Values::insert(const J& name, const typename J::Value& val) { values_.insert(make_pair(name, val)); } /* ************************************************************************* */ template - void LieValues::insert(const LieValues& cfg) { + void Values::insert(const Values& cfg) { BOOST_FOREACH(const KeyValuePair& v, cfg.values_) insert(v.first, v.second); } /* ************************************************************************* */ template - void LieValues::update(const LieValues& cfg) { + void Values::update(const Values& cfg) { BOOST_FOREACH(const KeyValuePair& v, values_) { boost::optional t = cfg.exists_(v.first); if (t) values_[v.first] = *t; @@ -104,13 +104,13 @@ namespace gtsam { /* ************************************************************************* */ template - void LieValues::update(const J& j, const typename J::Value& val) { + void Values::update(const J& j, const typename J::Value& val) { values_[j] = val; } /* ************************************************************************* */ template - std::list LieValues::keys() const { + std::list Values::keys() const { std::list ret; BOOST_FOREACH(const KeyValuePair& v, values_) ret.push_back(v.first); @@ -119,16 +119,16 @@ namespace gtsam { /* ************************************************************************* */ template - void LieValues::erase(const J& j) { + void Values::erase(const J& j) { size_t dim; // unused erase(j, dim); } /* ************************************************************************* */ template - void LieValues::erase(const J& j, size_t& dim) { + void Values::erase(const J& j, size_t& dim) { iterator it = values_.find(j); - if (it == values_.end()) throw std::invalid_argument("LieValues::erase() invalid j: " + (string)j); + if (it == values_.end()) throw std::invalid_argument("Values::erase() invalid j: " + (string)j); dim = it->second.dim(); values_.erase(it); } @@ -136,8 +136,8 @@ namespace gtsam { /* ************************************************************************* */ // todo: insert for every element is inefficient template - LieValues LieValues::retract(const VectorValues& delta, const Ordering& ordering) const { - LieValues newValues; + Values Values::retract(const VectorValues& delta, const Ordering& ordering) const { + Values newValues; typedef pair KeyValue; BOOST_FOREACH(const KeyValue& value, this->values_) { const J& j = value.first; @@ -153,7 +153,7 @@ namespace gtsam { /* ************************************************************************* */ template - std::vector LieValues::dims(const Ordering& ordering) const { + std::vector Values::dims(const Ordering& ordering) const { _ValuesDimensionCollector dimCollector(ordering); this->apply(dimCollector); return dimCollector.dimensions; @@ -161,7 +161,7 @@ namespace gtsam { /* ************************************************************************* */ template - Ordering::shared_ptr LieValues::orderingArbitrary(Index firstVar) const { + Ordering::shared_ptr Values::orderingArbitrary(Index firstVar) const { // Generate an initial key ordering in iterator order _ValuesKeyOrderer keyOrderer(firstVar); this->apply(keyOrderer); @@ -171,12 +171,12 @@ namespace gtsam { // /* ************************************************************************* */ // // todo: insert for every element is inefficient // template -// LieValues LieValues::retract(const Vector& delta) const { +// Values Values::retract(const Vector& delta) const { // if(delta.size() != dim()) { -// cout << "LieValues::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl; +// cout << "Values::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl; // throw invalid_argument("Delta vector length does not match config dimensionality."); // } -// LieValues newValues; +// Values newValues; // int delta_offset = 0; // typedef pair KeyValue; // BOOST_FOREACH(const KeyValue& value, this->values_) { @@ -193,7 +193,7 @@ namespace gtsam { // todo: insert for every element is inefficient // todo: currently only logmaps elements in both configs template - inline VectorValues LieValues::localCoordinates(const LieValues& cp, const Ordering& ordering) const { + inline VectorValues Values::localCoordinates(const Values& cp, const Ordering& ordering) const { VectorValues delta(this->dims(ordering)); localCoordinates(cp, ordering, delta); return delta; @@ -201,7 +201,7 @@ namespace gtsam { /* ************************************************************************* */ template - void LieValues::localCoordinates(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const { + void Values::localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const { typedef pair KeyValue; BOOST_FOREACH(const KeyValue& value, cp) { assert(this->exists(value.first)); diff --git a/gtsam/nonlinear/LieValues.h b/gtsam/nonlinear/Values.h similarity index 89% rename from gtsam/nonlinear/LieValues.h rename to gtsam/nonlinear/Values.h index 6d06cad43..22ab390a4 100644 --- a/gtsam/nonlinear/LieValues.h +++ b/gtsam/nonlinear/Values.h @@ -10,14 +10,14 @@ * -------------------------------------------------------------------------- */ /** - * @file LieValues.h + * @file Values.h * @author Richard Roberts * * @brief A templated config for Lie-group elements * * Detailed story: * A values structure is a map from keys to values. It is used to specify the value of a bunch - * of variables in a factor graph. A LieValues is a values structure which can hold variables that + * of variables in a factor graph. A Values is a values structure which can hold variables that * are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type * which is also a manifold element, and hence supports operations dim, retract, and localCoordinates. */ @@ -47,7 +47,7 @@ namespace gtsam { * labels (example: Pose2, Point2, etc) */ template - class LieValues { + class Values { public: @@ -72,18 +72,18 @@ namespace gtsam { public: - LieValues() {} - LieValues(const LieValues& config) : + Values() {} + Values(const Values& config) : values_(config.values_) {} template - LieValues(const LieValues& other) {} // do nothing when initializing with wrong type - virtual ~LieValues() {} + Values(const Values& other) {} // do nothing when initializing with wrong type + virtual ~Values() {} /** print */ void print(const std::string &s="") const; /** Test whether configs are identical in keys and values */ - bool equals(const LieValues& expected, double tol=1e-9) const; + bool equals(const Values& expected, double tol=1e-9) const; /** Retrieve a variable by j, throws std::invalid_argument if not found */ const Value& at(const J& j) const; @@ -120,13 +120,13 @@ namespace gtsam { size_t dim() const; /** Add a delta config to current config and returns a new config */ - LieValues retract(const VectorValues& delta, const Ordering& ordering) const; + Values retract(const VectorValues& delta, const Ordering& ordering) const; /** Get a delta config about a linearization point c0 (*this) */ - VectorValues localCoordinates(const LieValues& cp, const Ordering& ordering) const; + VectorValues localCoordinates(const Values& cp, const Ordering& ordering) const; /** Get a delta config about a linearization point c0 (*this) */ - void localCoordinates(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const; + void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const; // imperative methods: @@ -134,10 +134,10 @@ namespace gtsam { void insert(const J& j, const Value& val); /** Add a set of variables - does note replace existing values */ - void insert(const LieValues& cfg); + void insert(const Values& cfg); /** update the current available values without adding new ones */ - void update(const LieValues& cfg); + void update(const Values& cfg); /** single element change of existing element */ void update(const J& j, const Value& val); @@ -157,7 +157,7 @@ namespace gtsam { std::list keys() const; /** Replace all keys and variables */ - LieValues& operator=(const LieValues& rhs) { + Values& operator=(const Values& rhs) { values_ = rhs.values_; return (*this); } diff --git a/gtsam/nonlinear/tests/testLieValues.cpp b/gtsam/nonlinear/tests/testValues.cpp similarity index 86% rename from gtsam/nonlinear/tests/testLieValues.cpp rename to gtsam/nonlinear/tests/testValues.cpp index 9a0b39ffc..0cabc5f6d 100644 --- a/gtsam/nonlinear/tests/testLieValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testLieValues.cpp + * @file testValues.cpp * @author Richard Roberts */ @@ -22,32 +22,32 @@ using namespace boost::assign; #include #include -#include +#include using namespace gtsam; using namespace std; static double inf = std::numeric_limits::infinity(); typedef TypedSymbol VecKey; -typedef LieValues Values; +typedef Values TestValues; VecKey key1(1), key2(2), key3(3), key4(4); /* ************************************************************************* */ -TEST( LieValues, equals1 ) +TEST( TestValues, equals1 ) { - Values expected; + TestValues expected; Vector v = Vector_(3, 5.0, 6.0, 7.0); expected.insert(key1,v); - Values actual; + TestValues actual; actual.insert(key1,v); CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ -TEST( LieValues, equals2 ) +TEST( TestValues, equals2 ) { - Values cfg1, cfg2; + TestValues cfg1, cfg2; Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v2 = Vector_(3, 5.0, 6.0, 8.0); cfg1.insert(key1, v1); @@ -57,9 +57,9 @@ TEST( LieValues, equals2 ) } /* ************************************************************************* */ -TEST( LieValues, equals_nan ) +TEST( TestValues, equals_nan ) { - Values cfg1, cfg2; + TestValues cfg1, cfg2; Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v2 = Vector_(3, inf, inf, inf); cfg1.insert(key1, v1); @@ -69,9 +69,9 @@ TEST( LieValues, equals_nan ) } /* ************************************************************************* */ -TEST( LieValues, insert_config ) +TEST( TestValues, insert_config ) { - Values cfg1, cfg2, expected; + TestValues cfg1, cfg2, expected; Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v2 = Vector_(3, 8.0, 9.0, 1.0); Vector v3 = Vector_(3, 2.0, 4.0, 3.0); @@ -92,9 +92,9 @@ TEST( LieValues, insert_config ) } /* ************************************************************************* */ -TEST( LieValues, update_element ) +TEST( TestValues, update_element ) { - Values cfg; + TestValues cfg; Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v2 = Vector_(3, 8.0, 9.0, 1.0); @@ -108,9 +108,9 @@ TEST( LieValues, update_element ) } ///* ************************************************************************* */ -//TEST(LieValues, dim_zero) +//TEST(TestValues, dim_zero) //{ -// Values config0; +// TestValues config0; // config0.insert(key1, Vector_(2, 2.0, 3.0)); // config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); // LONGS_EQUAL(5,config0.dim()); @@ -122,9 +122,9 @@ TEST( LieValues, update_element ) //} /* ************************************************************************* */ -TEST(LieValues, expmap_a) +TEST(TestValues, expmap_a) { - Values config0; + TestValues config0; config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); @@ -133,7 +133,7 @@ TEST(LieValues, expmap_a) increment[ordering[key1]] = Vector_(3, 1.0, 1.1, 1.2); increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5); - Values expected; + TestValues expected; expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); @@ -141,9 +141,9 @@ TEST(LieValues, expmap_a) } ///* ************************************************************************* */ -//TEST(LieValues, expmap_b) +//TEST(TestValues, expmap_b) //{ -// Values config0; +// TestValues config0; // config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); // config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); // @@ -151,7 +151,7 @@ TEST(LieValues, expmap_a) // VectorValues increment(config0.dims(ordering)); // increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5); // -// Values expected; +// TestValues expected; // expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); // expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); // @@ -159,9 +159,9 @@ TEST(LieValues, expmap_a) //} ///* ************************************************************************* */ -//TEST(LieValues, expmap_c) +//TEST(TestValues, expmap_c) //{ -// Values config0; +// TestValues config0; // config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); // config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); // @@ -169,7 +169,7 @@ TEST(LieValues, expmap_a) // 1.0, 1.1, 1.2, // 1.3, 1.4, 1.5); // -// Values expected; +// TestValues expected; // expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); // expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); // @@ -177,16 +177,16 @@ TEST(LieValues, expmap_a) //} /* ************************************************************************* */ -/*TEST(LieValues, expmap_d) +/*TEST(TestValues, expmap_d) { - Values config0; + TestValues config0; config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); //config0.print("config0"); CHECK(equal(config0, config0)); CHECK(config0.equals(config0)); - LieValues poseconfig; + TestValues poseconfig; poseconfig.insert("p1", Pose2(1,2,3)); poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5)); //poseconfig.print("poseconfig"); @@ -195,10 +195,10 @@ TEST(LieValues, expmap_a) }*/ /* ************************************************************************* */ -/*TEST(LieValues, extract_keys) +/*TEST(TestValues, extract_keys) { typedef TypedSymbol PoseKey; - LieValues config; + TestValues config; config.insert(PoseKey(1), Pose2()); config.insert(PoseKey(2), Pose2()); @@ -217,9 +217,9 @@ TEST(LieValues, expmap_a) }*/ /* ************************************************************************* */ -TEST(LieValues, exists_) +TEST(TestValues, exists_) { - Values config0; + TestValues config0; config0.insert(key1, Vector_(1, 1.)); config0.insert(key2, Vector_(1, 2.)); @@ -228,40 +228,40 @@ TEST(LieValues, exists_) } /* ************************************************************************* */ -TEST(LieValues, update) +TEST(TestValues, update) { - Values config0; + TestValues config0; config0.insert(key1, Vector_(1, 1.)); config0.insert(key2, Vector_(1, 2.)); - Values superset; + TestValues superset; superset.insert(key1, Vector_(1, -1.)); superset.insert(key2, Vector_(1, -2.)); superset.insert(key3, Vector_(1, -3.)); config0.update(superset); - Values expected; + TestValues expected; expected.insert(key1, Vector_(1, -1.)); expected.insert(key2, Vector_(1, -2.)); CHECK(assert_equal(expected,config0)); } /* ************************************************************************* */ -TEST(LieValues, dummy_initialization) +TEST(TestValues, dummy_initialization) { typedef TypedSymbol Key; - typedef LieValues Values1; + typedef Values Values1; Values1 init1; init1.insert(Key(1), Vector_(2, 1.0, 2.0)); init1.insert(Key(2), Vector_(2, 4.0, 3.0)); - Values init2; + TestValues init2; init2.insert(key1, Vector_(2, 1.0, 2.0)); init2.insert(key2, Vector_(2, 4.0, 3.0)); Values1 actual1(init2); - Values actual2(init1); + TestValues actual2(init1); EXPECT(actual1.empty()); EXPECT(actual2.empty()); diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h index aa59d436d..9735e61d7 100644 --- a/gtsam/slam/PartialPriorFactor.h +++ b/gtsam/slam/PartialPriorFactor.h @@ -33,7 +33,7 @@ namespace gtsam { * * It takes two template parameters: * Key (typically TypedSymbol) is used to look up T's in a Values - * Values where the T's are stored, typically LieValues or a TupleValues<...> + * Values where the T's are stored, typically Values or a TupleValues<...> * * For practical use, it would be good to subclass this factor and have the class type * construct the mask. diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 90650460e..2d7b631cc 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -24,7 +24,7 @@ namespace gtsam { * A class for a soft prior on any Lie type * It takes two template parameters: * Key (typically TypedSymbol) is used to look up T's in a Values - * Values where the T's are stored, typically LieValues or a TupleValues<...> + * Values where the T's are stored, typically Values or a TupleValues<...> * The Key type is not arbitrary: we need to cast to a Symbol at linearize, so * a simple type like int will not work */ diff --git a/gtsam/slam/Simulated3D.cpp b/gtsam/slam/Simulated3D.cpp index 4d1583f65..3c73b5ba9 100644 --- a/gtsam/slam/Simulated3D.cpp +++ b/gtsam/slam/Simulated3D.cpp @@ -16,14 +16,15 @@ **/ #include -#include +#include #include namespace gtsam { +INSTANTIATE_VALUES(simulated3D::PointKey) +INSTANTIATE_VALUES(simulated3D::PoseKey) + using namespace simulated3D; -INSTANTIATE_LIE_VALUES(PointKey) -INSTANTIATE_LIE_VALUES(PoseKey) INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues) namespace simulated3D { diff --git a/gtsam/slam/Simulated3D.h b/gtsam/slam/Simulated3D.h index d8eac27b0..32f05e967 100644 --- a/gtsam/slam/Simulated3D.h +++ b/gtsam/slam/Simulated3D.h @@ -40,8 +40,8 @@ namespace simulated3D { typedef gtsam::TypedSymbol PoseKey; typedef gtsam::TypedSymbol PointKey; -typedef LieValues PoseValues; -typedef LieValues PointValues; +typedef Values PoseValues; +typedef Values PointValues; typedef TupleValues2 Values; /** diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp index e189928f4..955d190c2 100644 --- a/gtsam/slam/planarSLAM.cpp +++ b/gtsam/slam/planarSLAM.cpp @@ -24,11 +24,10 @@ // Use planarSLAM namespace for specific SLAM instance namespace gtsam { - using namespace planarSLAM; - INSTANTIATE_LIE_VALUES(PointKey) - INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues) - INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values) - INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values) + INSTANTIATE_VALUES(planarSLAM::PointKey) + INSTANTIATE_TUPLE_VALUES2(planarSLAM::PoseValues, planarSLAM::PointValues) + INSTANTIATE_NONLINEAR_FACTOR_GRAPH(planarSLAM::Values) + INSTANTIATE_NONLINEAR_OPTIMIZER(planarSLAM::Graph, planarSLAM::Values) namespace planarSLAM { diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 8b260b1fa..228ced08c 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -41,11 +41,11 @@ namespace gtsam { /// Typedef for a PointKey with Point2 data and 'l' symbol typedef TypedSymbol PointKey; - /// Typedef for LieValues structure with PoseKey type - typedef LieValues PoseValues; + /// Typedef for Values structure with PoseKey type + typedef Values PoseValues; - /// Typedef for LieValues structure with PointKey type - typedef LieValues PointValues; + /// Typedef for Values structure with PointKey type + typedef Values PointValues; /// Values class, inherited from TupleValues2, using PoseKeys and PointKeys struct Values: public TupleValues2 { diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index 88aaddd95..9219c242c 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -16,18 +16,17 @@ **/ #include -#include +#include #include #include // Use pose2SLAM namespace for specific SLAM instance namespace gtsam { - using namespace pose2SLAM; - INSTANTIATE_LIE_VALUES(Key) - INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values) - INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values) - template class NonlinearOptimizer; + INSTANTIATE_VALUES(pose2SLAM::Key) + INSTANTIATE_NONLINEAR_FACTOR_GRAPH(pose2SLAM::Values) + INSTANTIATE_NONLINEAR_OPTIMIZER(pose2SLAM::Graph, pose2SLAM::Values) + template class NonlinearOptimizer; namespace pose2SLAM { diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 4adbac766..45017a81b 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include #include @@ -37,8 +37,8 @@ namespace gtsam { /// Keys with Pose2 and symbol 'x' typedef TypedSymbol Key; - /// LieValues with type 'Key' - typedef LieValues Values; + /// Values with type 'Key' + typedef Values Values; /** * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0) diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp index 099c88f9d..7c7bb0d2e 100644 --- a/gtsam/slam/pose3SLAM.cpp +++ b/gtsam/slam/pose3SLAM.cpp @@ -16,17 +16,16 @@ **/ #include -#include +#include #include #include // Use pose3SLAM namespace for specific SLAM instance namespace gtsam { - using namespace pose3SLAM; - INSTANTIATE_LIE_VALUES(Key) - INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values) - INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values) + INSTANTIATE_VALUES(pose3SLAM::Key) + INSTANTIATE_NONLINEAR_FACTOR_GRAPH(pose3SLAM::Values) + INSTANTIATE_NONLINEAR_OPTIMIZER(pose3SLAM::Graph, pose3SLAM::Values) namespace pose3SLAM { diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h index 6413d4b3c..f4516e647 100644 --- a/gtsam/slam/pose3SLAM.h +++ b/gtsam/slam/pose3SLAM.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include #include #include #include @@ -33,8 +33,8 @@ namespace gtsam { /// Creates a Key with data Pose3 and symbol 'x' typedef TypedSymbol Key; - /// Creates a LieValues structure with type 'Key' - typedef LieValues Values; + /// Creates a Values structure with type 'Key' + typedef Values Values; /** * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) diff --git a/gtsam/slam/simulated2D.cpp b/gtsam/slam/simulated2D.cpp index 0cb44506f..17a11dc06 100644 --- a/gtsam/slam/simulated2D.cpp +++ b/gtsam/slam/simulated2D.cpp @@ -16,14 +16,15 @@ */ #include -#include +#include #include namespace gtsam { + INSTANTIATE_VALUES(simulated2D::PoseKey) + using namespace simulated2D; - INSTANTIATE_LIE_VALUES(PoseKey) INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues) namespace simulated2D { diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index 1f5b96150..3b63c9595 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -31,8 +31,8 @@ namespace gtsam { // Simulated2D robots have no orientation, just a position typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; - typedef LieValues PoseValues; - typedef LieValues PointValues; + typedef Values PoseValues; + typedef Values PointValues; /** * Custom Values class that holds poses and points diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index b85b479c3..3710f29ad 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -31,8 +31,8 @@ namespace gtsam { // The types that take an oriented pose2 rather than point2 typedef TypedSymbol PointKey; typedef TypedSymbol PoseKey; - typedef LieValues PoseValues; - typedef LieValues PointValues; + typedef Values PoseValues; + typedef Values PointValues; typedef TupleValues2 Values; //TODO:: point prior is not implemented right now diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 85cf0159e..053f7a971 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -33,14 +33,14 @@ typedef Cal3_S2Camera GeneralCamera; //typedef Cal3BundlerCamera GeneralCamera; typedef TypedSymbol CameraKey; typedef TypedSymbol PointKey; -typedef LieValues CameraConfig; -typedef LieValues PointConfig; -typedef TupleValues2 Values; -typedef GeneralSFMFactor Projection; -typedef NonlinearEquality CameraConstraint; -typedef NonlinearEquality Point3Constraint; +typedef Values CameraConfig; +typedef Values PointConfig; +typedef TupleValues2 VisualValues; +typedef GeneralSFMFactor Projection; +typedef NonlinearEquality CameraConstraint; +typedef NonlinearEquality Point3Constraint; -class Graph: public NonlinearFactorGraph { +class Graph: public NonlinearFactorGraph { public: void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) { push_back(boost::make_shared(z, model, i, j)); @@ -73,7 +73,7 @@ double getGaussian() return sqrt(-2.0f * (double)log(S) / S) * V1; } -typedef NonlinearOptimizer Optimizer; +typedef NonlinearOptimizer Optimizer; const SharedNoiseModel sigma1(noiseModel::Unit::Create(1)); @@ -90,7 +90,7 @@ TEST( GeneralSFMFactor, equals ) boost::shared_ptr factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); - CHECK(assert_equal(*factor1, *factor2)); + EXPECT(assert_equal(*factor1, *factor2)); } /* ************************************************************************* */ @@ -101,18 +101,18 @@ TEST( GeneralSFMFactor, error ) { boost::shared_ptr factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); // For the following configuration, the factor predicts 320,240 - Values values; + VisualValues values; Rot3 R; Point3 t1(0,0,-6); Pose3 x1(R,t1); values.insert(1, GeneralCamera(x1)); Point3 l1; values.insert(1, l1); - CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); + EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); } - static const double baseline = 5.0 ; +/* ************************************************************************* */ vector genPoint3() { const double z = 5; vector L ; @@ -174,7 +174,7 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new Values); + boost::shared_ptr values(new VisualValues); for ( size_t i = 0 ; i < X.size() ; ++i ) values->insert((int)i, X[i]) ; @@ -189,20 +189,13 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // Create an ordering of the variables shared_ptr ordering = getOrdering(X,L); - //graph->print("graph") ; values->print("values") ; NonlinearOptimizationParameters::sharedThis params ( new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); - //cout << "optimize_defaultK::" << endl ; - //cout << "before optimization, error is " << optimizer.error() << endl; Optimizer optimizer2 = optimizer.levenbergMarquardt(); - //cout << "after optimization, error is " << optimizer2.error() << endl; - //optimizer2.values()->print("optimized") ; - CHECK(optimizer2.error() < 0.5 * 1e-5 * nMeasurements); + EXPECT(optimizer2.error() < 0.5 * 1e-5 * nMeasurements); } - - /* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { vector L = genPoint3(); @@ -220,7 +213,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new Values); + boost::shared_ptr values(new VisualValues); for ( size_t i = 0 ; i < X.size() ; ++i ) values->insert((int)i, X[i]) ; @@ -244,13 +237,11 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { NonlinearOptimizationParameters::sharedThis params ( new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); - //cout << "optimize_varK_SingleMeasurementError::" << endl ; - //cout << "before optimization, error is " << optimizer.error() << endl; Optimizer optimizer2 = optimizer.levenbergMarquardt(); - //cout << "after optimization, error is " << optimizer2.error() << endl; - CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements); + EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements); } +/* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { vector L = genPoint3(); @@ -268,7 +259,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { const size_t nMeasurements = L.size()*X.size(); - boost::shared_ptr values(new Values); + boost::shared_ptr values(new VisualValues); for ( size_t i = 0 ; i < X.size() ; ++i ) values->insert((int)i, X[i]) ; @@ -290,14 +281,11 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); - //cout << "optimize_varK_FixCameras::" << endl ; - //cout << "before optimization, error is " << optimizer.error() << endl; Optimizer optimizer2 = optimizer.levenbergMarquardt(); - //cout << "after optimization, error is " << optimizer2.error() << endl; - CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements); + EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements); } - +/* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { vector L = genPoint3(); @@ -314,7 +302,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { const size_t nMeasurements = L.size()*X.size(); - boost::shared_ptr values(new Values); + boost::shared_ptr values(new VisualValues); for ( size_t i = 0 ; i < X.size() ; ++i ) { const double rot_noise = 1e-5, @@ -351,14 +339,10 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { shared_ptr ordering = getOrdering(X,L); NonlinearOptimizationParameters::sharedThis params ( new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); -// new NonlinearOptimizationParameters(1e-7, 1e-7, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::TRYDELTA)); Optimizer optimizer(graph, values, ordering, params); - //cout << "optimize_varK_FixLandmarks::" << endl ; - //cout << "before optimization, error is " << optimizer.error() << endl; Optimizer optimizer2 = optimizer.levenbergMarquardt(); - //cout << "after optimization, error is " << optimizer2.error() << endl; - CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements); + EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ @@ -379,7 +363,7 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new Values); + boost::shared_ptr values(new VisualValues); for ( size_t i = 0 ; i < X.size() ; ++i ) values->insert((int)i, X[i]) ; @@ -399,14 +383,10 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { new NonlinearOptimizationParameters(1e-2, 1e-2, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); - //cout << "optimize_varK_BA::" << endl ; - //cout << "before optimization, error is " << optimizer.error() << endl; Optimizer optimizer2 = optimizer.levenbergMarquardt(); - //cout << "after optimization, error is " << optimizer2.error() << endl; - CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements); + EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements); } - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index c0d80671d..36b80bd8e 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -32,14 +32,15 @@ using namespace gtsam; typedef Cal3BundlerCamera GeneralCamera; typedef TypedSymbol CameraKey; typedef TypedSymbol PointKey; -typedef LieValues CameraConfig; -typedef LieValues PointConfig; -typedef TupleValues2 Values; -typedef GeneralSFMFactor Projection; -typedef NonlinearEquality CameraConstraint; -typedef NonlinearEquality Point3Constraint; +typedef Values CameraConfig; +typedef Values PointConfig; +typedef TupleValues2 VisualValues; +typedef GeneralSFMFactor Projection; +typedef NonlinearEquality CameraConstraint; +typedef NonlinearEquality Point3Constraint; -class Graph: public NonlinearFactorGraph { +/* ************************************************************************* */ +class Graph: public NonlinearFactorGraph { public: void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) { push_back(boost::make_shared(z, model, i, j)); @@ -72,7 +73,7 @@ double getGaussian() return sqrt(-2.0f * (double)log(S) / S) * V1; } -typedef NonlinearOptimizer Optimizer; +typedef NonlinearOptimizer Optimizer; const SharedNoiseModel sigma1(noiseModel::Unit::Create(1)); @@ -89,7 +90,7 @@ TEST( GeneralSFMFactor, equals ) boost::shared_ptr factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); - CHECK(assert_equal(*factor1, *factor2)); + EXPECT(assert_equal(*factor1, *factor2)); } /* ************************************************************************* */ @@ -100,18 +101,19 @@ TEST( GeneralSFMFactor, error ) { boost::shared_ptr factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); // For the following configuration, the factor predicts 320,240 - Values values; + VisualValues values; Rot3 R; Point3 t1(0,0,-6); Pose3 x1(R,t1); values.insert(1, GeneralCamera(x1)); Point3 l1; values.insert(1, l1); - CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); + EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); } static const double baseline = 5.0 ; +/* ************************************************************************* */ vector genPoint3() { const double z = 5; vector L ; @@ -172,7 +174,7 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new Values); + boost::shared_ptr values(new VisualValues); for ( size_t i = 0 ; i < X.size() ; ++i ) values->insert((int)i, X[i]) ; @@ -187,20 +189,13 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // Create an ordering of the variables shared_ptr ordering = getOrdering(X,L); - //graph->print("graph") ; values->print("values") ; NonlinearOptimizationParameters::sharedThis params ( new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); - //cout << "optimize_defaultK::" << endl ; - //cout << "before optimization, error is " << optimizer.error() << endl; Optimizer optimizer2 = optimizer.levenbergMarquardt(); - //cout << "after optimization, error is " << optimizer2.error() << endl; - //optimizer2.values()->print("optimized") ; - CHECK(optimizer2.error() < 0.5 * 1e-5 * nMeasurements); + EXPECT(optimizer2.error() < 0.5 * 1e-5 * nMeasurements); } - - /* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { vector L = genPoint3(); @@ -218,7 +213,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new Values); + boost::shared_ptr values(new VisualValues); for ( size_t i = 0 ; i < X.size() ; ++i ) values->insert((int)i, X[i]) ; @@ -242,13 +237,11 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { NonlinearOptimizationParameters::sharedThis params ( new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); - //cout << "optimize_varK_SingleMeasurementError::" << endl ; - //cout << "before optimization, error is " << optimizer.error() << endl; Optimizer optimizer2 = optimizer.levenbergMarquardt(); - //cout << "after optimization, error is " << optimizer2.error() << endl; - CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements); + EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements); } +/* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { vector L = genPoint3(); @@ -266,7 +259,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { const size_t nMeasurements = L.size()*X.size(); - boost::shared_ptr values(new Values); + boost::shared_ptr values(new VisualValues); for ( size_t i = 0 ; i < X.size() ; ++i ) values->insert((int)i, X[i]) ; @@ -288,14 +281,11 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); - //cout << "optimize_varK_FixCameras::" << endl ; - //cout << "before optimization, error is " << optimizer.error() << endl; Optimizer optimizer2 = optimizer.levenbergMarquardt(); - //cout << "after optimization, error is " << optimizer2.error() << endl; - CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements); + EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements); } - +/* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { vector L = genPoint3(); @@ -312,7 +302,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { const size_t nMeasurements = L.size()*X.size(); - boost::shared_ptr values(new Values); + boost::shared_ptr values(new VisualValues); for ( size_t i = 0 ; i < X.size() ; ++i ) { const double rot_noise = 1e-5, trans_noise = 1e-3, @@ -345,14 +335,10 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { shared_ptr ordering = getOrdering(X,L); NonlinearOptimizationParameters::sharedThis params ( new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); -// new NonlinearOptimizationParameters(1e-7, 1e-7, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::TRYDELTA)); Optimizer optimizer(graph, values, ordering, params); - //cout << "optimize_varK_FixLandmarks::" << endl ; - //cout << "before optimization, error is " << optimizer.error() << endl; Optimizer optimizer2 = optimizer.levenbergMarquardt(); - //cout << "after optimization, error is " << optimizer2.error() << endl; - CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements); + EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ @@ -373,7 +359,7 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new Values); + boost::shared_ptr values(new VisualValues); for ( size_t i = 0 ; i < X.size() ; ++i ) values->insert((int)i, X[i]) ; @@ -393,14 +379,10 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); Optimizer optimizer(graph, values, ordering, params); - //cout << "optimize_varK_BA::" << endl ; - //cout << "before optimization, error is " << optimizer.error() << endl; Optimizer optimizer2 = optimizer.levenbergMarquardt(); - //cout << "after optimization, error is " << optimizer2.error() << endl; - CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements); + EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements); } - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index ad9550304..54f3efa38 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -42,8 +42,6 @@ static Matrix covariance = eye(6); const double tol=1e-5; -using namespace pose3SLAM; - /* ************************************************************************* */ // test optimization with 6 poses arranged in a hexagon and a loop closure TEST(Pose3Graph, optimizeCircle) { @@ -79,8 +77,8 @@ TEST(Pose3Graph, optimizeCircle) { shared_ptr ordering(new Ordering); *ordering += "x0","x1","x2","x3","x4","x5"; NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); - Optimizer optimizer0(fg, initial, ordering, params); - Optimizer optimizer = optimizer0.levenbergMarquardt(); + pose3SLAM::Optimizer optimizer0(fg, initial, ordering, params); + pose3SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(); Pose3Values actual = *optimizer.values(); @@ -93,12 +91,12 @@ TEST(Pose3Graph, optimizeCircle) { /* ************************************************************************* */ TEST(Pose3Graph, partial_prior_height) { - typedef PartialPriorFactor Partial; + typedef PartialPriorFactor Partial; // reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3) // height prior - single element interface - Key key(1); + pose3SLAM::Key key(1); double exp_height = 5.0; SharedDiagonal model = noiseModel::Unit::Create(1); Pose3 init(Rot3(), Point3(1.0, 2.0, 3.0)), expected(Rot3(), Point3(1.0, 2.0, exp_height)); @@ -108,17 +106,17 @@ TEST(Pose3Graph, partial_prior_height) { Matrix expA = Matrix_(1, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); EXPECT(assert_equal(expA, actA)); - Graph graph; + pose3SLAM::Graph graph; // graph.add(height); // FAIL - on compile, can't initialize a reference? graph.push_back(boost::shared_ptr(new Partial(height))); - Values values; + pose3SLAM::Values values; values.insert(key, init); // linearization EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol); - Values actual = *Optimizer::optimizeLM(graph, values); + pose3SLAM::Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values); EXPECT(assert_equal(expected, actual[key], tol)); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } @@ -148,10 +146,10 @@ TEST( Pose3Factor, error ) /* ************************************************************************* */ TEST(Pose3Graph, partial_prior_xy) { - typedef PartialPriorFactor Partial; + typedef PartialPriorFactor Partial; // XY prior - full mask interface - Key key(1); + pose3SLAM::Key key(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)); @@ -164,13 +162,13 @@ TEST(Pose3Graph, partial_prior_xy) { 0.0, 0.0, 0.0, 0.0, 1.0, 0.0); EXPECT(assert_equal(expA, actA)); - Graph graph; + pose3SLAM::Graph graph; graph.push_back(Partial::shared_ptr(new Partial(priorXY))); - Values values; + pose3SLAM::Values values; values.insert(key, init); - Values actual = *Optimizer::optimizeLM(graph, values); + pose3SLAM::Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values); EXPECT(assert_equal(expected, actual[key], tol)); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index 5ae357ef6..09fd612d7 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -25,7 +25,6 @@ using namespace std; using namespace gtsam; -using namespace gtsam::visualSLAM; // make cube static Point3 @@ -48,11 +47,11 @@ TEST( ProjectionFactor, error ) // Create the factor with a measurement that is 3 pixels off in x Point2 z(323.,240.); int cameraFrameNumber=1, landmarkNumber=1; - boost::shared_ptr - factor(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); + boost::shared_ptr + factor(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); // For the following values structure, the factor predicts 320,240 - Values config; + visualSLAM::Values config; Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1); Point3 l1; config.insert(1, l1); // Point should project to Point2(320.,240.) @@ -73,7 +72,7 @@ TEST( ProjectionFactor, error ) CHECK(assert_equal(expected,*actual,1e-3)); // linearize graph - Graph graph; + visualSLAM::Graph graph; graph.push_back(factor); FactorGraph expected_lfg; expected_lfg.push_back(actual); @@ -81,13 +80,13 @@ TEST( ProjectionFactor, error ) CHECK(assert_equal(expected_lfg,*actual_lfg)); // expmap on a config - Values expected_config; + visualSLAM::Values expected_config; Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(1, x2); Point3 l2(1,2,3); expected_config.insert(1, l2); VectorValues delta(expected_config.dims(ordering)); delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.); delta[ordering["l1"]] = Vector_(3, 1.,2.,3.); - Values actual_config = config.retract(delta, ordering); + visualSLAM::Values actual_config = config.retract(delta, ordering); CHECK(assert_equal(expected_config,actual_config,1e-9)); } @@ -97,11 +96,11 @@ TEST( ProjectionFactor, equals ) // Create two identical factors and make sure they're equal Vector z = Vector_(2,323.,240.); int cameraFrameNumber=1, landmarkNumber=1; - boost::shared_ptr - factor1(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); + boost::shared_ptr + factor1(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); - boost::shared_ptr - factor2(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); + boost::shared_ptr + factor2(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); CHECK(assert_equal(*factor1, *factor2)); } diff --git a/gtsam/slam/tests/testSimulated2D.cpp b/gtsam/slam/tests/testSimulated2D.cpp index 8c0f02d8f..c9b2bfd61 100644 --- a/gtsam/slam/tests/testSimulated2D.cpp +++ b/gtsam/slam/tests/testSimulated2D.cpp @@ -23,27 +23,26 @@ #include #include -using namespace gtsam; using namespace std; -using namespace simulated2D; +using namespace gtsam; /* ************************************************************************* */ TEST( simulated2D, Simulated2DValues ) { - Values actual; + simulated2D::Values actual; actual.insertPose(1,Point2(1,1)); actual.insertPoint(2,Point2(2,2)); - CHECK(assert_equal(actual,actual,1e-9)); + EXPECT(assert_equal(actual,actual,1e-9)); } /* ************************************************************************* */ TEST( simulated2D, Dprior ) { Point2 x(1,-9); - Matrix numerical = numericalDerivative11(prior,x); + Matrix numerical = numericalDerivative11(simulated2D::prior,x); Matrix computed; - prior(x,computed); - CHECK(assert_equal(numerical,computed,1e-9)); + simulated2D::prior(x,computed); + EXPECT(assert_equal(numerical,computed,1e-9)); } /* ************************************************************************* */ @@ -51,11 +50,11 @@ TEST( simulated2D, Dprior ) { Point2 x1(1,-9),x2(-5,6); Matrix H1,H2; - odo(x1,x2,H1,H2); - Matrix A1 = numericalDerivative21(odo,x1,x2); - CHECK(assert_equal(A1,H1,1e-9)); - Matrix A2 = numericalDerivative22(odo,x1,x2); - CHECK(assert_equal(A2,H2,1e-9)); + simulated2D::odo(x1,x2,H1,H2); + Matrix A1 = numericalDerivative21(simulated2D::odo,x1,x2); + EXPECT(assert_equal(A1,H1,1e-9)); + Matrix A2 = numericalDerivative22(simulated2D::odo,x1,x2); + EXPECT(assert_equal(A2,H2,1e-9)); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testSimulated2DOriented.cpp b/gtsam/slam/tests/testSimulated2DOriented.cpp index 4cc517b34..57951f6e1 100644 --- a/gtsam/slam/tests/testSimulated2DOriented.cpp +++ b/gtsam/slam/tests/testSimulated2DOriented.cpp @@ -24,26 +24,16 @@ #include #include -using namespace gtsam; using namespace std; -using namespace simulated2DOriented; - -/* ************************************************************************* */ -TEST( simulated2DOriented, Simulated2DValues ) -{ - simulated2D::Values actual; - actual.insertPose(1,Point2(1,1)); - actual.insertPoint(2,Point2(2,2)); - CHECK(assert_equal(actual,actual,1e-9)); -} +using namespace gtsam; /* ************************************************************************* */ TEST( simulated2DOriented, Dprior ) { Pose2 x(1,-9, 0.1); - Matrix numerical = numericalDerivative11(prior,x); + Matrix numerical = numericalDerivative11(simulated2DOriented::prior,x); Matrix computed; - prior(x,computed); + simulated2DOriented::prior(x,computed); CHECK(assert_equal(numerical,computed,1e-9)); } @@ -52,10 +42,10 @@ TEST( simulated2DOriented, Dprior ) { Pose2 x1(1,-9,0.1),x2(-5,6,0.2); Matrix H1,H2; - odo(x1,x2,H1,H2); - Matrix A1 = numericalDerivative21(odo,x1,x2); + simulated2DOriented::odo(x1,x2,H1,H2); + Matrix A1 = numericalDerivative21(simulated2DOriented::odo,x1,x2); CHECK(assert_equal(A1,H1,1e-9)); - Matrix A2 = numericalDerivative22(odo,x1,x2); + Matrix A2 = numericalDerivative22(simulated2DOriented::odo,x1,x2); CHECK(assert_equal(A2,H2,1e-9)); } @@ -64,11 +54,11 @@ TEST( simulated2DOriented, constructor ) { Pose2 measurement(0.2, 0.3, 0.1); SharedDiagonal model(Vector_(3, 1., 1., 1.)); - Odometry factor(measurement, model, PoseKey(1), PoseKey(2)); + simulated2DOriented::Odometry factor(measurement, model, simulated2DOriented::PoseKey(1), simulated2DOriented::PoseKey(2)); - Values config; - config.insert(PoseKey(1), Pose2(1., 0., 0.2)); - config.insert(PoseKey(2), Pose2(2., 0., 0.1)); + simulated2DOriented::Values config; + config.insert(simulated2DOriented::PoseKey(1), Pose2(1., 0., 0.2)); + config.insert(simulated2DOriented::PoseKey(2), Pose2(2., 0., 0.1)); boost::shared_ptr lf = factor.linearize(config, *config.orderingArbitrary()); } diff --git a/gtsam/slam/tests/testSimulated3D.cpp b/gtsam/slam/tests/testSimulated3D.cpp index f94cafb54..de2c534bb 100644 --- a/gtsam/slam/tests/testSimulated3D.cpp +++ b/gtsam/slam/tests/testSimulated3D.cpp @@ -24,14 +24,13 @@ #include using namespace gtsam; -using namespace simulated3D; /* ************************************************************************* */ TEST( simulated3D, Values ) { - Values actual; - actual.insert(PointKey(1),Point3(1,1,1)); - actual.insert(PoseKey(2),Point3(2,2,2)); + simulated3D::Values actual; + actual.insert(simulated3D::PointKey(1),Point3(1,1,1)); + actual.insert(simulated3D::PoseKey(2),Point3(2,2,2)); EXPECT(assert_equal(actual,actual,1e-9)); } @@ -39,9 +38,9 @@ TEST( simulated3D, Values ) TEST( simulated3D, Dprior ) { Point3 x(1,-9, 7); - Matrix numerical = numericalDerivative11(boost::bind(prior, _1, boost::none),x); + Matrix numerical = numericalDerivative11(boost::bind(simulated3D::prior, _1, boost::none),x); Matrix computed; - prior(x,computed); + simulated3D::prior(x,computed); EXPECT(assert_equal(numerical,computed,1e-9)); } @@ -50,10 +49,10 @@ TEST( simulated3D, DOdo ) { Point3 x1(1,-9,7),x2(-5,6,7); Matrix H1,H2; - odo(x1,x2,H1,H2); - Matrix A1 = numericalDerivative21(boost::bind(odo, _1, _2, boost::none, boost::none),x1,x2); + simulated3D::odo(x1,x2,H1,H2); + Matrix A1 = numericalDerivative21(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2); EXPECT(assert_equal(A1,H1,1e-9)); - Matrix A2 = numericalDerivative22(boost::bind(odo, _1, _2, boost::none, boost::none),x1,x2); + Matrix A2 = numericalDerivative22(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2); EXPECT(assert_equal(A2,H2,1e-9)); } diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index d3e5d4f70..3cf84538b 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -29,9 +29,7 @@ using namespace std; using namespace gtsam; -using namespace gtsam::visualSLAM; using namespace boost; -typedef NonlinearOptimizer Optimizer; Pose3 camera1(Matrix_(3,3, 1., 0., 0., @@ -52,16 +50,16 @@ TEST( StereoFactor, singlePoint) { //Cal3_S2 K(625, 625, 0, 320, 240, 0.5); boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); - boost::shared_ptr graph(new Graph()); + boost::shared_ptr graph(new visualSLAM::Graph()); - graph->add(PoseConstraint(1,camera1)); + graph->add(visualSLAM::PoseConstraint(1,camera1)); StereoPoint2 z14(320,320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) - graph->add(StereoFactor(z14,sigma, 1, 1, K)); + graph->add(visualSLAM::StereoFactor(z14,sigma, 1, 1, K)); // Create a configuration corresponding to the ground truth - boost::shared_ptr values(new Values()); + boost::shared_ptr values(new visualSLAM::Values()); values->insert(1, camera1); // add camera at z=6.25m looking towards origin Point3 l1(0, 0, 0); @@ -69,7 +67,7 @@ TEST( StereoFactor, singlePoint) Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); - typedef gtsam::NonlinearOptimizer Optimizer; // optimization engine for this domain + typedef gtsam::NonlinearOptimizer Optimizer; // optimization engine for this domain double absoluteThreshold = 1e-9; double relativeThreshold = 1e-5; diff --git a/gtsam/slam/tests/testVSLAM.cpp b/gtsam/slam/tests/testVSLAM.cpp index 1baa1642b..1b225f3d7 100644 --- a/gtsam/slam/tests/testVSLAM.cpp +++ b/gtsam/slam/tests/testVSLAM.cpp @@ -32,7 +32,6 @@ using namespace boost; using namespace std; using namespace gtsam; -using namespace gtsam::visualSLAM; using namespace boost; static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); @@ -58,7 +57,7 @@ Pose3 camera2(Matrix_(3,3, Point3(0,0,5.00)); /* ************************************************************************* */ -Graph testGraph() { +visualSLAM::Graph testGraph() { Point2 z11(-100, 100); Point2 z12(-100,-100); Point2 z13( 100,-100); @@ -69,7 +68,7 @@ Graph testGraph() { Point2 z24( 125, 125); shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0)); - Graph g; + visualSLAM::Graph g; g.addMeasurement(z11, sigma, 1, 1, sK); g.addMeasurement(z12, sigma, 1, 2, sK); g.addMeasurement(z13, sigma, 1, 3, sK); @@ -85,14 +84,14 @@ Graph testGraph() { TEST( Graph, optimizeLM) { // build a graph - shared_ptr graph(new Graph(testGraph())); + shared_ptr graph(new visualSLAM::Graph(testGraph())); // add 3 landmark constraints graph->addPointConstraint(1, landmark1); graph->addPointConstraint(2, landmark2); graph->addPointConstraint(3, landmark3); // Create an initial values structure corresponding to the ground truth - boost::shared_ptr initialEstimate(new Values); + boost::shared_ptr initialEstimate(new visualSLAM::Values); initialEstimate->insert(1, camera1); initialEstimate->insert(2, camera2); initialEstimate->insert(1, landmark1); @@ -106,12 +105,12 @@ TEST( Graph, optimizeLM) // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth - Optimizer optimizer(graph, initialEstimate, ordering); + visualSLAM::Optimizer optimizer(graph, initialEstimate, ordering); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // Iterate once, and the config should not have changed because we started // with the ground truth - Optimizer afterOneIteration = optimizer.iterate(); + visualSLAM::Optimizer afterOneIteration = optimizer.iterate(); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // check if correct @@ -123,13 +122,13 @@ TEST( Graph, optimizeLM) TEST( Graph, optimizeLM2) { // build a graph - shared_ptr graph(new Graph(testGraph())); + shared_ptr graph(new visualSLAM::Graph(testGraph())); // add 2 camera constraints graph->addPoseConstraint(1, camera1); graph->addPoseConstraint(2, camera2); // Create an initial values structure corresponding to the ground truth - boost::shared_ptr initialEstimate(new Values); + boost::shared_ptr initialEstimate(new visualSLAM::Values); initialEstimate->insert(1, camera1); initialEstimate->insert(2, camera2); initialEstimate->insert(1, landmark1); @@ -143,12 +142,12 @@ TEST( Graph, optimizeLM2) // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth - Optimizer optimizer(graph, initialEstimate, ordering); + visualSLAM::Optimizer optimizer(graph, initialEstimate, ordering); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // Iterate once, and the config should not have changed because we started // with the ground truth - Optimizer afterOneIteration = optimizer.iterate(); + visualSLAM::Optimizer afterOneIteration = optimizer.iterate(); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // check if correct @@ -160,13 +159,13 @@ TEST( Graph, optimizeLM2) TEST( Graph, CHECK_ORDERING) { // build a graph - shared_ptr graph(new Graph(testGraph())); + shared_ptr graph(new visualSLAM::Graph(testGraph())); // add 2 camera constraints graph->addPoseConstraint(1, camera1); graph->addPoseConstraint(2, camera2); // Create an initial values structure corresponding to the ground truth - boost::shared_ptr initialEstimate(new Values); + boost::shared_ptr initialEstimate(new visualSLAM::Values); initialEstimate->insert(1, camera1); initialEstimate->insert(2, camera2); initialEstimate->insert(1, landmark1); @@ -178,12 +177,12 @@ TEST( Graph, CHECK_ORDERING) // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth - Optimizer optimizer(graph, initialEstimate, ordering); + visualSLAM::Optimizer optimizer(graph, initialEstimate, ordering); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // Iterate once, and the config should not have changed because we started // with the ground truth - Optimizer afterOneIteration = optimizer.iterate(); + visualSLAM::Optimizer afterOneIteration = optimizer.iterate(); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // check if correct @@ -194,23 +193,23 @@ TEST( Graph, CHECK_ORDERING) TEST( Values, update_with_large_delta) { // this test ensures that if the update for delta is larger than // the size of the config, it only updates existing variables - Values init; + visualSLAM::Values init; init.insert(1, Pose3()); init.insert(1, Point3(1.0, 2.0, 3.0)); - Values expected; + visualSLAM::Values expected; expected.insert(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); expected.insert(1, Point3(1.1, 2.1, 3.1)); Ordering largeOrdering; - Values largeValues = init; + visualSLAM::Values largeValues = init; largeValues.insert(2, Pose3()); largeOrdering += "x1","l1","x2"; VectorValues delta(largeValues.dims(largeOrdering)); delta[largeOrdering["x1"]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1); delta[largeOrdering["l1"]] = Vector_(3, 0.1, 0.1, 0.1); delta[largeOrdering["x2"]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1); - Values actual = init.retract(delta, largeOrdering); + visualSLAM::Values actual = init.retract(delta, largeOrdering); CHECK(assert_equal(expected,actual)); } diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index cf96166ae..16e424344 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -39,8 +39,8 @@ namespace gtsam { */ typedef TypedSymbol PoseKey; ///< The key type used for poses typedef TypedSymbol PointKey; ///< The key type used for points - typedef LieValues PoseValues; ///< Values used for poses - typedef LieValues PointValues; ///< Values used for points + typedef Values PoseValues; ///< Values used for poses + typedef Values PointValues; ///< Values used for points typedef TupleValues2 Values; ///< Values data structure typedef boost::shared_ptr shared_values; ///< shared pointer to values data structure diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index e05ccde99..3027ab02f 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -20,14 +20,14 @@ #include #include #include -#include +#include #include using namespace gtsam; // Define Types for System Test typedef TypedSymbol TestKey; -typedef LieValues TestValues; +typedef Values TestValues; /* ************************************************************************* */ TEST( ExtendedKalmanFilter, linear ) { diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp index 4a849da1d..c7418a673 100644 --- a/tests/testGaussianJunctionTree.cpp +++ b/tests/testGaussianJunctionTree.cpp @@ -109,8 +109,8 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal ) TEST( GaussianJunctionTree, optimizeMultiFrontal2) { // create a graph - Graph nlfg = createNonlinearFactorGraph(); - Values noisy = createNoisyValues(); + example::Graph nlfg = createNonlinearFactorGraph(); + example::Values noisy = createNoisyValues(); Ordering ordering; ordering += "x1","x2","l1"; GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering); diff --git a/tests/testInference.cpp b/tests/testInference.cpp index 5dc593538..555f8a061 100644 --- a/tests/testInference.cpp +++ b/tests/testInference.cpp @@ -50,29 +50,27 @@ TEST( Inference, marginals ) /* ************************************************************************* */ TEST( Inference, marginals2) { - using namespace gtsam::planarSLAM; - - Graph fg; + planarSLAM::Graph fg; SharedDiagonal poseModel(sharedSigma(3, 0.1)); SharedDiagonal pointModel(sharedSigma(3, 0.1)); - fg.addPrior(PoseKey(0), Pose2(), poseModel); - fg.addOdometry(PoseKey(0), PoseKey(1), Pose2(1.0,0.0,0.0), poseModel); - fg.addOdometry(PoseKey(1), PoseKey(2), Pose2(1.0,0.0,0.0), poseModel); - fg.addBearingRange(PoseKey(0), PointKey(0), Rot2(), 1.0, pointModel); - fg.addBearingRange(PoseKey(1), PointKey(0), Rot2(), 1.0, pointModel); - fg.addBearingRange(PoseKey(2), PointKey(0), Rot2(), 1.0, pointModel); + 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); - Values init; - init.insert(PoseKey(0), Pose2(0.0,0.0,0.0)); - init.insert(PoseKey(1), Pose2(1.0,0.0,0.0)); - init.insert(PoseKey(2), Pose2(2.0,0.0,0.0)); - init.insert(PointKey(0), Point2(1.0,1.0)); + planarSLAM::Values init; + init.insert(planarSLAM::PoseKey(0), Pose2(0.0,0.0,0.0)); + init.insert(planarSLAM::PoseKey(1), Pose2(1.0,0.0,0.0)); + init.insert(planarSLAM::PoseKey(2), Pose2(2.0,0.0,0.0)); + init.insert(planarSLAM::PointKey(0), Point2(1.0,1.0)); Ordering ordering(*fg.orderingCOLAMD(init)); FactorGraph::shared_ptr gfg(fg.linearize(init, ordering)); GaussianMultifrontalSolver solver(*gfg); - solver.marginalFactor(ordering[PointKey(0)]); + solver.marginalFactor(ordering[planarSLAM::PointKey(0)]); } /* ************************************************************************* */ diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 6292d1f20..5e988f2f3 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -22,13 +22,13 @@ #include #include -#include +#include using namespace std; using namespace gtsam; typedef TypedSymbol PoseKey; -typedef LieValues PoseValues; +typedef Values PoseValues; typedef PriorFactor PosePrior; typedef NonlinearEquality PoseNLE; typedef boost::shared_ptr shared_poseNLE; diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 2eda663c6..3ea0c10ba 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -35,7 +35,7 @@ #include #include #include -#include +#include using namespace std; using namespace gtsam; @@ -83,14 +83,14 @@ TEST( NonlinearFactor, NonlinearFactor ) Graph fg = createNonlinearFactorGraph(); // create a values structure for the non linear factor graph - Values cfg = createNoisyValues(); + example::Values cfg = createNoisyValues(); // get the factor "f1" from the factor graph Graph::sharedFactor factor = fg[0]; // calculate the error_vector from the factor "f1" // error_vector = [0.1 0.1] - Vector actual_e = boost::dynamic_pointer_cast >(factor)->unwhitenedError(cfg); + Vector actual_e = boost::dynamic_pointer_cast >(factor)->unwhitenedError(cfg); CHECK(assert_equal(0.1*ones(2),actual_e)); // error = 0.5 * [1 1] * [1;1] = 1 @@ -104,7 +104,7 @@ TEST( NonlinearFactor, NonlinearFactor ) /* ************************************************************************* */ TEST( NonlinearFactor, linearize_f1 ) { - Values c = createNoisyValues(); + example::Values c = createNoisyValues(); // Grab a non-linear factor Graph nfg = createNonlinearFactorGraph(); @@ -126,7 +126,7 @@ TEST( NonlinearFactor, linearize_f1 ) /* ************************************************************************* */ TEST( NonlinearFactor, linearize_f2 ) { - Values c = createNoisyValues(); + example::Values c = createNoisyValues(); // Grab a non-linear factor Graph nfg = createNonlinearFactorGraph(); @@ -149,7 +149,7 @@ TEST( NonlinearFactor, linearize_f3 ) Graph::sharedFactor nlf = nfg[2]; // We linearize at noisy config from SmallExample - Values c = createNoisyValues(); + example::Values c = createNoisyValues(); GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary()); @@ -166,7 +166,7 @@ TEST( NonlinearFactor, linearize_f4 ) Graph::sharedFactor nlf = nfg[3]; // We linearize at noisy config from SmallExample - Values c = createNoisyValues(); + example::Values c = createNoisyValues(); GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary()); @@ -182,7 +182,7 @@ TEST( NonlinearFactor, size ) Graph fg = createNonlinearFactorGraph(); // create a values structure for the non linear factor graph - Values cfg = createNoisyValues(); + example::Values cfg = createNoisyValues(); // get some factors from the graph Graph::sharedFactor factor1 = fg[0], factor2 = fg[1], @@ -202,7 +202,7 @@ TEST( NonlinearFactor, linearize_constraint1 ) Point2 mu(1., -1.); Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, 1)); - Values config; + example::Values config; config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0)); GaussianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary()); @@ -222,7 +222,7 @@ TEST( NonlinearFactor, linearize_constraint2 ) Point2 z3(1.,-1.); simulated2D::Measurement f0(z3, constraint, 1,1); - Values config; + example::Values config; config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0)); config.insert(simulated2D::PointKey(1), Point2(5.0, 4.0)); GaussianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary()); @@ -236,7 +236,7 @@ TEST( NonlinearFactor, linearize_constraint2 ) /* ************************************************************************* */ typedef TypedSymbol TestKey; -typedef LieValues TestValues; +typedef gtsam::Values TestValues; /* ************************************************************************* */ class TestFactor4 : public NonlinearFactor4 { diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 31f9c495e..74e675777 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -51,11 +51,11 @@ TEST( Graph, equals ) TEST( Graph, error ) { Graph fg = createNonlinearFactorGraph(); - Values c1 = createValues(); + example::Values c1 = createValues(); double actual1 = fg.error(c1); DOUBLES_EQUAL( 0.0, actual1, 1e-9 ); - Values c2 = createNoisyValues(); + example::Values c2 = createNoisyValues(); double actual2 = fg.error(c2); DOUBLES_EQUAL( 5.625, actual2, 1e-9 ); } @@ -85,7 +85,7 @@ TEST( Graph, GET_ORDERING) TEST( Graph, probPrime ) { Graph fg = createNonlinearFactorGraph(); - Values cfg = createValues(); + example::Values cfg = createValues(); // evaluate the probability of the factor graph double actual = fg.probPrime(cfg); @@ -97,7 +97,7 @@ TEST( Graph, probPrime ) TEST( Graph, linearize ) { Graph fg = createNonlinearFactorGraph(); - Values initial = createNoisyValues(); + example::Values initial = createNoisyValues(); boost::shared_ptr > linearized = fg.linearize(initial, *initial.orderingArbitrary()); FactorGraph expected = createGaussianFactorGraph(*initial.orderingArbitrary()); CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 31edc5150..a7f3bb285 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -12,7 +12,7 @@ using namespace gtsam; using namespace planarSLAM; -typedef NonlinearISAM PlanarISAM; +typedef NonlinearISAM PlanarISAM; const double tol=1e-5; @@ -30,8 +30,8 @@ TEST(testNonlinearISAM, markov_chain ) { Graph start_factors; start_factors.addPoseConstraint(key, cur_pose); - Values init; - Values expected; + planarSLAM::Values init; + planarSLAM::Values expected; init.insert(key, cur_pose); expected.insert(key, cur_pose); isam.update(start_factors, init); @@ -43,7 +43,7 @@ TEST(testNonlinearISAM, markov_chain ) { Graph new_factors; PoseKey key1(i-1), key2(i); new_factors.addOdometry(key1, key2, z, model); - Values new_init; + planarSLAM::Values new_init; // perform a check on changing orderings if (i == 5) { @@ -67,7 +67,7 @@ TEST(testNonlinearISAM, markov_chain ) { } // verify values - all but the last one should be very close - Values actual = isam.estimate(); + planarSLAM::Values actual = isam.estimate(); for (size_t i=0; i(PoseKey(2))); EXPECT(equalsObj(PointKey(3))); - EXPECT(equalsObj(values)); + EXPECT(equalsObj(values)); EXPECT(equalsObj(prior)); EXPECT(equalsObj(bearing)); EXPECT(equalsObj(bearingRange)); @@ -502,7 +502,7 @@ TEST (Serialization, planar_system) { // xml EXPECT(equalsXML(PoseKey(2))); EXPECT(equalsXML(PointKey(3))); - EXPECT(equalsXML(values)); + EXPECT(equalsXML(values)); EXPECT(equalsXML(prior)); EXPECT(equalsXML(bearing)); EXPECT(equalsXML(bearingRange)); @@ -524,7 +524,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::visualSLAM::StereoFactor, "gtsam::visualSLAM:: /* ************************************************************************* */ TEST (Serialization, visual_system) { using namespace visualSLAM; - Values values; + visualSLAM::Values values; PoseKey x1(1), x2(2); PointKey l1(1), l2(2); Pose3 pose1 = pose3, pose2 = pose3.inverse(); diff --git a/tests/testTupleValues.cpp b/tests/testTupleValues.cpp index 4655ab303..b75e98102 100644 --- a/tests/testTupleValues.cpp +++ b/tests/testTupleValues.cpp @@ -39,9 +39,9 @@ static const double tol = 1e-5; typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; -typedef LieValues PoseValues; -typedef LieValues PointValues; -typedef TupleValues2 Values; +typedef Values PoseValues; +typedef Values PointValues; +typedef TupleValues2 TestValues; /* ************************************************************************* */ TEST( TupleValues, constructors ) @@ -49,14 +49,14 @@ TEST( TupleValues, constructors ) Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); - Values::Values1 cfg1; + TestValues::Values1 cfg1; cfg1.insert(PoseKey(1), x1); cfg1.insert(PoseKey(2), x2); - Values::Values2 cfg2; + TestValues::Values2 cfg2; cfg2.insert(PointKey(1), l1); cfg2.insert(PointKey(2), l2); - Values actual(cfg1, cfg2), expected; + TestValues actual(cfg1, cfg2), expected; expected.insert(PoseKey(1), x1); expected.insert(PoseKey(2), x2); expected.insert(PointKey(1), l1); @@ -71,13 +71,13 @@ TEST( TupleValues, insert_equals1 ) Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); - Values expected; + TestValues expected; expected.insert(PoseKey(1), x1); expected.insert(PoseKey(2), x2); expected.insert(PointKey(1), l1); expected.insert(PointKey(2), l2); - Values actual; + TestValues actual; actual.insert(PoseKey(1), x1); actual.insert(PoseKey(2), x2); actual.insert(PointKey(1), l1); @@ -92,13 +92,13 @@ TEST( TupleValues, insert_equals2 ) Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); - Values values1; + TestValues values1; values1.insert(PoseKey(1), x1); values1.insert(PoseKey(2), x2); values1.insert(PointKey(1), l1); values1.insert(PointKey(2), l2); - Values values2; + TestValues values2; values2.insert(PoseKey(1), x1); values2.insert(PoseKey(2), x2); values2.insert(PointKey(1), l1); @@ -116,7 +116,7 @@ TEST( TupleValues, insert_duplicate ) Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); - Values values1; + TestValues values1; values1.insert(1, x1); // 3 values1.insert(2, x2); // 6 values1.insert(1, l1); // 8 @@ -134,7 +134,7 @@ TEST( TupleValues, size_dim ) Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); - Values values1; + TestValues values1; values1.insert(PoseKey(1), x1); values1.insert(PoseKey(2), x2); values1.insert(PointKey(1), l1); @@ -150,7 +150,7 @@ TEST(TupleValues, at) Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); - Values values1; + TestValues values1; values1.insert(PoseKey(1), x1); values1.insert(PoseKey(2), x2); values1.insert(PointKey(1), l1); @@ -171,7 +171,7 @@ TEST(TupleValues, zero_expmap_logmap) Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); - Values values1; + TestValues values1; values1.insert(PoseKey(1), x1); values1.insert(PoseKey(2), x2); values1.insert(PointKey(1), l1); @@ -192,13 +192,13 @@ TEST(TupleValues, zero_expmap_logmap) delta[o["l1"]] = Vector_(2, 1.0, 1.1); delta[o["l2"]] = Vector_(2, 1.3, 1.4); - Values expected; + TestValues expected; expected.insert(PoseKey(1), x1.retract(Vector_(3, 1.0, 1.1, 1.2))); expected.insert(PoseKey(2), x2.retract(Vector_(3, 1.3, 1.4, 1.5))); expected.insert(PointKey(1), Point2(5.0, 6.1)); expected.insert(PointKey(2), Point2(10.3, 11.4)); - Values actual = values1.retract(delta, o); + TestValues actual = values1.retract(delta, o); CHECK(assert_equal(expected, actual)); // Check log @@ -216,12 +216,12 @@ typedef TypedSymbol Point3Key; typedef TypedSymbol Point3Key2; // some values types -typedef LieValues PoseValues; -typedef LieValues PointValues; -typedef LieValues LamValues; -typedef LieValues Pose3Values; -typedef LieValues Point3Values; -typedef LieValues Point3Values2; +typedef Values PoseValues; +typedef Values PointValues; +typedef Values LamValues; +typedef Values Pose3Values; +typedef Values Point3Values; +typedef Values Point3Values2; // some TupleValues types typedef TupleValues > ValuesA;