diff --git a/.cproject b/.cproject index 42f61fa16..72285e057 100644 --- a/.cproject +++ b/.cproject @@ -681,6 +681,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -1385,6 +1401,14 @@ true true + + make + -j2 + check + true + true + true + make -j2 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 23870a888..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)); @@ -118,7 +118,7 @@ int main() { // Extract the current estimate of x1,P1 from the Bayes Network VectorValues result = optimize(*linearBayesNet); - Point2 x1_predict = linearizationPoints[x1].expmap(result[ordering->at(x1)]); + Point2 x1_predict = linearizationPoints[x1].retract(result[ordering->at(x1)]); x1_predict.print("X1 Predict"); // Update the new linearization point to the new estimate @@ -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)); @@ -190,7 +190,7 @@ int main() { // Extract the current estimate of x1 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x1_update = linearizationPoints[x1].expmap(result[ordering->at(x1)]); + Point2 x1_update = linearizationPoints[x1].retract(result[ordering->at(x1)]); x1_update.print("X1 Update"); // Update the linearization point to the new estimate @@ -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); @@ -237,7 +237,7 @@ int main() { // Extract the current estimate of x2 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x2_predict = linearizationPoints[x2].expmap(result[ordering->at(x2)]); + Point2 x2_predict = linearizationPoints[x2].retract(result[ordering->at(x2)]); x2_predict.print("X2 Predict"); // Update the linearization point to the new estimate @@ -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)); @@ -281,7 +281,7 @@ int main() { // Extract the current estimate of x2 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x2_update = linearizationPoints[x2].expmap(result[ordering->at(x2)]); + Point2 x2_update = linearizationPoints[x2].retract(result[ordering->at(x2)]); x2_update.print("X2 Update"); // Update the linearization point to the new estimate @@ -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); @@ -326,7 +326,7 @@ int main() { // Extract the current estimate of x3 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x3_predict = linearizationPoints[x3].expmap(result[ordering->at(x3)]); + Point2 x3_predict = linearizationPoints[x3].retract(result[ordering->at(x3)]); x3_predict.print("X3 Predict"); // Update the linearization point to the new estimate @@ -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)); @@ -370,7 +370,7 @@ int main() { // Extract the current estimate of x2 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x3_update = linearizationPoints[x3].expmap(result[ordering->at(x3)]); + Point2 x3_update = linearizationPoints[x3].retract(result[ordering->at(x3)]); x3_update.print("X3 Update"); // Update the linearization point to the new estimate 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/base/Group.h b/gtsam/base/Group.h new file mode 100644 index 000000000..3c56a541a --- /dev/null +++ b/gtsam/base/Group.h @@ -0,0 +1,47 @@ +/** + * @file Group.h + * + * @brief Concept check class for variable types with Group properties + * A Group concept extends a Manifold + * + * @date Nov 5, 2011 + * @author Alex Cunningham + */ + +#pragma once + +namespace gtsam { + +/** + * Concept check for general Group structure + */ +template +class GroupConcept { +private: + static void concept_check(const T& t) { + /** assignment */ + T t2 = t; + + /** compose with another object */ + T compose_ret = t.compose(t2); + + /** invert the object and yield a new one */ + T inverse_ret = t.inverse(); + + /** identity */ + T identity = T::identity(); + } +}; + +} // \namespace gtsam + +/** + * Macros for using the GroupConcept + * - An instantiation for use inside unit tests + * - A typedef for use inside generic algorithms + * + * NOTE: intentionally not in the gtsam namespace to allow for classes not in + * the gtsam namespace to be more easily enforced as testable + */ +#define GTSAM_CONCEPT_GROUP_INST(T) template class gtsam::GroupConcept; +#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; diff --git a/gtsam/base/Lie-inl.h b/gtsam/base/Lie-inl.h index 559a01960..f1bb947a2 100644 --- a/gtsam/base/Lie-inl.h +++ b/gtsam/base/Lie-inl.h @@ -23,8 +23,6 @@ #define INSTANTIATE_LIE(T) \ template T between_default(const T&, const T&); \ template Vector logmap_default(const T&, const T&); \ - template T expmap_default(const T&, const Vector&); \ - template bool equal(const T&, const T&, double); \ - template bool equal(const T&, const T&); \ - template class Lie; + template T expmap_default(const T&, const Vector&); + diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index ea3c730bf..756790f16 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -15,26 +15,18 @@ * @author Richard Roberts * @author Alex Cunningham * + * This concept check provides a specialization on the Manifold type, + * in which the Manifolds represented require an algebra and group structure. + * All Lie types must also be a Manifold. + * * The necessary functions to implement for Lie are defined * below with additional details as to the interface. The * concept checking function in class Lie will check whether or not * the function exists and throw compile-time errors. * - * Returns dimensionality of the tangent space - * inline size_t dim() const; - * - * Returns Exponential map update of T - * A default implementation of expmap(*this, lp) is available: - * expmap_default() - * T expmap(const Vector& v) const; - * - * expmap around identity + * Expmap around identity * static T Expmap(const Vector& v); * - * Returns Log map - * A default implementation of logmap(*this, lp) is available: - * logmap_default() - * Vector logmap(const T& lp) const; * * Logmap around identity * static Vector Logmap(const T& p); @@ -44,19 +36,13 @@ * between_default() * T between(const T& l2) const; * - * compose with another object - * T compose(const T& p) const; - * - * invert the object and yield a new one - * T inverse() const; - * */ #pragma once -#include -#include +#include +#include namespace gtsam { @@ -78,18 +64,15 @@ namespace gtsam { inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));} /** - * Base class for Lie group type - * This class uses the Curiously Recurring Template design pattern to allow for - * concept checking using a private function. + * Concept check class for Lie group type * - * T is the derived Lie type, like Point2, Pose3, etc. + * T is the Lie type, like Point2, Pose3, etc. * * By convention, we use capital letters to designate a static function */ template - class Lie { + class LieConcept { private: - /** concept checking function - implement the functions this demands */ static void concept_check(const T& t) { @@ -101,54 +84,18 @@ namespace gtsam { */ size_t dim_ret = t.dim(); - /** - * Returns Exponential map update of T - * Default implementation calls global binary function - */ - T expmap_ret = t.expmap(gtsam::zero(dim_ret)); - /** expmap around identity */ T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); - /** - * Returns Log map - * Default Implementation calls global binary function - */ - Vector logmap_ret = t.logmap(t2); - /** Logmap around identity */ Vector logmap_identity_ret = T::Logmap(t); /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ T between_ret = t.between(t2); - - /** compose with another object */ - T compose_ret = t.compose(t2); - - /** invert the object and yield a new one */ - T inverse_ret = t.inverse(); } }; - /** Call print on the object */ - template - inline void print(const T& object, const std::string& s = "") { - object.print(s); - } - - /** Call equal on the object */ - template - inline bool equal(const T& obj1, const T& obj2, double tol) { - return obj1.equals(obj2, tol); - } - - /** Call equal on the object without tolerance (use default tolerance) */ - template - inline bool equal(const T& obj1, const T& obj2) { - return obj1.equals(obj2); - } - /** * Three term approximation of the Baker�Campbell�Hausdorff formula * In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y) @@ -156,6 +103,7 @@ namespace gtsam { * formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24 * http://en.wikipedia.org/wiki/Baker�Campbell�Hausdorff_formula */ + /// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere? template T BCH(const T& X, const T& Y) { static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.; @@ -182,25 +130,22 @@ namespace gtsam { return expm(xhat,K); } - /** - * function wrappers for full versions of expmap/logmap - * these will default simple types to using the existing expmap/logmap, - * but more complex ones can be specialized to use improved versions - * - * TODO: replace this approach with a naming scheme that doesn't call - * non-expmap operations "expmap" - use same approach, but with "update" - */ - - /** unary versions */ - template - T ExpmapFull(const Vector& xi) { return T::Expmap(xi); } - template - Vector LogmapFull(const T& p) { return T::Logmap(p); } - - /** binary versions */ - template - T expmapFull(const T& t, const Vector& v) { return t.expmap(v); } - template - Vector logmapFull(const T& t, const T& p2) { return t.logmap(p2); } - } // namespace gtsam + +/** + * Macros for using the LieConcept + * - An instantiation for use inside unit tests + * - A typedef for use inside generic algorithms + * + * NOTE: intentionally not in the gtsam namespace to allow for classes not in + * the gtsam namespace to be more easily enforced as testable + */ +#define GTSAM_CONCEPT_LIE_INST(T) \ + template class gtsam::ManifoldConcept; \ + template class gtsam::GroupConcept; \ + template class gtsam::LieConcept; + +#define GTSAM_CONCEPT_LIE_TYPE(T) \ + typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; \ + typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; \ + typedef gtsam::LieConcept _gtsam_LieConcept_##T; diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 7d73255b7..51439c6f1 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -24,10 +24,10 @@ namespace gtsam { /** * LieScalar is a wrapper around double to allow it to be a Lie type */ - struct LieScalar : public Lie { + struct LieScalar { - /** default constructor - should be unnecessary */ - LieScalar() {} + /** default constructor */ + LieScalar() : d_(0.0) {} /** wrap a double */ LieScalar(double d) : d_(d) {} @@ -45,33 +45,51 @@ namespace gtsam { return fabs(expected.d_ - d_) <= tol; } - /** - * Returns dimensionality of the tangent space - * with member and static versions - */ + // Manifold requirements + + /** Returns dimensionality of the tangent space */ inline size_t dim() const { return 1; } inline static size_t Dim() { return 1; } - /** - * Returns Exponential map update of T - * Default implementation calls global binary function - */ - inline LieScalar expmap(const Vector& v) const { return LieScalar(d_ + v(0)); } + /** Update the LieScalar with a tangent space update */ + inline LieScalar retract(const Vector& v) const { return LieScalar(value() + v(0)); } - /** expmap around identity */ - static inline LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); } + /** @return the local coordinates of another object */ + inline Vector localCoordinates(const LieScalar& t2) const { return Vector_(1,(t2.value() - value())); } - /** - * Returns Log map - * Default Implementation calls global binary function - */ - inline Vector logmap(const LieScalar& lp) const { - return Vector_(1, lp.d_ - d_); + // Group requirements + + /** identity */ + inline static LieScalar identity() { + return LieScalar(); } - /** Logmap around identity - just returns with default cast back */ - static inline Vector Logmap(const LieScalar& p) { return Vector_(1, p.d_); } + /** compose with another object */ + inline LieScalar compose(const LieScalar& p) const { + return LieScalar(d_ + p.d_); + } + /** between operation */ + inline LieScalar between(const LieScalar& l2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -eye(1); + if(H2) *H2 = eye(1); + return LieScalar(l2.value() - value()); + } + + /** invert the object and yield a new one */ + inline LieScalar inverse() const { + return LieScalar(-1.0 * value()); + } + + // Lie functions + + /** Expmap around identity */ + static inline LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); } + + /** Logmap around identity - just returns with default cast back */ + static inline Vector Logmap(const LieScalar& p) { return Vector_(1,p.value()); } private: double d_; diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index bec42f775..5a1b3d580 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -25,7 +25,7 @@ namespace gtsam { /** * LieVector is a wrapper around vector to allow it to be a Lie type */ -struct LieVector : public Vector, public Lie { +struct LieVector : public Vector { /** default constructor - should be unnecessary */ LieVector() {} @@ -57,31 +57,25 @@ struct LieVector : public Vector, public Lie { return gtsam::equal(vector(), expected.vector(), tol); } - /** - * Returns dimensionality of the tangent space - */ + // Manifold requirements + + /** Returns dimensionality of the tangent space */ inline size_t dim() const { return this->size(); } - /** - * Returns Exponential map update of T - * Default implementation calls global binary function - */ - inline LieVector expmap(const Vector& v) const { return LieVector(vector() + v); } + /** Update the LieVector with a tangent space update */ + inline LieVector retract(const Vector& v) const { return LieVector(vector() + v); } - /** expmap around identity */ - static inline LieVector Expmap(const Vector& v) { return LieVector(v); } + /** @return the local coordinates of another object */ + inline Vector localCoordinates(const LieVector& t2) const { return LieVector(t2 - vector()); } - /** - * Returns Log map - * Default Implementation calls global binary function - */ - inline Vector logmap(const LieVector& lp) const { - return lp.vector() - vector(); + // Group requirements + + /** identity - NOTE: no known size at compile time - so zero length */ + inline static LieVector identity() { + throw std::runtime_error("LieVector::identity(): Don't use this function"); + return LieVector(); } - /** Logmap around identity - just returns with default cast back */ - static inline Vector Logmap(const LieVector& p) { return p; } - /** compose with another object */ inline LieVector compose(const LieVector& p) const { return LieVector(vector() + p); @@ -100,5 +94,14 @@ struct LieVector : public Vector, public Lie { inline LieVector inverse() const { return LieVector(-1.0 * vector()); } + + // Lie functions + + /** Expmap around identity */ + static inline LieVector Expmap(const Vector& v) { return LieVector(v); } + + /** Logmap around identity - just returns with default cast back */ + static inline Vector Logmap(const LieVector& p) { return p; } + }; } // \namespace gtsam diff --git a/gtsam/base/Makefile.am b/gtsam/base/Makefile.am index 1414532b7..0d4189dd7 100644 --- a/gtsam/base/Makefile.am +++ b/gtsam/base/Makefile.am @@ -25,7 +25,8 @@ headers += Testable.h TestableAssertions.h numericalDerivative.h sources += timing.cpp debug.cpp check_PROGRAMS += tests/testDebug tests/testTestableAssertions -# Lie Groups +# Manifolds and Lie Groups +headers += Manifold.h Group.h headers += Lie.h Lie-inl.h lieProxies.h LieScalar.h sources += LieVector.cpp check_PROGRAMS += tests/testLieVector tests/testLieScalar diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h new file mode 100644 index 000000000..87506f31f --- /dev/null +++ b/gtsam/base/Manifold.h @@ -0,0 +1,87 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Manifold.h + * @brief Base class and basic functions for Manifold types + * @author Richard Roberts + * @author Alex Cunningham + * + * The necessary functions to implement for Manifold are defined + * below with additional details as to the interface. The + * concept checking function in class Manifold will check whether or not + * the function exists and throw compile-time errors. + * + * Returns dimensionality of the tangent space + * inline size_t dim() const; + * + * Returns Retraction update of T + * T retract(const Vector& v) const; + * + * Returns inverse retraction operation + * Vector localCoordinates(const T& lp) const; + * + */ + +#pragma once + +#include +#include + +namespace gtsam { + + /** + * Concept check class for Manifold types + * Requires a mapping between a linear tangent space and the underlying + * manifold, of which Lie is a specialization. + * + * T is the Manifold type, like Point2, Pose3, etc. + * + * By convention, we use capital letters to designate a static function + */ + template + class ManifoldConcept { + private: + /** concept checking function - implement the functions this demands */ + static void concept_check(const T& t) { + + /** assignment */ + T t2 = t; + + /** + * Returns dimensionality of the tangent space + */ + size_t dim_ret = t.dim(); + + /** + * Returns Retraction update of T + */ + T retract_ret = t.retract(gtsam::zero(dim_ret)); + + /** + * Returns local coordinates of another object + */ + Vector localCoords_ret = t.localCoordinates(t2); + } + }; + +} // namespace gtsam + +/** + * Macros for using the ManifoldConcept + * - An instantiation for use inside unit tests + * - A typedef for use inside generic algorithms + * + * NOTE: intentionally not in the gtsam namespace to allow for classes not in + * the gtsam namespace to be more easily enforced as testable + */ +#define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::ManifoldConcept; +#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index 7fa8bca80..d8829798b 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -63,6 +63,24 @@ namespace gtsam { } }; + /** Call print on the object */ + template + inline void print(const T& object, const std::string& s = "") { + object.print(s); + } + + /** Call equal on the object */ + template + inline bool equal(const T& obj1, const T& obj2, double tol) { + return obj1.equals(obj2, tol); + } + + /** Call equal on the object without tolerance (use default tolerance) */ + template + inline bool equal(const T& obj1, const T& obj2) { + return obj1.equals(obj2); + } + /** * This template works for any type with equals */ @@ -111,6 +129,5 @@ namespace gtsam { * NOTE: intentionally not in the gtsam namespace to allow for classes not in * the gtsam namespace to be more easily enforced as testable */ -/// TODO: find better name for "INST" macro, something like "UNIT" or similar #define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::TestableConcept; #define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::TestableConcept _gtsam_TestableConcept_##T; diff --git a/gtsam/base/lieProxies.h b/gtsam/base/lieProxies.h index 41e031cf7..b1992f741 100644 --- a/gtsam/base/lieProxies.h +++ b/gtsam/base/lieProxies.h @@ -36,13 +36,6 @@ namespace testing { template T compose(const T& t1, const T& t2) { return t1.compose(t2); } - /** expmap and logmap */ - template - Vector logmap(const T& t1, const T& t2) { return t1.logmap(t2); } - - template - T expmap(const T& t1, const Vector& t2) { return t1.expmap(t2); } - /** unary functions */ template T inverse(const T& t) { return t.inverse(); } @@ -54,6 +47,7 @@ namespace testing { template P unrotate(const T& r, const P& pt) { return r.unrotate(pt); } -} -} +} // \namespace testing +} // \namespace gtsam + diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 04bc3eb84..d18db38be 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -67,8 +67,8 @@ namespace gtsam { const size_t n = x.dim(); Vector d = zero(n), g = zero(n); for (size_t j=0;j #include +#include #include using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(LieScalar) +GTSAM_CONCEPT_LIE_INST(LieScalar) const double tol=1e-9; @@ -37,10 +39,10 @@ TEST( testLieScalar, construction ) { } /* ************************************************************************* */ -TEST( testLieScalar, logmap ) { +TEST( testLieScalar, localCoordinates ) { LieScalar lie1(1.), lie2(3.); - EXPECT(assert_equal(Vector_(1, 2.), lie1.logmap(lie2))); + EXPECT(assert_equal(Vector_(1, 2.), lie1.localCoordinates(lie2))); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index e8dfae741..c4c88dc9b 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -17,11 +17,13 @@ #include #include +#include #include using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(LieVector) +GTSAM_CONCEPT_LIE_INST(LieVector) /* ************************************************************************* */ TEST( testLieVector, construction ) { diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 6bdef025c..18bcf8db8 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -23,21 +23,35 @@ namespace gtsam { +/* ************************************************************************* */ Cal3Bundler::Cal3Bundler() : f_(1), k1_(0), k2_(0) {} + +/* ************************************************************************* */ Cal3Bundler::Cal3Bundler(const Vector &v) : f_(v(0)), k1_(v(1)), k2_(v(2)) {} + +/* ************************************************************************* */ Cal3Bundler::Cal3Bundler(const double f, const double k1, const double k2) : f_(f), k1_(k1), k2_(k2) {} +/* ************************************************************************* */ Matrix Cal3Bundler::K() const { return Matrix_(3,3, f_, 0.0, 0.0, 0.0, f_, 0.0, 0.0, 0.0, 1.0); } + +/* ************************************************************************* */ Vector Cal3Bundler::k() const { return Vector_(4, k1_, k2_, 0, 0) ; } + +/* ************************************************************************* */ Vector Cal3Bundler::vector() const { return Vector_(3, f_, k1_, k2_) ; } + +/* ************************************************************************* */ void Cal3Bundler::print(const std::string& s) const { gtsam::print(vector(), s + ".K") ; } +/* ************************************************************************* */ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { if (fabs(f_ - K.f_) > tol || fabs(k1_ - K.k1_) > tol || fabs(k2_ - K.k2_) > tol) return false; return true ; } +/* ************************************************************************* */ Point2 Cal3Bundler::uncalibrate(const Point2& p, boost::optional H1, boost::optional H2) const { @@ -78,6 +92,7 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, return Point2(fg * x, fg * y) ; } +/* ************************************************************************* */ Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const { const double x = p.x(), y = p.y(), xx = x*x, yy = y*y; const double r = xx + yy ; @@ -95,6 +110,7 @@ Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const { return DK * DR; } +/* ************************************************************************* */ Matrix Cal3Bundler::D2d_calibration(const Point2& p) const { const double x = p.x(), y = p.y(), xx = x*x, yy = y*y ; @@ -109,6 +125,7 @@ Matrix Cal3Bundler::D2d_calibration(const Point2& p) const { g*y, f*y*r , f*y*r2); } +/* ************************************************************************* */ Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const { const double x = p.x(), y = p.y(), xx = x*x, yy = y*y; @@ -128,12 +145,10 @@ Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const { return H ; } +/* ************************************************************************* */ +Cal3Bundler Cal3Bundler::retract(const Vector& d) const { return Cal3Bundler(vector() + d) ; } -Cal3Bundler Cal3Bundler::expmap(const Vector& d) const { return Cal3Bundler(vector() + d) ; } -Vector Cal3Bundler::logmap(const Cal3Bundler& T2) const { return vector() - T2.vector(); } -Cal3Bundler Cal3Bundler::Expmap(const Vector& v) { return Cal3Bundler(v) ; } -Vector Cal3Bundler::Logmap(const Cal3Bundler& p) { return p.vector(); } - - +/* ************************************************************************* */ +Vector Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const { return vector() - T2.vector(); } } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 620dabad6..bb8ea59c8 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -44,18 +44,15 @@ public: bool equals(const Cal3Bundler& K, double tol = 10e-9) const; Point2 uncalibrate(const Point2& p, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const ; + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const ; Matrix D2d_intrinsic(const Point2& p) const ; Matrix D2d_calibration(const Point2& p) const ; Matrix D2d_intrinsic_calibration(const Point2& p) const ; - Cal3Bundler expmap(const Vector& d) const ; - Vector logmap(const Cal3Bundler& T2) const ; - - static Cal3Bundler Expmap(const Vector& v) ; - static Vector Logmap(const Cal3Bundler& p) ; + Cal3Bundler retract(const Vector& d) const ; + Vector localCoordinates(const Cal3Bundler& T2) const ; int dim() const { return 3 ; } static size_t Dim() { return 3; } diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index 069ec4efb..dcb6d6d54 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -23,20 +23,30 @@ namespace gtsam { +/* ************************************************************************* */ Cal3DS2::Cal3DS2():fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0){} -// Construction +/* ************************************************************************* */ Cal3DS2::Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4) : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {} +/* ************************************************************************* */ Cal3DS2::Cal3DS2(const Vector &v): fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(v[8]){} +/* ************************************************************************* */ Matrix Cal3DS2::K() const { return Matrix_(3,3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); } + +/* ************************************************************************* */ Vector Cal3DS2::k() const { return Vector_(4, k1_, k2_, k3_, k4_); } + +/* ************************************************************************* */ Vector Cal3DS2::vector() const { return Vector_(9, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_) ; } + +/* ************************************************************************* */ void Cal3DS2::print(const std::string& s) const { gtsam::print(K(), s + ".K") ; gtsam::print(k(), s + ".k") ; } +/* ************************************************************************* */ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || @@ -45,6 +55,7 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { return true ; } +/* ************************************************************************* */ Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional H1, boost::optional H2) const { @@ -67,6 +78,7 @@ Point2 Cal3DS2::uncalibrate(const Point2& p, return Point2(fx_* x2 + s_ * y2 + u0_, fy_ * y2 + v0_) ; } +/* ************************************************************************* */ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const { //const double fx = fx_, fy = fy_, s = s_ ; const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_; @@ -94,7 +106,7 @@ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const { return DK * DR; } - +/* ************************************************************************* */ Matrix Cal3DS2::D2d_calibration(const Point2& p) const { const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y ; const double r = xx + yy ; @@ -111,10 +123,11 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const { 0.0, pny, 0.0, 0.0, 1.0, fy*y*r , fy*y*r2 , fy*(r+2*yy) , fy*(2*xy) ); } -Cal3DS2 Cal3DS2::expmap(const Vector& d) const { return Cal3DS2(vector() + d) ; } -Vector Cal3DS2::logmap(const Cal3DS2& T2) const { return vector() - T2.vector(); } -Cal3DS2 Cal3DS2::Expmap(const Vector& v) { return Cal3DS2(v) ; } -Vector Cal3DS2::Logmap(const Cal3DS2& p) { return p.vector(); } +/* ************************************************************************* */ +Cal3DS2 Cal3DS2::retract(const Vector& d) const { return Cal3DS2(vector() + d) ; } + +/* ************************************************************************* */ +Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const { return vector() - T2.vector(); } } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 595247249..dcb596d70 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -22,72 +22,69 @@ namespace gtsam { - /** - * @brief Calibration of a camera with radial distortion - * @ingroup geometry - */ - class Cal3DS2 { - private: - double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point - double k1_, k2_ ; // radial 2nd-order and 4th-order - double k3_, k4_ ; // tagential distortion +/** + * @brief Calibration of a camera with radial distortion + * @ingroup geometry + */ +class Cal3DS2 { +private: + double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point + double k1_, k2_ ; // radial 2nd-order and 4th-order + double k3_, k4_ ; // tagential distortion - // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] - // r = Pn.x^2 + Pn.y^2 - // \hat{pn} = (1 + k1*r + k2*r^2 ) pn + [ 2*k3 pn.x pn.y + k4 (r + 2 Pn.x^2) ; - // k3 (r + 2 Pn.y^2) + 2*k4 pn.x pn.y ] - // pi = K*pn + // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] + // r = Pn.x^2 + Pn.y^2 + // \hat{pn} = (1 + k1*r + k2*r^2 ) pn + [ 2*k3 pn.x pn.y + k4 (r + 2 Pn.x^2) ; + // k3 (r + 2 Pn.y^2) + 2*k4 pn.x pn.y ] + // pi = K*pn - public: - // Default Constructor with only unit focal length - Cal3DS2(); +public: + // Default Constructor with only unit focal length + Cal3DS2(); - // Construction - Cal3DS2(double fx, double fy, double s, double u0, double v0, - double k1, double k2, double k3, double k4) ; + // Construction + Cal3DS2(double fx, double fy, double s, double u0, double v0, + double k1, double k2, double k3, double k4) ; - Cal3DS2(const Vector &v) ; + Cal3DS2(const Vector &v) ; - Matrix K() const ; - Vector k() const ; - Vector vector() const ; - void print(const std::string& s = "") const ; - bool equals(const Cal3DS2& K, double tol = 10e-9) const; + Matrix K() const ; + Vector k() const ; + Vector vector() const ; + void print(const std::string& s = "") const ; + bool equals(const Cal3DS2& K, double tol = 10e-9) const; - Point2 uncalibrate(const Point2& p, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const ; + Point2 uncalibrate(const Point2& p, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const ; - Matrix D2d_intrinsic(const Point2& p) const ; - Matrix D2d_calibration(const Point2& p) const ; + Matrix D2d_intrinsic(const Point2& p) const ; + Matrix D2d_calibration(const Point2& p) const ; - Cal3DS2 expmap(const Vector& d) const ; - Vector logmap(const Cal3DS2& T2) const ; + Cal3DS2 retract(const Vector& d) const ; + Vector localCoordinates(const Cal3DS2& T2) const ; - static Cal3DS2 Expmap(const Vector& v) ; - static Vector Logmap(const Cal3DS2& p) ; + int dim() const { return 9 ; } + static size_t Dim() { return 9; } - int dim() const { return 9 ; } - static size_t Dim() { return 9; } +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(fx_); + ar & BOOST_SERIALIZATION_NVP(fy_); + ar & BOOST_SERIALIZATION_NVP(s_); + ar & BOOST_SERIALIZATION_NVP(u0_); + ar & BOOST_SERIALIZATION_NVP(v0_); + ar & BOOST_SERIALIZATION_NVP(k1_); + ar & BOOST_SERIALIZATION_NVP(k2_); + ar & BOOST_SERIALIZATION_NVP(k3_); + ar & BOOST_SERIALIZATION_NVP(k4_); + } - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) - { - ar & BOOST_SERIALIZATION_NVP(fx_); - ar & BOOST_SERIALIZATION_NVP(fy_); - ar & BOOST_SERIALIZATION_NVP(s_); - ar & BOOST_SERIALIZATION_NVP(u0_); - ar & BOOST_SERIALIZATION_NVP(v0_); - ar & BOOST_SERIALIZATION_NVP(k1_); - ar & BOOST_SERIALIZATION_NVP(k2_); - ar & BOOST_SERIALIZATION_NVP(k3_); - ar & BOOST_SERIALIZATION_NVP(k4_); - } - - }; +}; } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index d2235e699..e78b1d418 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -129,12 +129,12 @@ namespace gtsam { } /// Given 5-dim tangent vector, create new calibration - inline Cal3_S2 expmap(const Vector& d) const { + inline Cal3_S2 retract(const Vector& d) const { return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4)); } - /// logmap for the calibration - Vector logmap(const Cal3_S2& T2) const { + /// Unretraction for the calibration + Vector localCoordinates(const Cal3_S2& T2) const { return vector() - T2.vector(); } diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 6b528b51a..f0c70408e 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -21,77 +21,73 @@ namespace gtsam { - CalibratedCamera::CalibratedCamera() {} +/* ************************************************************************* */ +CalibratedCamera::CalibratedCamera(const Pose3& pose) : + pose_(pose) { +} - CalibratedCamera::CalibratedCamera(const Pose3& pose) : - pose_(pose) { +/* ************************************************************************* */ +CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(Pose3::Expmap(v)) {} + +/* ************************************************************************* */ +Point2 CalibratedCamera::project_to_camera(const Point3& P, boost::optional H1) { + if (H1) { + double d = 1.0 / P.z(), d2 = d * d; + *H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); } + return Point2(P.x() / P.z(), P.y() / P.z()); +} - CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(Pose3::Expmap(v)) {} +/* ************************************************************************* */ +Point3 CalibratedCamera::backproject_from_camera(const Point2& p, const double scale) { + return Point3(p.x() * scale, p.y() * scale, scale); +} - CalibratedCamera::~CalibratedCamera() {} +/* ************************************************************************* */ +CalibratedCamera CalibratedCamera::level(const Pose2& pose2, double height) { + double st = sin(pose2.theta()), ct = cos(pose2.theta()); + Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); + Rot3 wRc(x, y, z); + Point3 t(pose2.x(), pose2.y(), height); + Pose3 pose3(wRc, t); + return CalibratedCamera(pose3); +} - Point2 CalibratedCamera::project_to_camera(const Point3& P, boost::optional H1) { - if (H1) { - double d = 1.0 / P.z(), d2 = d * d; - *H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); - } - return Point2(P.x() / P.z(), P.y() / P.z()); +/* ************************************************************************* */ +Point2 CalibratedCamera::project(const Point3& point, + boost::optional D_intrinsic_pose, + boost::optional D_intrinsic_point) const { + const Pose3& pose = pose_; + const Rot3& R = pose.rotation(); + const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); + Point3 q = pose.transform_to(point); + + if (D_intrinsic_pose || D_intrinsic_point) { + double X = q.x(), Y = q.y(), Z = q.z(); + double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2; + double dp11 = d*r1.x()-r3.x()*Xd2, dp12 = d*r1.y()-r3.y()*Xd2, dp13 = d*r1.z()-r3.z()*Xd2; + double dp21 = d*r2.x()-r3.x()*Yd2, dp22 = d*r2.y()-r3.y()*Yd2, dp23 = d*r2.z()-r3.z()*Yd2; + if (D_intrinsic_pose) + *D_intrinsic_pose = Matrix_(2,6, + X*Yd2, -Z*d-X*Xd2, d*Y, -dp11, -dp12, -dp13, + d*Z+Y*Yd2, -X*Yd2, -d*X, -dp21, -dp22, -dp23); + if (D_intrinsic_point) + *D_intrinsic_point = Matrix_(2,3, + dp11, dp12, dp13, + dp21, dp22, dp23); } + return project_to_camera(q); +} - Point3 CalibratedCamera::backproject_from_camera(const Point2& p, const double scale) { - return Point3(p.x() * scale, p.y() * scale, scale); - } +/* ************************************************************************* */ +CalibratedCamera CalibratedCamera::retract(const Vector& d) const { + return CalibratedCamera(pose().retract(d)) ; +} - CalibratedCamera CalibratedCamera::level(const Pose2& pose2, double height) { - double st = sin(pose2.theta()), ct = cos(pose2.theta()); - Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); - Rot3 wRc(x, y, z); - Point3 t(pose2.x(), pose2.y(), height); - Pose3 pose3(wRc, t); - return CalibratedCamera(pose3); - } - - Point2 CalibratedCamera::project(const Point3& point, - boost::optional D_intrinsic_pose, - boost::optional D_intrinsic_point) const { - const Pose3& pose = pose_; - const Rot3& R = pose.rotation(); - const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); - Point3 q = pose.transform_to(point); - - if (D_intrinsic_pose || D_intrinsic_point) { - double X = q.x(), Y = q.y(), Z = q.z(); - double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2; - double dp11 = d*r1.x()-r3.x()*Xd2, dp12 = d*r1.y()-r3.y()*Xd2, dp13 = d*r1.z()-r3.z()*Xd2; - double dp21 = d*r2.x()-r3.x()*Yd2, dp22 = d*r2.y()-r3.y()*Yd2, dp23 = d*r2.z()-r3.z()*Yd2; - if (D_intrinsic_pose) - *D_intrinsic_pose = Matrix_(2,6, - X*Yd2, -Z*d-X*Xd2, d*Y, -dp11, -dp12, -dp13, - d*Z+Y*Yd2, -X*Yd2, -d*X, -dp21, -dp22, -dp23); - if (D_intrinsic_point) - *D_intrinsic_point = Matrix_(2,3, - dp11, dp12, dp13, - dp21, dp22, dp23); - } - return project_to_camera(q); - } - - - CalibratedCamera CalibratedCamera::expmap(const Vector& d) const { - return CalibratedCamera(pose().expmap(d)) ; - } - Vector CalibratedCamera::logmap(const CalibratedCamera& T2) const { - return pose().logmap(T2.pose()) ; - } - - CalibratedCamera CalibratedCamera::Expmap(const Vector& v) { - return CalibratedCamera(Pose3::Expmap(v)) ; - } - - Vector CalibratedCamera::Logmap(const CalibratedCamera& p) { - return Pose3::Logmap(p.pose()) ; - } +/* ************************************************************************* */ +Vector CalibratedCamera::localCoordinates(const CalibratedCamera& T2) const { + return pose().localCoordinates(T2.pose()) ; +} /* ************************************************************************* */ } diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index a7d11d13e..880bb9316 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -34,10 +34,10 @@ namespace gtsam { Pose3 pose_; // 6DOF pose public: - CalibratedCamera(); ///< default constructor + CalibratedCamera() {} ///< default constructor CalibratedCamera(const Pose3& pose); ///< construct with pose CalibratedCamera(const Vector &v) ; ///< construct from vector - virtual ~CalibratedCamera(); ///< destructor + virtual ~CalibratedCamera() {} ///< destructor /// return pose inline const Pose3& pose() const { return pose_; } @@ -58,16 +58,10 @@ namespace gtsam { } /// move a cameras pose according to d - CalibratedCamera expmap(const Vector& d) const; + CalibratedCamera retract(const Vector& d) const; /// Return canonical coordinate - Vector logmap(const CalibratedCamera& T2) const; - - /// move a cameras pose according to d - static CalibratedCamera Expmap(const Vector& v); - - /// Return canonical coordinate - static Vector Logmap(const CalibratedCamera& p); + Vector localCoordinates(const CalibratedCamera& T2) const; /// Lie group dimensionality inline size_t dim() const { return 6 ; } diff --git a/gtsam/geometry/CalibratedCameraT.h b/gtsam/geometry/CalibratedCameraT.h index c432364e6..bfc622851 100644 --- a/gtsam/geometry/CalibratedCameraT.h +++ b/gtsam/geometry/CalibratedCameraT.h @@ -17,6 +17,7 @@ namespace gtsam { * A Calibrated camera class [R|-R't], calibration K. * If calibration is known, it is more computationally efficient * to calibrate the measurements rather than try to predict in pixels. + * AGC: Is this used or tested anywhere? * @ingroup geometry */ template @@ -49,11 +50,11 @@ namespace gtsam { return CalibratedCameraT( pose_.inverse(), k_ ) ; } - CalibratedCameraT expmap(const Vector& d) const { - return CalibratedCameraT(pose().expmap(d), k_) ; + CalibratedCameraT retract(const Vector& d) const { + return CalibratedCameraT(pose().retract(d), k_) ; } - Vector logmap(const CalibratedCameraT& T2) const { - return pose().logmap(T2.pose()) ; + Vector localCoordinates(const CalibratedCameraT& T2) const { + return pose().localCoordinates(T2.pose()) ; } void print(const std::string& s = "") const { diff --git a/gtsam/geometry/GeneralCameraT.h b/gtsam/geometry/GeneralCameraT.h index 6ec87e0d8..61d82b62b 100644 --- a/gtsam/geometry/GeneralCameraT.h +++ b/gtsam/geometry/GeneralCameraT.h @@ -32,181 +32,164 @@ namespace gtsam { template class GeneralCameraT { - private: - Camera calibrated_; // Calibrated camera - Calibration calibration_; // Calibration +private: + Camera calibrated_; // Calibrated camera + Calibration calibration_; // Calibration - public: - GeneralCameraT(){} - GeneralCameraT(const Camera& calibrated, const Calibration& K) : calibrated_(calibrated), calibration_(K) {} - GeneralCameraT(const Camera& calibrated ) : calibrated_(calibrated) {} - GeneralCameraT(const Pose3& pose, const Calibration& K) : calibrated_(pose), calibration_(K) {} - GeneralCameraT(const Pose3& pose) : calibrated_(pose) {} - GeneralCameraT(const Pose3& pose, const Vector &v) : calibrated_(pose), calibration_(v) {} +public: + GeneralCameraT(){} + GeneralCameraT(const Camera& calibrated, const Calibration& K) : calibrated_(calibrated), calibration_(K) {} + GeneralCameraT(const Camera& calibrated ) : calibrated_(calibrated) {} + GeneralCameraT(const Pose3& pose, const Calibration& K) : calibrated_(pose), calibration_(K) {} + GeneralCameraT(const Pose3& pose) : calibrated_(pose) {} + GeneralCameraT(const Pose3& pose, const Vector &v) : calibrated_(pose), calibration_(v) {} - // Vector Initialization - GeneralCameraT(const Vector &v) : - calibrated_(sub(v, 0, Camera::Dim())), - calibration_(sub(v, Camera::Dim(), Camera::Dim() + Calibration::Dim() )) {} + // Vector Initialization + GeneralCameraT(const Vector &v) : + calibrated_(sub(v, 0, Camera::Dim())), + calibration_(sub(v, Camera::Dim(), Camera::Dim() + Calibration::Dim() )) {} - inline const Pose3& pose() const { return calibrated_.pose(); } - inline const Camera& calibrated() const { return calibrated_; } - inline const Calibration& calibration() const { return calibration_; } + inline const Pose3& pose() const { return calibrated_.pose(); } + inline const Camera& calibrated() const { return calibrated_; } + inline const Calibration& calibration() const { return calibration_; } - std::pair projectSafe( - const Point3& P, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + std::pair projectSafe( + const Point3& P, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { - Point3 cameraPoint = calibrated_.pose().transform_to(P); - return std::pair( - project(P,H1,H2), - cameraPoint.z() > 0); + Point3 cameraPoint = calibrated_.pose().transform_to(P); + return std::pair( + project(P,H1,H2), + cameraPoint.z() > 0); + } + + Point3 backproject(const Point2& projection, const double scale) const { + Point2 intrinsic = calibration_.calibrate(projection); + Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale); + return calibrated_.pose().transform_from(cameraPoint); + } + + /** + * project function that does not merge the Jacobians of calibration and pose + */ + Point2 project(const Point3& P, Matrix& H1_pose, Matrix& H1_k, Matrix& H2_pt) const { + Matrix tmp; + Point2 intrinsic = calibrated_.project(P, H1_pose, H2_pt); + Point2 projection = calibration_.uncalibrate(intrinsic, H1_k, tmp); + H1_pose = tmp * H1_pose; + H2_pt = tmp * H2_pt; + return projection; + } + + /** + * project a 3d point to the camera + * P is point in camera coordinate + * H1 is respect to pose + calibration + * H2 is respect to landmark + */ + Point2 project(const Point3& P, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + + if (!H1 && !H2) { + Point2 intrinsic = calibrated_.project(P); + return calibration_.uncalibrate(intrinsic) ; } - Point3 backproject(const Point2& projection, const double scale) const { - Point2 intrinsic = calibration_.calibrate(projection); - Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale); - return calibrated_.pose().transform_from(cameraPoint); - } + Matrix H1_k, H1_pose, H2_pt; + Point2 projection = project(P, H1_pose, H1_k, H2_pt); + if ( H1 ) *H1 = collect(2, &H1_pose, &H1_k); + if ( H2 ) *H2 = H2_pt; - /** - * project function that does not merge the Jacobians of calibration and pose - */ - Point2 project(const Point3& P, Matrix& H1_pose, Matrix& H1_k, Matrix& H2_pt) const { - Matrix tmp; - Point2 intrinsic = calibrated_.project(P, H1_pose, H2_pt); - Point2 projection = calibration_.uncalibrate(intrinsic, H1_k, tmp); - H1_pose = tmp * H1_pose; - H2_pt = tmp * H2_pt; - return projection; - } + return projection; + } - /** - * project a 3d point to the camera - * P is point in camera coordinate - * H1 is respect to pose + calibration - * H2 is respect to landmark - */ - Point2 project(const Point3& P, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + // dump functions for compilation for now + bool equals (const GeneralCameraT &camera, double tol = 1e-9) const { + return calibrated_.equals(camera.calibrated_, tol) && + calibration_.equals(camera.calibration_, tol) ; + } - if (!H1 && !H2) { - Point2 intrinsic = calibrated_.project(P); - return calibration_.uncalibrate(intrinsic) ; - } + void print(const std::string& s = "") const { + calibrated_.pose().print(s + ".camera.") ; + calibration_.print(s + ".calibration.") ; + } - Matrix H1_k, H1_pose, H2_pt; - Point2 projection = project(P, H1_pose, H1_k, H2_pt); - if ( H1 ) *H1 = collect(2, &H1_pose, &H1_k); - if ( H2 ) *H2 = H2_pt; + GeneralCameraT retract(const Vector &v) const { + Vector v1 = sub(v,0,Camera::Dim()); + Vector v2 = sub(v,Camera::Dim(),Camera::Dim()+Calibration::Dim()); + return GeneralCameraT(calibrated_.retract(v1), calibration_.retract(v2)); + } - return projection; - } + Vector localCoordinates(const GeneralCameraT &C) const { + const Vector v1(calibrated().localCoordinates(C.calibrated())), + v2(calibration().localCoordinates(C.calibration())); + return concatVectors(2,&v1,&v2) ; + } - // dump functions for compilation for now - bool equals (const GeneralCameraT &camera, double tol = 1e-9) const { - return calibrated_.equals(camera.calibrated_, tol) && - calibration_.equals(camera.calibration_, tol) ; - } + inline GeneralCameraT compose(const Pose3 &p) const { + return GeneralCameraT( pose().compose(p), calibration_ ) ; + } - void print(const std::string& s = "") const { - calibrated_.pose().print(s + ".camera.") ; - calibration_.print(s + ".calibration.") ; - } + Matrix D2d_camera(const Point3& point) const { + Point2 intrinsic = calibrated_.project(point); + Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point); + Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); + Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose; + Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic); - GeneralCameraT expmap(const Vector &v) const { - Vector v1 = sub(v,0,Camera::Dim()); - Vector v2 = sub(v,Camera::Dim(),Camera::Dim()+Calibration::Dim()); - return GeneralCameraT(calibrated_.expmap(v1), calibration_.expmap(v2)); - } + const int n1 = calibrated_.dim() ; + const int n2 = calibration_.dim() ; + Matrix D(2,n1+n2) ; - Vector logmap(const GeneralCameraT &C) const { - //std::cout << "logmap" << std::endl; - const Vector v1(calibrated().logmap(C.calibrated())), - v2(calibration().logmap(C.calibration())); - return concatVectors(2,&v1,&v2) ; - } + sub(D,0,2,0,n1) = D_2d_pose ; + sub(D,0,2,n1,n1+n2) = D_2d_calibration ; + return D; + } - static GeneralCameraT Expmap(const Vector& v) { - //std::cout << "Expmap" << std::endl; - return GeneralCameraT( - Camera::Expmap(sub(v,0,Camera::Dim())), - Calibration::Expmap(sub(v,Camera::Dim(), Camera::Dim()+Calibration::Dim())) - ); - } + Matrix D2d_3d(const Point3& point) const { + Point2 intrinsic = calibrated_.project(point); + Matrix D_intrinsic_3d = Dproject_point(calibrated_, point); + Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); + return D_2d_intrinsic * D_intrinsic_3d; + } - static Vector Logmap(const GeneralCameraT& p) { - //std::cout << "Logmap" << std::endl; - const Vector v1(Camera::Logmap(p.calibrated())), - v2(Calibration::Logmap(p.calibration())); - return concatVectors(2,&v1,&v2); + Matrix D2d_camera_3d(const Point3& point) const { + Point2 intrinsic = calibrated_.project(point); + Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point); + Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); + Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose; + Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic); - } + Matrix D_intrinsic_3d = Dproject_point(calibrated_, point); + Matrix D_2d_3d = D_2d_intrinsic * D_intrinsic_3d; - inline GeneralCameraT compose(const Pose3 &p) const { - return GeneralCameraT( pose().compose(p), calibration_ ) ; - } + const int n1 = calibrated_.dim() ; + const int n2 = calibration_.dim() ; - Matrix D2d_camera(const Point3& point) const { - Point2 intrinsic = calibrated_.project(point); - Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point); - Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); - Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose; - Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic); + Matrix D(2,n1+n2+3) ; - const int n1 = calibrated_.dim() ; - const int n2 = calibration_.dim() ; - Matrix D(2,n1+n2) ; + sub(D,0,2,0,n1) = D_2d_pose ; + sub(D,0,2,n1,n1+n2) = D_2d_calibration ; + sub(D,0,2,n1+n2,n1+n2+3) = D_2d_3d ; + return D; + } - sub(D,0,2,0,n1) = D_2d_pose ; - sub(D,0,2,n1,n1+n2) = D_2d_calibration ; - return D; - } - - Matrix D2d_3d(const Point3& point) const { - Point2 intrinsic = calibrated_.project(point); - Matrix D_intrinsic_3d = Dproject_point(calibrated_, point); - Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); - return D_2d_intrinsic * D_intrinsic_3d; - } - - Matrix D2d_camera_3d(const Point3& point) const { - Point2 intrinsic = calibrated_.project(point); - Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point); - Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); - Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose; - Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic); - - Matrix D_intrinsic_3d = Dproject_point(calibrated_, point); - Matrix D_2d_3d = D_2d_intrinsic * D_intrinsic_3d; - - const int n1 = calibrated_.dim() ; - const int n2 = calibration_.dim() ; - - Matrix D(2,n1+n2+3) ; - - sub(D,0,2,0,n1) = D_2d_pose ; - sub(D,0,2,n1,n1+n2) = D_2d_calibration ; - sub(D,0,2,n1+n2,n1+n2+3) = D_2d_3d ; - return D; - } - - //inline size_t dim() { return Camera::dim() + Calibration::dim() ; } - inline size_t dim() const { return calibrated().dim() + calibration().dim() ; } - static inline size_t Dim() { return Camera::Dim() + Calibration::Dim() ; } + //inline size_t dim() { return Camera::dim() + Calibration::dim() ; } + inline size_t dim() const { return calibrated().dim() + calibration().dim() ; } + static inline size_t Dim() { return Camera::Dim() + Calibration::Dim() ; } private: - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) - { - ar & BOOST_SERIALIZATION_NVP(calibrated_); - ar & BOOST_SERIALIZATION_NVP(calibration_); - } + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(calibrated_); + ar & BOOST_SERIALIZATION_NVP(calibration_); + } - }; +}; typedef GeneralCameraT Cal3BundlerCamera; typedef GeneralCameraT Cal3DS2Camera; diff --git a/gtsam/geometry/Makefile.am b/gtsam/geometry/Makefile.am index 820922d32..e8c7510f4 100644 --- a/gtsam/geometry/Makefile.am +++ b/gtsam/geometry/Makefile.am @@ -24,7 +24,7 @@ check_PROGRAMS += tests/testCal3DS2 tests/testCal3_S2 tests/testCal3Bundler test # Stereo sources += StereoPoint2.cpp StereoCamera.cpp -check_PROGRAMS += tests/testStereoCamera +check_PROGRAMS += tests/testStereoCamera tests/testStereoPoint2 # Tensors headers += tensors.h Tensor1.h Tensor2.h Tensor3.h Tensor4.h Tensor5.h diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index ca83dfa8a..0834cdcda 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -22,22 +22,22 @@ using namespace std; namespace gtsam { - /** Explicit instantiation of base class to export members */ - INSTANTIATE_LIE(Point2); +/** Explicit instantiation of base class to export members */ +INSTANTIATE_LIE(Point2); - /* ************************************************************************* */ - void Point2::print(const string& s) const { - cout << s << "(" << x_ << ", " << y_ << ")" << endl; - } +/* ************************************************************************* */ +void Point2::print(const string& s) const { + cout << s << "(" << x_ << ", " << y_ << ")" << endl; +} - /* ************************************************************************* */ - bool Point2::equals(const Point2& q, double tol) const { - return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol); - } +/* ************************************************************************* */ +bool Point2::equals(const Point2& q, double tol) const { + return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol); +} - /* ************************************************************************* */ - double Point2::norm() const { - return sqrt(x_*x_ + y_*y_); - } +/* ************************************************************************* */ +double Point2::norm() const { + return sqrt(x_*x_ + y_*y_); +} } // namespace gtsam diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 0e9acdbf4..5b335f564 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -23,112 +23,122 @@ namespace gtsam { - /** - * A 2D point - * Complies with the Testable Concept - * Functional, so no set functions: once created, a point is constant. - * @ingroup geometry - */ - class Point2: public Lie { - public: - /// dimension of the variable - used to autodetect sizes - static const size_t dimension = 2; - private: - double x_, y_; - - public: - Point2(): x_(0), y_(0) {} - Point2(const Point2 &p) : x_(p.x_), y_(p.y_) {} - Point2(double x, double y): x_(x), y_(y) {} - Point2(const Vector& v) : x_(v(0)), y_(v(1)) { assert(v.size() == 2); } +/** + * A 2D point + * Complies with the Testable Concept + * Functional, so no set functions: once created, a point is constant. + * @ingroup geometry + */ +class Point2 { +public: + /// dimension of the variable - used to autodetect sizes + static const size_t dimension = 2; +private: + double x_, y_; - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } +public: + Point2(): x_(0), y_(0) {} + Point2(const Point2 &p) : x_(p.x_), y_(p.y_) {} + Point2(double x, double y): x_(x), y_(y) {} + Point2(const Vector& v) : x_(v(0)), y_(v(1)) { assert(v.size() == 2); } - /** print with optional string */ - void print(const std::string& s = "") const; + /** dimension of the variable - used to autodetect sizes */ + inline static size_t Dim() { return dimension; } - /** equals with an tolerance, prints out message if unequal*/ - bool equals(const Point2& q, double tol = 1e-9) const; + /** print with optional string */ + void print(const std::string& s = "") const; - /** Lie requirements */ + /** equals with an tolerance, prints out message if unequal*/ + bool equals(const Point2& q, double tol = 1e-9) const; - /** Size of the tangent space of the Lie type */ - inline size_t dim() const { return dimension; } + // Group requirements - /** "Compose", just adds the coordinates of two points. With optional derivatives */ - inline Point2 compose(const Point2& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = eye(2); - if(H2) *H2 = eye(2); - return *this + p2; - } + /** "Compose", just adds the coordinates of two points. With optional derivatives */ + inline Point2 compose(const Point2& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = eye(2); + if(H2) *H2 = eye(2); + return *this + p2; + } - /** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */ - inline Point2 inverse() const { return Point2(-x_, -y_); } + /** identity */ + inline static Point2 identity() { + return Point2(); + } - /** Binary expmap - just adds the points */ - inline Point2 expmap(const Vector& v) const { return *this + Point2(v); } + /** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */ + inline Point2 inverse() const { return Point2(-x_, -y_); } - /** Binary Logmap - just subtracts the points */ - inline Vector logmap(const Point2& p2) const { return Logmap(between(p2));} + // Manifold requirements - /** Exponential map around identity - just create a Point2 from a vector */ - static inline Point2 Expmap(const Vector& v) { return Point2(v); } + /** Size of the tangent space */ + inline size_t dim() const { return dimension; } - /** Log map around identity - just return the Point2 as a vector */ - static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } + /** Updates a with tangent space delta */ + inline Point2 retract(const Vector& v) const { return *this + Point2(v); } - /** "Between", subtracts point coordinates */ - inline Point2 between(const Point2& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(2); - if(H2) *H2 = eye(2); - return p2 - (*this); - } + /// Local coordinates of manifold neighborhood around current value + inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); } - /** get functions for x, y */ - double x() const {return x_;} - double y() const {return y_;} + /** Lie requirements */ - /** return vectorized form (column-wise) */ - Vector vector() const { return Vector_(2, x_, y_); } + /** Exponential map around identity - just create a Point2 from a vector */ + static inline Point2 Expmap(const Vector& v) { return Point2(v); } - /** operators */ - inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} - inline void operator *= (double s) {x_*=s;y_*=s;} - inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;} - inline Point2 operator- () const {return Point2(-x_,-y_);} - inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} - inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);} - inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);} - inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);} + /** Log map around identity - just return the Point2 as a vector */ + static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } - /** norm of point */ - double norm() const; - /** creates a unit vector */ - Point2 unit() const { return *this/norm(); } + /** "Between", subtracts point coordinates */ + inline Point2 between(const Point2& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -eye(2); + if(H2) *H2 = eye(2); + return p2 - (*this); + } - /** distance between two points */ - inline double dist(const Point2& p2) const { - return (p2 - *this).norm(); - } + /** get functions for x, y */ + double x() const {return x_;} + double y() const {return y_;} - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) - { - ar & BOOST_SERIALIZATION_NVP(x_); - ar & BOOST_SERIALIZATION_NVP(y_); - } - }; + /** return vectorized form (column-wise) */ + Vector vector() const { return Vector_(2, x_, y_); } - /** multiply with scalar */ - inline Point2 operator*(double s, const Point2& p) {return p*s;} + /** operators */ + inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} + inline void operator *= (double s) {x_*=s;y_*=s;} + inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;} + inline Point2 operator- () const {return Point2(-x_,-y_);} + inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} + inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);} + inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);} + inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);} + + /** norm of point */ + double norm() const; + + /** creates a unit vector */ + Point2 unit() const { return *this/norm(); } + + /** distance between two points */ + inline double dist(const Point2& p2) const { + return (p2 - *this).norm(); + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(x_); + ar & BOOST_SERIALIZATION_NVP(y_); + } +}; + +/** multiply with scalar */ +inline Point2 operator*(double s, const Point2& p) {return p*s;} } diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 9573cc187..c9f89d83c 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -20,71 +20,71 @@ namespace gtsam { - /** Explicit instantiation of base class to export members */ - INSTANTIATE_LIE(Point3); +/** Explicit instantiation of base class to export members */ +INSTANTIATE_LIE(Point3); - /* ************************************************************************* */ - bool Point3::equals(const Point3 & q, double tol) const { - return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol); - } +/* ************************************************************************* */ +bool Point3::equals(const Point3 & q, double tol) const { + return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol); +} - /* ************************************************************************* */ +/* ************************************************************************* */ - void Point3::print(const std::string& s) const { - std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl; - } +void Point3::print(const std::string& s) const { + std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl; +} - /* ************************************************************************* */ +/* ************************************************************************* */ - bool Point3::operator== (const Point3& q) const { - return x_ == q.x_ && y_ == q.y_ && z_ == q.z_; - } +bool Point3::operator== (const Point3& q) const { + return x_ == q.x_ && y_ == q.y_ && z_ == q.z_; +} - /* ************************************************************************* */ - Point3 Point3::operator+(const Point3& q) const { - return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ ); - } +/* ************************************************************************* */ +Point3 Point3::operator+(const Point3& q) const { + return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ ); +} - /* ************************************************************************* */ - Point3 Point3::operator- (const Point3& q) const { - return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ ); - } - /* ************************************************************************* */ - Point3 Point3::operator*(double s) const { - return Point3(x_ * s, y_ * s, z_ * s); - } - /* ************************************************************************* */ - Point3 Point3::operator/(double s) const { - return Point3(x_ / s, y_ / s, z_ / s); - } - /* ************************************************************************* */ - Point3 Point3::add(const Point3 &q, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = eye(3,3); - if (H2) *H2 = eye(3,3); - return *this + q; - } - /* ************************************************************************* */ - Point3 Point3::sub(const Point3 &q, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = eye(3,3); - if (H2) *H2 = -eye(3,3); - return *this - q; - } - /* ************************************************************************* */ - Point3 Point3::cross(const Point3 &q) const { - return Point3( y_*q.z_ - z_*q.y_, - z_*q.x_ - x_*q.z_, - x_*q.y_ - y_*q.x_ ); - } - /* ************************************************************************* */ - double Point3::dot(const Point3 &q) const { - return ( x_*q.x_ + y_*q.y_ + z_*q.z_ ); - } - /* ************************************************************************* */ - double Point3::norm() const { - return sqrt( x_*x_ + y_*y_ + z_*z_ ); - } - /* ************************************************************************* */ +/* ************************************************************************* */ +Point3 Point3::operator- (const Point3& q) const { + return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ ); +} +/* ************************************************************************* */ +Point3 Point3::operator*(double s) const { + return Point3(x_ * s, y_ * s, z_ * s); +} +/* ************************************************************************* */ +Point3 Point3::operator/(double s) const { + return Point3(x_ / s, y_ / s, z_ / s); +} +/* ************************************************************************* */ +Point3 Point3::add(const Point3 &q, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = eye(3,3); + if (H2) *H2 = eye(3,3); + return *this + q; +} +/* ************************************************************************* */ +Point3 Point3::sub(const Point3 &q, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = eye(3,3); + if (H2) *H2 = -eye(3,3); + return *this - q; +} +/* ************************************************************************* */ +Point3 Point3::cross(const Point3 &q) const { + return Point3( y_*q.z_ - z_*q.y_, + z_*q.x_ - x_*q.z_, + x_*q.y_ - y_*q.x_ ); +} +/* ************************************************************************* */ +double Point3::dot(const Point3 &q) const { + return ( x_*q.x_ + y_*q.y_ + z_*q.z_ ); +} +/* ************************************************************************* */ +double Point3::norm() const { + return sqrt( x_*x_ + y_*y_ + z_*z_ ); +} +/* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 095a2d9fe..2f8c9f8cc 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -60,6 +60,11 @@ namespace gtsam { /** return DOF, dimensionality of tangent space */ inline size_t dim() const { return dimension; } + /** identity */ + inline static Point3 identity() { + return Point3(); + } + /** "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() */ inline Point3 inverse() const { return Point3(-x_, -y_, -z_); } @@ -78,9 +83,14 @@ namespace gtsam { /** Log map at identity - return the x,y,z of this point */ static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); } - /** default implementations of binary functions */ - inline Point3 expmap(const Vector& v) const { return gtsam::expmap_default(*this, v); } - inline Vector logmap(const Point3& p2) const { return gtsam::logmap_default(*this, p2);} + // Manifold requirements + + inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); } + + /** + * Returns inverse retraction + */ + inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); } /** Between using the default implementation */ inline Point3 between(const Point3& p2, diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 605163d5d..a15929c67 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -25,287 +25,278 @@ using namespace std; namespace gtsam { - /** Explicit instantiation of base class to export members */ - INSTANTIATE_LIE(Pose2); +/** Explicit instantiation of base class to export members */ +INSTANTIATE_LIE(Pose2); - /** instantiate concept checks */ - GTSAM_CONCEPT_POSE_INST(Pose2); +/** instantiate concept checks */ +GTSAM_CONCEPT_POSE_INST(Pose2); - static const Matrix I3 = eye(3), Z12 = zeros(1,2); - static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); +static const Matrix I3 = eye(3), Z12 = zeros(1,2); +static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); - /* ************************************************************************* */ - Matrix Pose2::matrix() const { - Matrix R = r_.matrix(); - R = stack(2, &R, &Z12); - Matrix T = Matrix_(3,1, t_.x(), t_.y(), 1.0); - return collect(2, &R, &T); - } +/* ************************************************************************* */ +Matrix Pose2::matrix() const { + Matrix R = r_.matrix(); + R = stack(2, &R, &Z12); + Matrix T = Matrix_(3,1, t_.x(), t_.y(), 1.0); + return collect(2, &R, &T); +} - /* ************************************************************************* */ - void Pose2::print(const string& s) const { - cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl; - } +/* ************************************************************************* */ +void Pose2::print(const string& s) const { + cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl; +} - /* ************************************************************************* */ - bool Pose2::equals(const Pose2& q, double tol) const { - return t_.equals(q.t_, tol) && r_.equals(q.r_, tol); - } +/* ************************************************************************* */ +bool Pose2::equals(const Pose2& q, double tol) const { + return t_.equals(q.t_, tol) && r_.equals(q.r_, tol); +} - /* ************************************************************************* */ - Pose2 Pose2::ExpmapFull(const Vector& xi) { - assert(xi.size() == 3); - Point2 v(xi(0),xi(1)); - double w = xi(2); - if (std::abs(w) < 1e-10) - return Pose2(xi[0], xi[1], xi[2]); - else { - Rot2 R(Rot2::fromAngle(w)); - Point2 v_ortho = R_PI_2 * v; // points towards rot center - Point2 t = (v_ortho - R.rotate(v_ortho)) / w; - return Pose2(R, t); - } +/* ************************************************************************* */ +Pose2 Pose2::Expmap(const Vector& xi) { + assert(xi.size() == 3); + Point2 v(xi(0),xi(1)); + double w = xi(2); + if (std::abs(w) < 1e-10) + return Pose2(xi[0], xi[1], xi[2]); + else { + Rot2 R(Rot2::fromAngle(w)); + Point2 v_ortho = R_PI_2 * v; // points towards rot center + Point2 t = (v_ortho - R.rotate(v_ortho)) / w; + return Pose2(R, t); } +} - /* ************************************************************************* */ - Vector Pose2::LogmapFull(const Pose2& p) { - const Rot2& R = p.r(); - const Point2& t = p.t(); - double w = R.theta(); - if (std::abs(w) < 1e-10) - return Vector_(3, t.x(), t.y(), w); - else { - double c_1 = R.c()-1.0, s = R.s(); - double det = c_1*c_1 + s*s; - Point2 p = R_PI_2 * (R.unrotate(t) - t); - Point2 v = (w/det) * p; - return Vector_(3, v.x(), v.y(), w); - } - } +/* ************************************************************************* */ +Vector Pose2::Logmap(const Pose2& p) { + const Rot2& R = p.r(); + const Point2& t = p.t(); + double w = R.theta(); + if (std::abs(w) < 1e-10) + return Vector_(3, t.x(), t.y(), w); + else { + double c_1 = R.c()-1.0, s = R.s(); + double det = c_1*c_1 + s*s; + Point2 p = R_PI_2 * (R.unrotate(t) - t); + Point2 v = (w/det) * p; + return Vector_(3, v.x(), v.y(), w); + } +} - /* ************************************************************************* */ +/* ************************************************************************* */ +Pose2 Pose2::retract(const Vector& v) const { #ifdef SLOW_BUT_CORRECT_EXPMAP - /* ************************************************************************* */ - // Changes default to use the full verions of expmap/logmap - /* ************************************************************************* */ - Pose2 Pose2::Expmap(const Vector& xi) { - return ExpmapFull(xi); - } - - /* ************************************************************************* */ - Vector Pose2::Logmap(const Pose2& p) { - return LogmapFull(p); - } - + return compose(Expmap(v)); #else - - /* ************************************************************************* */ - Pose2 Pose2::Expmap(const Vector& v) { - assert(v.size() == 3); - return Pose2(v[0], v[1], v[2]); - } - - /* ************************************************************************* */ - Vector Pose2::Logmap(const Pose2& p) { - return Vector_(3, p.x(), p.y(), p.theta()); - } - + assert(v.size() == 3); + return compose(Pose2(v[0], v[1], v[2])); #endif +} - /* ************************************************************************* */ - // Calculate Adjoint map - // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) - Matrix Pose2::AdjointMap() const { - double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y(); - return Matrix_(3,3, - c, -s, y, - s, c, -x, - 0.0, 0.0, 1.0 - ); - } +/* ************************************************************************* */ +Vector Pose2::localCoordinates(const Pose2& p2) const { +#ifdef SLOW_BUT_CORRECT_EXPMAP + return Logmap(between(p2)); +#else + Pose2 r = between(p2); + return Vector_(3, r.x(), r.y(), r.theta()); +#endif +} - /* ************************************************************************* */ - Pose2 Pose2::inverse(boost::optional H1) const { - if (H1) *H1 = -AdjointMap(); - return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); - } +/* ************************************************************************* */ +// Calculate Adjoint map +// Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) +Matrix Pose2::AdjointMap() const { + double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y(); + return Matrix_(3,3, + c, -s, y, + s, c, -x, + 0.0, 0.0, 1.0 + ); +} - /* ************************************************************************* */ - // see doc/math.lyx, SE(2) section - Point2 Pose2::transform_to(const Point2& point, - boost::optional H1, boost::optional H2) const { - Point2 d = point - t_; - Point2 q = r_.unrotate(d); - if (!H1 && !H2) return q; - if (H1) *H1 = Matrix_(2, 3, - -1.0, 0.0, q.y(), - 0.0, -1.0, -q.x()); - if (H2) *H2 = r_.transpose(); - return q; +/* ************************************************************************* */ +Pose2 Pose2::inverse(boost::optional H1) const { + if (H1) *H1 = -AdjointMap(); + return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); +} + +/* ************************************************************************* */ +// see doc/math.lyx, SE(2) section +Point2 Pose2::transform_to(const Point2& point, + boost::optional H1, boost::optional H2) const { + Point2 d = point - t_; + Point2 q = r_.unrotate(d); + if (!H1 && !H2) return q; + if (H1) *H1 = Matrix_(2, 3, + -1.0, 0.0, q.y(), + 0.0, -1.0, -q.x()); + if (H2) *H2 = r_.transpose(); + return q; +} + +/* ************************************************************************* */ +// see doc/math.lyx, SE(2) section +Pose2 Pose2::compose(const Pose2& p2, boost::optional H1, + boost::optional H2) const { + // TODO: inline and reuse? + if(H1) *H1 = p2.inverse().AdjointMap(); + if(H2) *H2 = I3; + return (*this)*p2; +} + +/* ************************************************************************* */ +// see doc/math.lyx, SE(2) section +Point2 Pose2::transform_from(const Point2& p, + boost::optional H1, boost::optional H2) const { + const Point2 q = r_ * p; + if (H1 || H2) { + const Matrix R = r_.matrix(); + const Matrix Drotate1 = Matrix_(2, 1, -q.y(), q.x()); + if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q] + if (H2) *H2 = R; // R + } + return q + t_; +} + +/* ************************************************************************* */ +Pose2 Pose2::between(const Pose2& p2, boost::optional H1, + boost::optional H2) const { + // get cosines and sines from rotation matrices + const Rot2& R1 = r_, R2 = p2.r(); + double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); + + // Assert that R1 and R2 are normalized + assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); + + // Calculate delta rotation = between(R1,R2) + double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; + Rot2 R(Rot2::atan2(s,c)); // normalizes + + // Calculate delta translation = unrotate(R1, dt); + Point2 dt = p2.t() - t_; + double x = dt.x(), y = dt.y(); + Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); + + // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above + if (H1) { + double dt1 = -s2 * x + c2 * y; + double dt2 = -c2 * x - s2 * y; + *H1 = Matrix_(3,3, + -c, -s, dt1, + s, -c, dt2, + 0.0, 0.0,-1.0); + } + if (H2) *H2 = I3; + + return Pose2(R,t); +} + +/* ************************************************************************* */ +Rot2 Pose2::bearing(const Point2& point, + boost::optional H1, boost::optional H2) const { + Point2 d = transform_to(point, H1, H2); + if (!H1 && !H2) return Rot2::relativeBearing(d); + Matrix D_result_d; + Rot2 result = Rot2::relativeBearing(d, D_result_d); + if (H1) *H1 = D_result_d * (*H1); + if (H2) *H2 = D_result_d * (*H2); + return result; +} + +/* ************************************************************************* */ +Rot2 Pose2::bearing(const Pose2& point, + boost::optional H1, boost::optional H2) const { + Rot2 result = bearing(point.t(), H1, H2); + if (H2) { + Matrix H2_ = *H2 * point.r().matrix(); + *H2 = zeros(1, 3); + insertSub(*H2, H2_, 0, 0); + } + return result; +} + +/* ************************************************************************* */ +double Pose2::range(const Point2& point, + boost::optional H1, boost::optional H2) const { + if (!H1 && !H2) return transform_to(point).norm(); + Point2 d = transform_to(point, H1, H2); + double x = d.x(), y = d.y(), d2 = x * x + y * y, r = sqrt(d2); + Matrix D_result_d; + if(std::abs(r) > 1e-10) + D_result_d = Matrix_(1, 2, x / r, y / r); + else { + D_result_d = Matrix_(1,2, 1.0, 1.0); + } + if (H1) *H1 = D_result_d * (*H1); + if (H2) *H2 = D_result_d * (*H2); + return r; +} + +/* ************************************************************************* */ +double Pose2::range(const Pose2& point, + boost::optional H1, + boost::optional H2) const { + double r = range(point.t(), H1, H2); + if (H2) { + // NOTE: expmap changes the orientation of expmap direction, + // so we must rotate the jacobian + Matrix H2_ = *H2 * point.r().matrix(); + *H2 = zeros(1, 3); + insertSub(*H2, H2_, 0, 0); + } + return r; +} + +/* ************************************************************************* + * New explanation, from scan.ml + * It finds the angle using a linear method: + * q = Pose2::transform_from(p) = t + R*p + * We need to remove the centroids from the data to find the rotation + * using dp=[dpx;dpy] and q=[dqx;dqy] we have + * |dqx| |c -s| |dpx| |dpx -dpy| |c| + * | | = | | * | | = | | * | | = H_i*cs + * |dqy| |s c| |dpy| |dpy dpx| |s| + * where the Hi are the 2*2 matrices. Then we will minimize the criterion + * J = \sum_i norm(q_i - H_i * cs) + * Taking the derivative with respect to cs and setting to zero we have + * cs = (\sum_i H_i' * q_i)/(\sum H_i'*H_i) + * The hessian is diagonal and just divides by a constant, but this + * normalization constant is irrelevant, since we take atan2. + * i.e., cos ~ sum(dpx*dqx + dpy*dqy) and sin ~ sum(-dpy*dqx + dpx*dqy) + * The translation is then found from the centroids + * as they also satisfy cq = t + R*cp, hence t = cq - R*cp + */ + +boost::optional align(const vector& pairs) { + + size_t n = pairs.size(); + if (n<2) return boost::none; // we need at least two pairs + + // calculate centroids + Point2 cp,cq; + BOOST_FOREACH(const Point2Pair& pair, pairs) { + cp += pair.first; + cq += pair.second; + } + double f = 1.0/n; + cp *= f; cq *= f; + + // calculate cos and sin + double c=0,s=0; + BOOST_FOREACH(const Point2Pair& pair, pairs) { + Point2 dq = pair.first - cp; + Point2 dp = pair.second - cq; + c += dp.x() * dq.x() + dp.y() * dq.y(); + s += dp.y() * dq.x() - dp.x() * dq.y(); // this works but is negative from formula above !! :-( } - /* ************************************************************************* */ - // see doc/math.lyx, SE(2) section - Pose2 Pose2::compose(const Pose2& p2, boost::optional H1, - boost::optional H2) const { - // TODO: inline and reuse? - if(H1) *H1 = p2.inverse().AdjointMap(); - if(H2) *H2 = I3; - return (*this)*p2; - } + // calculate angle and translation + double theta = atan2(s,c); + Rot2 R = Rot2::fromAngle(theta); + Point2 t = cq - R*cp; + return Pose2(R, t); +} - /* ************************************************************************* */ - // see doc/math.lyx, SE(2) section - Point2 Pose2::transform_from(const Point2& p, - boost::optional H1, boost::optional H2) const { - const Point2 q = r_ * p; - if (H1 || H2) { - const Matrix R = r_.matrix(); - const Matrix Drotate1 = Matrix_(2, 1, -q.y(), q.x()); - if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q] - if (H2) *H2 = R; // R - } - return q + t_; - } - - /* ************************************************************************* */ - Pose2 Pose2::between(const Pose2& p2, boost::optional H1, - boost::optional H2) const { - // get cosines and sines from rotation matrices - const Rot2& R1 = r_, R2 = p2.r(); - double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); - - // Assert that R1 and R2 are normalized - assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); - - // Calculate delta rotation = between(R1,R2) - double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; - Rot2 R(Rot2::atan2(s,c)); // normalizes - - // Calculate delta translation = unrotate(R1, dt); - Point2 dt = p2.t() - t_; - double x = dt.x(), y = dt.y(); - Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); - - // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above - if (H1) { - double dt1 = -s2 * x + c2 * y; - double dt2 = -c2 * x - s2 * y; - *H1 = Matrix_(3,3, - -c, -s, dt1, - s, -c, dt2, - 0.0, 0.0,-1.0); - } - if (H2) *H2 = I3; - - return Pose2(R,t); - } - - /* ************************************************************************* */ - Rot2 Pose2::bearing(const Point2& point, - boost::optional H1, boost::optional H2) const { - Point2 d = transform_to(point, H1, H2); - if (!H1 && !H2) return Rot2::relativeBearing(d); - Matrix D_result_d; - Rot2 result = Rot2::relativeBearing(d, D_result_d); - if (H1) *H1 = D_result_d * (*H1); - if (H2) *H2 = D_result_d * (*H2); - return result; - } - - /* ************************************************************************* */ - Rot2 Pose2::bearing(const Pose2& point, - boost::optional H1, boost::optional H2) const { - Rot2 result = bearing(point.t(), H1, H2); - if (H2) { - Matrix H2_ = *H2 * point.r().matrix(); - *H2 = zeros(1, 3); - insertSub(*H2, H2_, 0, 0); - } - return result; - } - - /* ************************************************************************* */ - double Pose2::range(const Point2& point, - boost::optional H1, boost::optional H2) const { - if (!H1 && !H2) return transform_to(point).norm(); - Point2 d = transform_to(point, H1, H2); - double x = d.x(), y = d.y(), d2 = x * x + y * y, r = sqrt(d2); - Matrix D_result_d; - if(std::abs(r) > 1e-10) - D_result_d = Matrix_(1, 2, x / r, y / r); - else { - D_result_d = Matrix_(1,2, 1.0, 1.0); - } - if (H1) *H1 = D_result_d * (*H1); - if (H2) *H2 = D_result_d * (*H2); - return r; - } - - /* ************************************************************************* */ - double Pose2::range(const Pose2& point, - boost::optional H1, - boost::optional H2) const { - double r = range(point.t(), H1, H2); - if (H2) { - // NOTE: expmap changes the orientation of expmap direction, - // so we must rotate the jacobian - Matrix H2_ = *H2 * point.r().matrix(); - *H2 = zeros(1, 3); - insertSub(*H2, H2_, 0, 0); - } - return r; - } - - /* ************************************************************************* - * New explanation, from scan.ml - * It finds the angle using a linear method: - * q = Pose2::transform_from(p) = t + R*p - * We need to remove the centroids from the data to find the rotation - * using dp=[dpx;dpy] and q=[dqx;dqy] we have - * |dqx| |c -s| |dpx| |dpx -dpy| |c| - * | | = | | * | | = | | * | | = H_i*cs - * |dqy| |s c| |dpy| |dpy dpx| |s| - * where the Hi are the 2*2 matrices. Then we will minimize the criterion - * J = \sum_i norm(q_i - H_i * cs) - * Taking the derivative with respect to cs and setting to zero we have - * cs = (\sum_i H_i' * q_i)/(\sum H_i'*H_i) - * The hessian is diagonal and just divides by a constant, but this - * normalization constant is irrelevant, since we take atan2. - * i.e., cos ~ sum(dpx*dqx + dpy*dqy) and sin ~ sum(-dpy*dqx + dpx*dqy) - * The translation is then found from the centroids - * as they also satisfy cq = t + R*cp, hence t = cq - R*cp - */ - - boost::optional align(const vector& pairs) { - - size_t n = pairs.size(); - if (n<2) return boost::none; // we need at least two pairs - - // calculate centroids - Point2 cp,cq; - BOOST_FOREACH(const Point2Pair& pair, pairs) { - cp += pair.first; - cq += pair.second; - } - double f = 1.0/n; - cp *= f; cq *= f; - - // calculate cos and sin - double c=0,s=0; - BOOST_FOREACH(const Point2Pair& pair, pairs) { - Point2 dq = pair.first - cp; - Point2 dp = pair.second - cq; - c += dp.x() * dq.x() + dp.y() * dq.y(); - s += dp.y() * dq.x() - dp.x() * dq.y(); // this works but is negative from formula above !! :-( - } - - // calculate angle and translation - double theta = atan2(s,c); - Rot2 R = Rot2::fromAngle(theta); - Point2 t = cq - R*cp; - return Pose2(R, t); - } - - /* ************************************************************************* */ +/* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index d69e09874..0e4bf4824 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -27,148 +27,155 @@ namespace gtsam { - /** - * A 2D pose (Point2,Rot2) - * @ingroup geometry - */ - class Pose2: public Lie { +/** + * A 2D pose (Point2,Rot2) + * @ingroup geometry + */ +class Pose2 { - public: - static const size_t dimension = 3; +public: + static const size_t dimension = 3; - /** Pose Concept requirements */ - typedef Rot2 Rotation; - typedef Point2 Translation; + /** Pose Concept requirements */ + typedef Rot2 Rotation; + typedef Point2 Translation; - private: - Rot2 r_; - Point2 t_; +private: + Rot2 r_; + Point2 t_; - public: +public: - /** default constructor = origin */ - Pose2() {} // default is origin + /** default constructor = origin */ + Pose2() {} // default is origin - /** copy constructor */ - Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {} + /** copy constructor */ + Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {} - /** - * construct from (x,y,theta) - * @param x x coordinate - * @param y y coordinate - * @param theta angle with positive X-axis - */ - Pose2(double x, double y, double theta) : - r_(Rot2::fromAngle(theta)), t_(x, y) { - } + /** + * construct from (x,y,theta) + * @param x x coordinate + * @param y y coordinate + * @param theta angle with positive X-axis + */ + Pose2(double x, double y, double theta) : + r_(Rot2::fromAngle(theta)), t_(x, y) { + } - /** construct from rotation and translation */ - Pose2(double theta, const Point2& t) : - r_(Rot2::fromAngle(theta)), t_(t) { - } - Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {} + /** construct from rotation and translation */ + Pose2(double theta, const Point2& t) : + r_(Rot2::fromAngle(theta)), t_(t) { + } + Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {} - /** Constructor from 3*3 matrix */ - Pose2(const Matrix &T) : - r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { - assert(T.rows() == 3 && T.cols() == 3); - } + /** Constructor from 3*3 matrix */ + Pose2(const Matrix &T) : + r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { + assert(T.rows() == 3 && T.cols() == 3); + } - /** Construct from canonical coordinates (Lie algebra) */ - Pose2(const Vector& v) { - *this = Expmap(v); - } + /** Construct from canonical coordinates (Lie algebra) */ + Pose2(const Vector& v) { + *this = Expmap(v); + } - /** print with optional string */ - void print(const std::string& s = "") const; - /** assert equality up to a tolerance */ - bool equals(const Pose2& pose, double tol = 1e-9) const; + /// @name Testable + /// @{ - /** compose syntactic sugar */ - inline Pose2 operator*(const Pose2& p2) const { - return Pose2(r_*p2.r(), t_ + r_*p2.t()); - } + /** print with optional string */ + void print(const std::string& s = "") const; - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /** assert equality up to a tolerance */ + bool equals(const Pose2& pose, double tol = 1e-9) const; - /** Lie requirements */ + /// @} + /// @name Group + /// @{ - /** return DOF, dimensionality of tangent space = 3 */ - inline size_t dim() const { return dimension; } + /// identity for group operation + inline static Pose2 identity() { + return Pose2(); + } - /** - * inverse transformation with derivatives - */ - Pose2 inverse(boost::optional H1=boost::none) const; + /// inverse transformation with derivatives + Pose2 inverse(boost::optional H1=boost::none) const; - /** - * compose this transformation onto another (first *this and then p2) - * With optional derivatives - */ - Pose2 compose(const Pose2& p2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + /// compose this transformation onto another (first *this and then p2) + Pose2 compose(const Pose2& p2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const; - /// MATLAB version returns shared pointer - boost::shared_ptr compose_(const Pose2& p2) { - return boost::shared_ptr(new Pose2(compose(p2))); - } + /// MATLAB version returns shared pointer + boost::shared_ptr compose_(const Pose2& p2) { + return boost::shared_ptr(new Pose2(compose(p2))); + } - /** syntactic sugar for transform_from */ - inline Point2 operator*(const Point2& point) const { return transform_from(point);} + /// compose syntactic sugar + inline Pose2 operator*(const Pose2& p2) const { + return Pose2(r_*p2.r(), t_ + r_*p2.t()); + } - /** - * Exponential map from se(2) to SE(2) - */ - static Pose2 Expmap(const Vector& v); + /// @} + /// @name Manifold + /// @{ - /** - * Inverse of exponential map, from SE(2) to se(2) - */ - static Vector Logmap(const Pose2& p); + /// Dimensionality of tangent space = 3 DOF - used to autodetect sizes + inline static size_t Dim() { return dimension; } - /** non-approximated versions of Expmap/Logmap */ - static Pose2 ExpmapFull(const Vector& xi); - static Vector LogmapFull(const Pose2& p); - /** default implementations of binary functions */ - inline Pose2 expmap(const Vector& v) const { return gtsam::expmap_default(*this, v); } - inline Vector logmap(const Pose2& p2) const { return gtsam::logmap_default(*this, p2);} + /// Dimensionality of tangent space = 3 DOF + inline size_t dim() const { return dimension; } - /** non-approximated versions of expmap/logmap */ - inline Pose2 expmapFull(const Vector& v) const { return compose(ExpmapFull(v)); } - inline Vector logmapFull(const Pose2& p2) const { return LogmapFull(between(p2));} + /// Retraction from R^3 to Pose2 manifold neighborhood around current pose + Pose2 retract(const Vector& v) const; - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ - Pose2 between(const Pose2& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + /// Local 3D coordinates of Pose2 manifold neighborhood around current pose + Vector localCoordinates(const Pose2& p2) const; - /// MATLAB version returns shared pointer - boost::shared_ptr between_(const Pose2& p2) { - return boost::shared_ptr(new Pose2(between(p2))); - } + /// @} + /// @name Lie Group + /// @{ - /** return transformation matrix */ - Matrix matrix() const; + /// Exponential map from Lie algebra se(2) to SE(2) + static Pose2 Expmap(const Vector& xi); - /** - * Return point coordinates in pose coordinate frame - */ - Point2 transform_to(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + /// Exponential map from SE(2) to Lie algebra se(2) + static Vector Logmap(const Pose2& p); - /** - * Return point coordinates in global frame - */ - Point2 transform_from(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + /// @} + + /** return transformation matrix */ + Matrix matrix() const; + + /** + * Return relative pose between p1 and p2, in p1 coordinate frame + */ + Pose2 between(const Pose2& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; + + /// MATLAB version returns shared pointer + boost::shared_ptr between_(const Pose2& p2) { + return boost::shared_ptr(new Pose2(between(p2))); + } + + /** + * Return point coordinates in pose coordinate frame + */ + Point2 transform_to(const Point2& point, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; + + /** + * Return point coordinates in global frame + */ + Point2 transform_from(const Point2& point, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; + + /** syntactic sugar for transform_from */ + inline Point2 operator*(const Point2& point) const { return transform_from(point);} /** * Calculate bearing to a landmark @@ -212,7 +219,7 @@ namespace gtsam { */ Matrix AdjointMap() const; inline Vector Adjoint(const Vector& xi) const { - assert(xi.size() == 3); + assert(xi.size() == 3); return AdjointMap()*xi; } @@ -230,59 +237,41 @@ namespace gtsam { 0., 0., 0.); } - /** get functions for x, y, theta */ - inline double x() const { return t_.x(); } - inline double y() const { return t_.y(); } - inline double theta() const { return r_.theta(); } + /** get functions for x, y, theta */ + inline double x() const { return t_.x(); } + inline double y() const { return t_.y(); } + inline double theta() const { return r_.theta(); } - /** shorthand access functions */ - inline const Point2& t() const { return t_; } - inline const Rot2& r() const { return r_; } + /** shorthand access functions */ + inline const Point2& t() const { return t_; } + inline const Rot2& r() const { return r_; } - /** full access functions required by Pose concept */ - inline const Point2& translation() const { return t_; } - inline const Rot2& rotation() const { return r_; } + /** full access functions required by Pose concept */ + inline const Point2& translation() const { return t_; } + inline const Rot2& rotation() const { return r_; } - private: - // Serialization function - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { +private: + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(t_); ar & BOOST_SERIALIZATION_NVP(r_); - } - }; // Pose2 + } +}; // Pose2 - /** specialization for pose2 wedge function (generic template in Lie.h) */ - template <> - inline Matrix wedge(const Vector& xi) { - return Pose2::wedge(xi(0),xi(1),xi(2)); - } +/** specialization for pose2 wedge function (generic template in Lie.h) */ +template <> +inline Matrix wedge(const Vector& xi) { + return Pose2::wedge(xi(0),xi(1),xi(2)); +} - /** - * Calculate pose between a vector of 2D point correspondences (p,q) - * where q = Pose2::transform_from(p) = t + R*p - */ - typedef std::pair Point2Pair; - boost::optional align(const std::vector& pairs); - - /** - * Specializations for access to full expmap/logmap in templated functions - * - * NOTE: apparently, these *must* be indicated as inline to prevent compile error - */ - - /** unary versions */ - template<> - inline Pose2 ExpmapFull(const Vector& xi) { return Pose2::ExpmapFull(xi); } - template<> - inline Vector LogmapFull(const Pose2& p) { return Pose2::LogmapFull(p); } - - /** binary versions */ - template<> - inline Pose2 expmapFull(const Pose2& t, const Vector& v) { return t.expmapFull(v); } - template<> - inline Vector logmapFull(const Pose2& t, const Pose2& p2) { return t.logmapFull(p2); } +/** + * Calculate pose between a vector of 2D point correspondences (p,q) + * where q = Pose2::transform_from(p) = t + R*p + */ +typedef std::pair Point2Pair; +boost::optional align(const std::vector& pairs); } // namespace gtsam diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index b1b00582d..b7eef0a2f 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -60,7 +60,7 @@ namespace gtsam { /* ************************************************************************* */ /** Modified from Murray94book version (which assumes w and v normalized?) */ - Pose3 Pose3::ExpmapFull(const Vector& xi) { + Pose3 Pose3::Expmap(const Vector& xi) { // get angular velocity omega and translational velocity v from twist xi Point3 w(xi(0),xi(1),xi(2)), v(xi(3),xi(4),xi(5)); @@ -81,7 +81,7 @@ namespace gtsam { } /* ************************************************************************* */ - Vector Pose3::LogmapFull(const Pose3& p) { + Vector Pose3::Logmap(const Pose3& p) { Vector w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); double t = w.norm(); if (t < 1e-10) @@ -95,59 +95,59 @@ namespace gtsam { } #ifdef CORRECT_POSE3_EXMAP +// /* ************************************************************************* */ +// // Changes default to use the full verions of expmap/logmap +// /* ************************************************************************* */ +// Pose3 Retract(const Vector& xi) { +// return Pose3::Expmap(xi); +// } +// +// /* ************************************************************************* */ +// Vector Unretract(const Pose3& p) { +// return Pose3::Logmap(p); +// } + /* ************************************************************************* */ - // Changes default to use the full verions of expmap/logmap - /* ************************************************************************* */ - Pose3 Expmap(const Vector& xi) { - return Pose3::ExpmapFull(xi); + Pose3 retract(const Vector& d) { + return retract(d); } /* ************************************************************************* */ - Vector Logmap(const Pose3& p) { - return Pose3::LogmapFull(p); - } - - /* ************************************************************************* */ - Pose3 expmap(const Vector& d) { - return expmapFull(d); - } - - /* ************************************************************************* */ - Vector logmap(const Pose3& T1, const Pose3& T2) { - return logmapFull(T2); + Vector localCoordinates(const Pose3& T1, const Pose3& T2) { + return localCoordinates(T2); } #else - /* ************************************************************************* */ - /* incorrect versions for which we know how to compute derivatives */ - Pose3 Pose3::Expmap(const Vector& d) { - Vector w = sub(d, 0,3); - Vector u = sub(d, 3,6); - return Pose3(Rot3::Expmap(w), Point3::Expmap(u)); - } - - /* ************************************************************************* */ - // Log map at identity - return the translation and canonical rotation - // coordinates of a pose. - Vector Pose3::Logmap(const Pose3& p) { - const Vector w = Rot3::Logmap(p.rotation()), u = Point3::Logmap(p.translation()); - return concatVectors(2, &w, &u); - } +// /* ************************************************************************* */ +// /* incorrect versions for which we know how to compute derivatives */ +// Pose3 Pose3::Retract(const Vector& d) { +// Vector w = sub(d, 0,3); +// Vector u = sub(d, 3,6); +// return Pose3(Rot3::Retract(w), Point3::Retract(u)); +// } +// +// /* ************************************************************************* */ +// // Log map at identity - return the translation and canonical rotation +// // coordinates of a pose. +// Vector Pose3::Unretract(const Pose3& p) { +// const Vector w = Rot3::Unretract(p.rotation()), u = Point3::Unretract(p.translation()); +// return concatVectors(2, &w, &u); +// } /** These are the "old-style" expmap and logmap about the specified * pose. Increments the offset and rotation independently given a translation and * canonical rotation coordinates. Created to match ML derivatives, but * superseded by the correct exponential map story in .cpp */ - Pose3 Pose3::expmap(const Vector& d) const { - return Pose3(R_.expmap(sub(d, 0, 3)), - t_.expmap(sub(d, 3, 6))); + Pose3 Pose3::retract(const Vector& d) const { + return Pose3(R_.retract(sub(d, 0, 3)), + t_.retract(sub(d, 3, 6))); } /** Independently computes the logmap of the translation and rotation. */ - Vector Pose3::logmap(const Pose3& pp) const { - const Vector r(R_.logmap(pp.rotation())), - t(t_.logmap(pp.translation())); + Vector Pose3::localCoordinates(const Pose3& pp) const { + const Vector r(R_.localCoordinates(pp.rotation())), + t(t_.localCoordinates(pp.translation())); return concatVectors(2, &r, &t); } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 8b8659585..422cf4357 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -23,149 +23,142 @@ namespace gtsam { - /** - * A 3D pose (R,t) : (Rot3,Point3) - * @ingroup geometry - */ - class Pose3 : public Lie { - public: - static const size_t dimension = 6; +/** + * A 3D pose (R,t) : (Rot3,Point3) + * @ingroup geometry + */ +class Pose3 { +public: + static const size_t dimension = 6; - /** Pose Concept requirements */ - typedef Rot3 Rotation; - typedef Point3 Translation; + /** Pose Concept requirements */ + typedef Rot3 Rotation; + typedef Point3 Translation; - private: - Rot3 R_; - Point3 t_; +private: + Rot3 R_; + Point3 t_; - public: +public: - /** Default constructor is origin */ - Pose3() {} + /** Default constructor is origin */ + Pose3() {} - /** Copy constructor */ - Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {} + /** Copy constructor */ + Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {} - /** Construct from R,t */ - Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {} + /** Construct from R,t */ + Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {} - /** Constructor from 4*4 matrix */ - Pose3(const Matrix &T) : - R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), - T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {} + /** Constructor from 4*4 matrix */ + Pose3(const Matrix &T) : + R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), + T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {} - /** Constructor from 12D vector */ - Pose3(const Vector &V) : - R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)), - t_(V(9), V(10),V(11)) {} + /** Constructor from 12D vector */ + Pose3(const Vector &V) : + R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)), + t_(V(9), V(10),V(11)) {} - inline const Rot3& rotation() const { return R_; } - inline const Point3& translation() const { return t_; } + inline const Rot3& rotation() const { return R_; } + inline const Point3& translation() const { return t_; } - inline double x() const { return t_.x(); } - inline double y() const { return t_.y(); } - inline double z() const { return t_.z(); } + inline double x() const { return t_.x(); } + inline double y() const { return t_.y(); } + inline double z() const { return t_.z(); } - /** convert to 4*4 matrix */ - Matrix matrix() const; + /** convert to 4*4 matrix */ + Matrix matrix() const; - /** print with optional string */ - void print(const std::string& s = "") const; + /** print with optional string */ + void print(const std::string& s = "") const; - /** assert equality up to a tolerance */ - bool equals(const Pose3& pose, double tol = 1e-9) const; + /** assert equality up to a tolerance */ + bool equals(const Pose3& pose, double tol = 1e-9) const; - /** Compose two poses */ - inline Pose3 operator*(const Pose3& T) const { - return Pose3(R_*T.R_, t_ + R_*T.t_); - } + /** Compose two poses */ + inline Pose3 operator*(const Pose3& T) const { + return Pose3(R_*T.R_, t_ + R_*T.t_); + } - Pose3 transform_to(const Pose3& pose) const; + Pose3 transform_to(const Pose3& pose) const; - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /** dimension of the variable - used to autodetect sizes */ + inline static size_t Dim() { return dimension; } - /** Lie requirements */ + /** Lie requirements */ - /** Dimensionality of the tangent space */ - inline size_t dim() const { return dimension; } + /** Dimensionality of the tangent space */ + inline size_t dim() const { return dimension; } - /** - * Derivative of inverse - */ - Pose3 inverse(boost::optional H1=boost::none) const; + /** identity */ + inline static Pose3 identity() { + return Pose3(); + } - /** - * composes two poses (first (*this) then p2) - * with optional derivatives - */ - Pose3 compose(const Pose3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + /** + * Derivative of inverse + */ + Pose3 inverse(boost::optional H1=boost::none) const; - /** receives the point in Pose coordinates and transforms it to world coordinates */ - Point3 transform_from(const Point3& p, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /** + * composes two poses (first (*this) then p2) + * with optional derivatives + */ + Pose3 compose(const Pose3& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; - /** syntactic sugar for transform */ - inline Point3 operator*(const Point3& p) const { return transform_from(p); } + /** receives the point in Pose coordinates and transforms it to world coordinates */ + Point3 transform_from(const Point3& p, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** receives the point in world coordinates and transforms it to Pose coordinates */ - Point3 transform_to(const Point3& p, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /** syntactic sugar for transform */ + inline Point3 operator*(const Point3& p) const { return transform_from(p); } - /** Exponential map at identity - create a pose with a translation and - * rotation (in canonical coordinates). */ - static Pose3 Expmap(const Vector& v); + /** receives the point in world coordinates and transforms it to Pose coordinates */ + Point3 transform_to(const Point3& p, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** Log map at identity - return the translation and canonical rotation - * coordinates of a pose. */ - static Vector Logmap(const Pose3& p); + /** Exponential map around another pose */ + Pose3 retract(const Vector& d) const; - /** Exponential map around another pose */ - Pose3 expmap(const Vector& d) const; + /** Logarithm map around another pose T1 */ + Vector localCoordinates(const Pose3& T2) const; - /** Logarithm map around another pose T1 */ - Vector logmap(const Pose3& T2) const; + /** non-approximated versions of Expmap/Logmap */ + static Pose3 Expmap(const Vector& xi); + static Vector Logmap(const Pose3& p); - /** non-approximated versions of Expmap/Logmap */ - static Pose3 ExpmapFull(const Vector& xi); - static Vector LogmapFull(const Pose3& p); + /** + * Return relative pose between p1 and p2, in p1 coordinate frame + * as well as optionally the derivatives + */ + Pose3 between(const Pose3& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; - /** non-approximated versions of expmap/logmap */ - inline Pose3 expmapFull(const Vector& v) const { return compose(Pose3::ExpmapFull(v)); } - inline Vector logmapFull(const Pose3& p2) const { return Pose3::LogmapFull(between(p2));} + /** + * Calculate Adjoint map + * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) + */ + Matrix AdjointMap() const; + inline Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; } - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - * as well as optionally the derivatives - */ - Pose3 between(const Pose3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; - - /** - * Calculate Adjoint map - * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) - */ - Matrix AdjointMap() const; - inline Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; } - - /** - * wedge for Pose3: - * @param xi 6-dim twist (omega,v) where - * omega = (wx,wy,wz) 3D angular velocity - * v (vx,vy,vz) = 3D velocity - * @return xihat, 4*4 element of Lie algebra that can be exponentiated - */ - static inline Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) { - return Matrix_(4,4, - 0.,-wz, wy, vx, - wz, 0.,-wx, vy, - -wy, wx, 0., vz, - 0., 0., 0., 0.); - } + /** + * wedge for Pose3: + * @param xi 6-dim twist (omega,v) where + * omega = (wx,wy,wz) 3D angular velocity + * v (vx,vy,vz) = 3D velocity + * @return xihat, 4*4 element of Lie algebra that can be exponentiated + */ + static inline Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) { + return Matrix_(4,4, + 0.,-wz, wy, vx, + wz, 0.,-wx, vy, + -wy, wx, 0., vz, + 0., 0., 0., 0.); + } /** * Calculate range to a landmark @@ -185,44 +178,26 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const; - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(R_); - ar & BOOST_SERIALIZATION_NVP(t_); - } - }; // Pose3 class +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(R_); + ar & BOOST_SERIALIZATION_NVP(t_); + } +}; // Pose3 class - /** - * wedge for Pose3: - * @param xi 6-dim twist (omega,v) where - * omega = 3D angular velocity - * v = 3D velocity - * @return xihat, 4*4 element of Lie algebra that can be exponentiated - */ - template <> - inline Matrix wedge(const Vector& xi) { - return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5)); - } - - /** - * Specializations for access to full expmap/logmap in templated functions - * - * NOTE: apparently, these *must* be indicated as inline to prevent compile error - */ - - /** unary versions */ - template<> - inline Pose3 ExpmapFull(const Vector& xi) { return Pose3::ExpmapFull(xi); } - template<> - inline Vector LogmapFull(const Pose3& p) { return Pose3::LogmapFull(p); } - - /** binary versions */ - template<> - inline Pose3 expmapFull(const Pose3& t, const Vector& v) { return t.expmapFull(v); } - template<> - inline Vector logmapFull(const Pose3& t, const Pose3& p2) { return t.logmapFull(p2); } +/** + * wedge for Pose3: + * @param xi 6-dim twist (omega,v) where + * omega = 3D angular velocity + * v = 3D velocity + * @return xihat, 4*4 element of Lie algebra that can be exponentiated + */ +template <> +inline Matrix wedge(const Vector& xi) { + return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5)); +} } // namespace gtsam diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index b2ceafca0..3a8fb86f0 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -28,7 +28,7 @@ namespace gtsam { * NOTE: the angle theta is in radians unless explicitly stated * @ingroup geometry */ - class Rot2: public Lie { + class Rot2 { public: /** get the dimension by the type */ @@ -127,6 +127,11 @@ namespace gtsam { return dimension; } + /** identity */ + inline static Rot2 identity() { + return Rot2(); + } + /** Compose - make a new rotation by adding angles */ inline Rot2 compose(const Rot2& R1, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { @@ -148,15 +153,14 @@ namespace gtsam { return Vector_(1, r.theta()); } - /** Binary expmap */ - inline Rot2 expmap(const Vector& v) const { - return *this * Expmap(v); - } + // Manifold requirements - /** Binary Logmap */ - inline Vector logmap(const Rot2& p2) const { - return Logmap(between(p2)); - } + inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); } + + /** + * Returns inverse retraction + */ + inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } /** Between using the default implementation */ inline Rot2 between(const Rot2& p2, boost::optional H1 = diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index a37c58d86..82a5ec39e 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -28,245 +28,256 @@ namespace gtsam { typedef Eigen::Quaterniond Quaternion; - /** - * @brief 3D Rotations represented as rotation matrices - * @ingroup geometry - */ - class Rot3: public Lie { - public: - static const size_t dimension = 3; +/** + * @brief 3D Rotations represented as rotation matrices + * @ingroup geometry + */ +class Rot3 { +public: + static const size_t dimension = 3; - private: - /** we store columns ! */ - Point3 r1_, r2_, r3_; +private: + /** we store columns ! */ + Point3 r1_, r2_, r3_; - public: +public: - /** default constructor, unit rotation */ - Rot3() : - r1_(Point3(1.0,0.0,0.0)), - r2_(Point3(0.0,1.0,0.0)), - r3_(Point3(0.0,0.0,1.0)) {} + /** default constructor, unit rotation */ + Rot3() : + r1_(Point3(1.0,0.0,0.0)), + r2_(Point3(0.0,1.0,0.0)), + r3_(Point3(0.0,0.0,1.0)) {} - /** constructor from columns */ - Rot3(const Point3& r1, const Point3& r2, const Point3& r3) : - r1_(r1), r2_(r2), r3_(r3) {} + /** constructor from columns */ + Rot3(const Point3& r1, const Point3& r2, const Point3& r3) : + r1_(r1), r2_(r2), r3_(r3) {} - /** constructor from doubles in *row* order !!! */ - Rot3(double R11, double R12, double R13, - double R21, double R22, double R23, - double R31, double R32, double R33) : - r1_(Point3(R11, R21, R31)), - r2_(Point3(R12, R22, R32)), - r3_(Point3(R13, R23, R33)) {} + /** constructor from doubles in *row* order !!! */ + Rot3(double R11, double R12, double R13, + double R21, double R22, double R23, + double R31, double R32, double R33) : + r1_(Point3(R11, R21, R31)), + r2_(Point3(R12, R22, R32)), + r3_(Point3(R13, R23, R33)) {} - /** constructor from matrix */ - Rot3(const Matrix& R): - r1_(Point3(R(0,0), R(1,0), R(2,0))), - r2_(Point3(R(0,1), R(1,1), R(2,1))), - r3_(Point3(R(0,2), R(1,2), R(2,2))) {} + /** constructor from matrix */ + Rot3(const Matrix& R): + r1_(Point3(R(0,0), R(1,0), R(2,0))), + r2_(Point3(R(0,1), R(1,1), R(2,1))), + r3_(Point3(R(0,2), R(1,2), R(2,2))) {} - /** Constructor from a quaternion. This can also be called using a plain - * Vector, due to implicit conversion from Vector to Quaternion - * @param q The quaternion - */ - Rot3(const Quaternion& q) { - Eigen::Matrix3d R = q.toRotationMatrix(); - r1_ = Point3(R.col(0)); - r2_ = Point3(R.col(1)); - r3_ = Point3(R.col(2)); - } + /** Constructor from a quaternion. This can also be called using a plain + * Vector, due to implicit conversion from Vector to Quaternion + * @param q The quaternion + */ + Rot3(const Quaternion& q) { + Eigen::Matrix3d R = q.toRotationMatrix(); + r1_ = Point3(R.col(0)); + r2_ = Point3(R.col(1)); + r3_ = Point3(R.col(2)); + } - /** Static member function to generate some well known rotations */ + /** Static member function to generate some well known rotations */ - /** - * Rotations around axes as in http://en.wikipedia.org/wiki/Rotation_matrix - * Counterclockwise when looking from unchanging axis. - */ - static Rot3 Rx(double t); - static Rot3 Ry(double t); - static Rot3 Rz(double t); - static Rot3 RzRyRx(double x, double y, double z); - inline static Rot3 RzRyRx(const Vector& xyz) { - assert(xyz.size() == 3); - return RzRyRx(xyz(0), xyz(1), xyz(2)); - } + /** + * Rotations around axes as in http://en.wikipedia.org/wiki/Rotation_matrix + * Counterclockwise when looking from unchanging axis. + */ + static Rot3 Rx(double t); + static Rot3 Ry(double t); + static Rot3 Rz(double t); + static Rot3 RzRyRx(double x, double y, double z); + inline static Rot3 RzRyRx(const Vector& xyz) { + assert(xyz.size() == 3); + return RzRyRx(xyz(0), xyz(1), xyz(2)); + } - /** - * Tait-Bryan system from Spatial Reference Model (SRM) (x,y,z) = (roll,pitch,yaw) - * as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf - * Assumes vehicle coordinate frame X forward, Y right, Z down - */ - static Rot3 yaw (double t) { return Rz(t);} // positive yaw is to right (as in aircraft heading) - static Rot3 pitch(double t) { return Ry(t);} // positive pitch is up (increasing aircraft altitude) - static Rot3 roll (double t) { return Rx(t);} // positive roll is to right (increasing yaw in aircraft) - static Rot3 ypr (double y, double p, double r) { return RzRyRx(r,p,y);} + /** + * Tait-Bryan system from Spatial Reference Model (SRM) (x,y,z) = (roll,pitch,yaw) + * as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf + * Assumes vehicle coordinate frame X forward, Y right, Z down + */ + static Rot3 yaw (double t) { return Rz(t);} // positive yaw is to right (as in aircraft heading) + static Rot3 pitch(double t) { return Ry(t);} // positive pitch is up (increasing aircraft altitude) + static Rot3 roll (double t) { return Rx(t);} // positive roll is to right (increasing yaw in aircraft) + static Rot3 ypr (double y, double p, double r) { return RzRyRx(r,p,y);} - /** Create from Quaternion parameters */ - static Rot3 quaternion(double w, double x, double y, double z) { Quaternion q(w, x, y, z); return Rot3(q); } + /** Create from Quaternion parameters */ + static Rot3 quaternion(double w, double x, double y, double z) { Quaternion q(w, x, y, z); return Rot3(q); } - /** - * Rodriguez' formula to compute an incremental rotation matrix - * @param w is the rotation axis, unit length - * @param theta rotation angle - * @return incremental rotation matrix - */ - static Rot3 rodriguez(const Vector& w, double theta); + /** + * Rodriguez' formula to compute an incremental rotation matrix + * @param w is the rotation axis, unit length + * @param theta rotation angle + * @return incremental rotation matrix + */ + static Rot3 rodriguez(const Vector& w, double theta); - /** - * Rodriguez' formula to compute an incremental rotation matrix - * @param v a vector of incremental roll,pitch,yaw - * @return incremental rotation matrix - */ - static Rot3 rodriguez(const Vector& v); + /** + * Rodriguez' formula to compute an incremental rotation matrix + * @param v a vector of incremental roll,pitch,yaw + * @return incremental rotation matrix + */ + static Rot3 rodriguez(const Vector& v); - /** - * Rodriguez' formula to compute an incremental rotation matrix - * @param wx - * @param wy - * @param wz - * @return incremental rotation matrix - */ - static inline Rot3 rodriguez(double wx, double wy, double wz) - { return rodriguez(Vector_(3,wx,wy,wz));} + /** + * Rodriguez' formula to compute an incremental rotation matrix + * @param wx + * @param wy + * @param wz + * @return incremental rotation matrix + */ + static inline Rot3 rodriguez(double wx, double wy, double wz) + { return rodriguez(Vector_(3,wx,wy,wz));} - /** print */ - void print(const std::string& s="R") const { gtsam::print(matrix(), s);} + /** print */ + void print(const std::string& s="R") const { gtsam::print(matrix(), s);} - /** equals with an tolerance */ - bool equals(const Rot3& p, double tol = 1e-9) const; + /** equals with an tolerance */ + bool equals(const Rot3& p, double tol = 1e-9) const; - /** return 3*3 rotation matrix */ - Matrix matrix() const; + /** return 3*3 rotation matrix */ + Matrix matrix() const; - /** return 3*3 transpose (inverse) rotation matrix */ - Matrix transpose() const; + /** return 3*3 transpose (inverse) rotation matrix */ + Matrix transpose() const; - /** returns column vector specified by index */ - Point3 column(int index) const; - Point3 r1() const { return r1_; } - Point3 r2() const { return r2_; } - Point3 r3() const { return r3_; } + /** returns column vector specified by index */ + Point3 column(int index) const; + Point3 r1() const { return r1_; } + Point3 r2() const { return r2_; } + Point3 r3() const { return r3_; } - /** - * Use RQ to calculate xyz angle representation - * @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z) - */ - Vector xyz() const; + /** + * Use RQ to calculate xyz angle representation + * @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z) + */ + Vector xyz() const; - /** - * Use RQ to calculate yaw-pitch-roll angle representation - * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) - */ - Vector ypr() const; + /** + * Use RQ to calculate yaw-pitch-roll angle representation + * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) + */ + Vector ypr() const; - /** - * Use RQ to calculate roll-pitch-yaw angle representation - * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) - */ - Vector rpy() const; + /** + * Use RQ to calculate roll-pitch-yaw angle representation + * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) + */ + Vector rpy() const; - /** Compute the quaternion representation of this rotation. - * @return The quaternion - */ - Quaternion toQuaternion() const { - return Quaternion((Eigen::Matrix3d() << - r1_.x(), r2_.x(), r3_.x(), - r1_.y(), r2_.y(), r3_.y(), - r1_.z(), r2_.z(), r3_.z()).finished()); - } + /** Compute the quaternion representation of this rotation. + * @return The quaternion + */ + Quaternion toQuaternion() const { + return Quaternion((Eigen::Matrix3d() << + r1_.x(), r2_.x(), r3_.x(), + r1_.y(), r2_.y(), r3_.y(), + r1_.z(), r2_.z(), r3_.z()).finished()); + } - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /** dimension of the variable - used to autodetect sizes */ + inline static size_t Dim() { return dimension; } - /** Lie requirements */ + /** Lie requirements */ - /** return DOF, dimensionality of tangent space */ - inline size_t dim() const { return dimension; } + /** return DOF, dimensionality of tangent space */ + inline size_t dim() const { return dimension; } - /** Compose two rotations i.e., R= (*this) * R2 - */ - Rot3 compose(const Rot3& R2, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /** Compose two rotations i.e., R= (*this) * R2 + */ + Rot3 compose(const Rot3& R2, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** Exponential map at identity - create a rotation from canonical coordinates - * using Rodriguez' formula - */ - static Rot3 Expmap(const Vector& v) { - if(zero(v)) return Rot3(); - else return rodriguez(v); - } + /** Exponential map at identity - create a rotation from canonical coordinates + * using Rodriguez' formula + */ + static Rot3 Expmap(const Vector& v) { + if(zero(v)) return Rot3(); + else return rodriguez(v); + } - // Log map at identity - return the canonical coordinates of this rotation - static Vector Logmap(const Rot3& R); + /** identity */ + inline static Rot3 identity() { + return Rot3(); + } - /** default implementations of binary functions */ - inline Rot3 expmap(const Vector& v) const { return gtsam::expmap_default(*this, v); } - inline Vector logmap(const Rot3& p2) const { return gtsam::logmap_default(*this, p2);} + // Log map at identity - return the canonical coordinates of this rotation + static Vector Logmap(const Rot3& R); - // derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3() - inline Rot3 inverse(boost::optional H1=boost::none) const { - if (H1) *H1 = -matrix(); - return Rot3( - r1_.x(), r1_.y(), r1_.z(), - r2_.x(), r2_.y(), r2_.z(), - r3_.x(), r3_.y(), r3_.z()); - } + // Manifold requirements - /** - * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' - */ - Rot3 between(const Rot3& R2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + inline Rot3 retract(const Vector& v) const { return compose(Expmap(v)); } - /** compose two rotations */ - Rot3 operator*(const Rot3& R2) const { - return Rot3(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_)); - } + /** + * Returns inverse retraction + */ + inline Vector localCoordinates(const Rot3& t2) const { return Logmap(between(t2)); } - /** - * rotate point from rotated coordinate frame to - * world = R*p - */ - inline Point3 operator*(const Point3& p) const { return rotate(p);} - /** - * rotate point from rotated coordinate frame to - * world = R*p - */ - Point3 rotate(const Point3& p, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + // derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3() + inline Rot3 inverse(boost::optional H1=boost::none) const { + if (H1) *H1 = -matrix(); + return Rot3( + r1_.x(), r1_.y(), r1_.z(), + r2_.x(), r2_.y(), r2_.z(), + r3_.x(), r3_.y(), r3_.z()); + } - /** - * rotate point from world to rotated - * frame = R'*p - */ - Point3 unrotate(const Point3& p, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /** + * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' + */ + Rot3 between(const Rot3& R2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) - { - ar & BOOST_SERIALIZATION_NVP(r1_); - ar & BOOST_SERIALIZATION_NVP(r2_); - ar & BOOST_SERIALIZATION_NVP(r3_); - } - }; + /** compose two rotations */ + Rot3 operator*(const Rot3& R2) const { + return Rot3(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_)); + } - /** - * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R - * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' - * such that A = R*Q = R*Qz'*Qy'*Qx'. When A is a rotation matrix, R will - * be the identity and Q is a yaw-pitch-roll decomposition of A. - * The implementation uses Givens rotations and is based on Hartley-Zisserman. - * @param a 3 by 3 matrix A=RQ - * @return an upper triangular matrix R - * @return a vector [thetax, thetay, thetaz] in radians. - */ - std::pair RQ(const Matrix& A); + /** + * rotate point from rotated coordinate frame to + * world = R*p + */ + inline Point3 operator*(const Point3& p) const { return rotate(p);} + + /** + * rotate point from rotated coordinate frame to + * world = R*p + */ + Point3 rotate(const Point3& p, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + + /** + * rotate point from world to rotated + * frame = R'*p + */ + Point3 unrotate(const Point3& p, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(r1_); + ar & BOOST_SERIALIZATION_NVP(r2_); + ar & BOOST_SERIALIZATION_NVP(r3_); + } +}; + +/** + * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R + * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' + * such that A = R*Q = R*Qz'*Qy'*Qx'. When A is a rotation matrix, R will + * be the identity and Q is a yaw-pitch-roll decomposition of A. + * The implementation uses Givens rotations and is based on Hartley-Zisserman. + * @param a 3 by 3 matrix A=RQ + * @return an upper triangular matrix R + * @return a vector [thetax, thetay, thetaz] in radians. + */ +std::pair RQ(const Matrix& A); } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index f64c404a6..cd3dbd19e 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -24,117 +24,116 @@ namespace gtsam { - /** - * A stereo camera class, parameterize by left camera pose and stereo calibration - * @ingroup geometry +/** + * A stereo camera class, parameterize by left camera pose and stereo calibration + * @ingroup geometry + */ +class StereoCamera { + +private: + Pose3 leftCamPose_; + Cal3_S2Stereo::shared_ptr K_; + +public: + StereoCamera() { + } + + StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K); + + const Cal3_S2Stereo::shared_ptr calibration() const { + return K_; + } + + const Pose3& pose() const { + return leftCamPose_; + } + + double baseline() const { + return K_->baseline(); + } + + /* + * project 3D point and compute optional derivatives */ - class StereoCamera { + StereoPoint2 project(const Point3& point, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const; - private: - Pose3 leftCamPose_; - Cal3_S2Stereo::shared_ptr K_; + /* + * to accomodate tsam's assumption that K is estimated, too + */ + StereoPoint2 project(const Point3& point, + boost::optional H1, + boost::optional H1_k, + boost::optional H2) const { + return project(point, H1, H2); + } - public: - StereoCamera() { - } + /* + * backproject using left camera calibration, up to scale only + * i.e. does not rely on baseline + */ + Point3 backproject(const Point2& projection, const double scale) const { + Point2 intrinsic = K_->calibrate(projection); + Point3 cameraPoint = Point3(intrinsic.x() * scale, intrinsic.y() * scale, scale);; + return pose().transform_from(cameraPoint); + } - StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K); + Point3 backproject(const StereoPoint2& z) const { + Vector measured = z.vector(); + double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]); + double X = Z *(measured[0]- K_->px()) / K_->fx(); + double Y = Z *(measured[2]- K_->py()) / K_->fy(); + Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z)); + return world_point; + } - const Cal3_S2Stereo::shared_ptr calibration() const { - return K_; - } + /** Dimensionality of the tangent space */ + inline size_t dim() const { + return 6; + } - const Pose3& pose() const { - return leftCamPose_; - } + /** Dimensionality of the tangent space */ + static inline size_t Dim() { + return 6; + } - double baseline() const { - return K_->baseline(); - } + bool equals(const StereoCamera &camera, double tol = 1e-9) const { + return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals( + *camera.K_, tol); + } - /* - * project 3D point and compute optional derivatives - */ - StereoPoint2 project(const Point3& point, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + // Manifold requirements - use existing expmap/logmap + inline StereoCamera retract(const Vector& v) const { + return StereoCamera(pose().retract(v), K_); + } - /* - * to accomodate tsam's assumption that K is estimated, too - */ - StereoPoint2 project(const Point3& point, - boost::optional H1, - boost::optional H1_k, - boost::optional H2) const { - return project(point, H1, H2); - } + inline Vector localCoordinates(const StereoCamera& t2) const { + return Vector(leftCamPose_.localCoordinates(t2.leftCamPose_)); + } - /* - * backproject using left camera calibration, up to scale only - * i.e. does not rely on baseline - */ - Point3 backproject(const Point2& projection, const double scale) const { - Point2 intrinsic = K_->calibrate(projection); - Point3 cameraPoint = Point3(intrinsic.x() * scale, intrinsic.y() * scale, scale);; - return pose().transform_from(cameraPoint); - } + Pose3 between(const StereoCamera &camera, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + return leftCamPose_.between(camera.pose(), H1, H2); + } - Point3 backproject(const StereoPoint2& z) const { - Vector measured = z.vector(); - double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]); - double X = Z *(measured[0]- K_->px()) / K_->fx(); - double Y = Z *(measured[2]- K_->py()) / K_->fy(); - Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z)); - return world_point; - } + void print(const std::string& s = "") const { + leftCamPose_.print(s + ".camera."); + K_->print(s + ".calibration."); + } - /** Dimensionality of the tangent space */ - inline size_t dim() const { - return 6; - } +private: + /** utility functions */ + Matrix Dproject_to_stereo_camera1(const Point3& P) const; - /** Dimensionality of the tangent space */ - static inline size_t Dim() { - return 6; - } + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(leftCamPose_); + ar & BOOST_SERIALIZATION_NVP(K_); + } - /** Exponential map around p0 */ - inline StereoCamera expmap(const Vector& d) const { - return StereoCamera(pose().expmap(d), K_); - } - - Vector logmap(const StereoCamera &camera) const { - const Vector v1(leftCamPose_.logmap(camera.leftCamPose_)); - return v1; - } - - bool equals(const StereoCamera &camera, double tol = 1e-9) const { - return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals( - *camera.K_, tol); - } - - Pose3 between(const StereoCamera &camera, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - return leftCamPose_.between(camera.pose(), H1, H2); - } - - void print(const std::string& s = "") const { - leftCamPose_.print(s + ".camera."); - K_->print(s + ".calibration."); - } - - private: - /** utility functions */ - Matrix Dproject_to_stereo_camera1(const Point3& P) const; - - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(leftCamPose_); - ar & BOOST_SERIALIZATION_NVP(K_); - } - - }; +}; } diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index d6ad8f352..e493b0047 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -26,7 +26,7 @@ namespace gtsam { * A 2D stereo point, v will be same for rectified images * @ingroup geometry */ - class StereoPoint2: Lie { + class StereoPoint2 { public: static const size_t dimension = 3; private: @@ -86,6 +86,11 @@ namespace gtsam { return *this + p1; } + /** identity */ + inline static StereoPoint2 identity() { + return StereoPoint2(); + } + /** inverse */ inline StereoPoint2 inverse() const { return StereoPoint2()- (*this); @@ -101,14 +106,14 @@ namespace gtsam { return p.vector(); } - /** default implementations of binary functions */ - inline StereoPoint2 expmap(const Vector& v) const { - return gtsam::expmap_default(*this, v); - } + // Manifold requirements - inline Vector logmap(const StereoPoint2& p2) const { - return gtsam::logmap_default(*this, p2); - } + inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); } + + /** + * Returns inverse retraction + */ + inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); } inline StereoPoint2 between(const StereoPoint2& p2) const { return gtsam::between_default(*this, p2); diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index ef00c6d00..bb1bd861d 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -22,6 +22,9 @@ using namespace gtsam; +GTSAM_CONCEPT_TESTABLE_INST(Cal3Bundler) +GTSAM_CONCEPT_MANIFOLD_INST(Cal3Bundler) + Cal3Bundler K(500, 1e-3, 1e-3); Point2 p(2,3); diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index 22bd9c8a1..4c79a447b 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -22,6 +22,9 @@ using namespace gtsam; +GTSAM_CONCEPT_TESTABLE_INST(Cal3DS2) +GTSAM_CONCEPT_MANIFOLD_INST(Cal3DS2) + Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); Point2 p(2,3); diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index c341f7b54..fd82557a3 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -22,6 +22,7 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Cal3_S2) +GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2) Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2); Point2 p(1, -2); diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 080a23a81..4be47a4c0 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -25,6 +25,8 @@ using namespace std; using namespace gtsam; +GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera) + const Pose3 pose1(Matrix_(3,3, 1., 0., 0., 0.,-1., 0., diff --git a/gtsam/geometry/tests/testHomography2.cpp b/gtsam/geometry/tests/testHomography2.cpp index 639462955..aa5aa4515 100644 --- a/gtsam/geometry/tests/testHomography2.cpp +++ b/gtsam/geometry/tests/testHomography2.cpp @@ -124,7 +124,7 @@ namespace gtsam { Index<3, 'C'> I; // contravariant 2D camera return toVector(H(I,_T)); } - Vector logmap(const tensors::Tensor2<3, 3>& A, const tensors::Tensor2<3, 3>& B) { + Vector localCoordinates(const tensors::Tensor2<3, 3>& A, const tensors::Tensor2<3, 3>& B) { return toVector(A)-toVector(B); // TODO correct order ? } } diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 2aac1703b..ec83c2ac7 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -22,6 +22,9 @@ using namespace std; using namespace gtsam; +GTSAM_CONCEPT_TESTABLE_INST(Point2) +GTSAM_CONCEPT_LIE_INST(Point2) + /* ************************************************************************* */ TEST(Point2, Lie) { Point2 p1(1,2); @@ -36,8 +39,8 @@ TEST(Point2, Lie) { EXPECT(assert_equal(-eye(2), H1)); EXPECT(assert_equal(eye(2), H2)); - EXPECT(assert_equal(Point2(5,7), p1.expmap(Vector_(2, 4.,5.)))); - EXPECT(assert_equal(Vector_(2, 3.,3.), p1.logmap(p2))); + EXPECT(assert_equal(Point2(5,7), p1.retract(Vector_(2, 4.,5.)))); + EXPECT(assert_equal(Vector_(2, 3.,3.), p1.localCoordinates(p2))); } /* ************************************************************************* */ @@ -46,7 +49,7 @@ TEST( Point2, expmap) Vector d(2); d(0) = 1; d(1) = -1; - Point2 a(4, 5), b = a.expmap(d), c(5, 4); + Point2 a(4, 5), b = a.retract(d), c(5, 4); EXPECT(assert_equal(b,c)); } diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 5f2aa32b0..c7e25f7a9 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -20,6 +20,9 @@ using namespace gtsam; +GTSAM_CONCEPT_TESTABLE_INST(Point3) +GTSAM_CONCEPT_LIE_INST(Point3) + Point3 P(0.2,0.7,-2); /* ************************************************************************* */ @@ -36,8 +39,8 @@ TEST(Point3, Lie) { EXPECT(assert_equal(-eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(5,7,9), p1.expmap(Vector_(3, 4.,5.,6.)))); - EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.logmap(p2))); + EXPECT(assert_equal(Point3(5,7,9), p1.retract(Vector_(3, 4.,5.,6.)))); + EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.localCoordinates(p2))); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 104d47e31..0763cfba5 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -35,11 +35,8 @@ using namespace std; // #define SLOW_BUT_CORRECT_EXPMAP -// concept checks for testable GTSAM_CONCEPT_TESTABLE_INST(Pose2) -GTSAM_CONCEPT_TESTABLE_INST(Point2) -GTSAM_CONCEPT_TESTABLE_INST(Rot2) -GTSAM_CONCEPT_TESTABLE_INST(LieVector) +GTSAM_CONCEPT_LIE_INST(Pose2) /* ************************************************************************* */ TEST(Pose2, constructors) { @@ -56,44 +53,44 @@ TEST(Pose2, manifold) { Pose2 t1(M_PI_2, Point2(1, 2)); Pose2 t2(M_PI_2+0.018, Point2(1.015, 2.01)); Pose2 origin; - Vector d12 = t1.logmap(t2); - EXPECT(assert_equal(t2, t1.expmap(d12))); - EXPECT(assert_equal(t2, t1*origin.expmap(d12))); - Vector d21 = t2.logmap(t1); - EXPECT(assert_equal(t1, t2.expmap(d21))); - EXPECT(assert_equal(t1, t2*origin.expmap(d21))); + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); + EXPECT(assert_equal(t2, t1*origin.retract(d12))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); + EXPECT(assert_equal(t1, t2*origin.retract(d21))); } /* ************************************************************************* */ -TEST(Pose2, expmap) { +TEST(Pose2, retract) { Pose2 pose(M_PI_2, Point2(1, 2)); #ifdef SLOW_BUT_CORRECT_EXPMAP Pose2 expected(1.00811, 2.01528, 2.5608); #else Pose2 expected(M_PI_2+0.99, Point2(1.015, 2.01)); #endif - Pose2 actual = pose.expmap(Vector_(3, 0.01, -0.015, 0.99)); + Pose2 actual = pose.retract(Vector_(3, 0.01, -0.015, 0.99)); EXPECT(assert_equal(expected, actual, 1e-5)); } /* ************************************************************************* */ -TEST(Pose2, expmap_full) { +TEST(Pose2, expmap) { Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 expected(1.00811, 2.01528, 2.5608); - Pose2 actual = pose.expmapFull(Vector_(3, 0.01, -0.015, 0.99)); - EXPECT(assert_equal(expected, actual, 1e-5)); -} - -/* ************************************************************************* */ -TEST(Pose2, expmap_full2) { - Pose2 pose(M_PI_2, Point2(1, 2)); - Pose2 expected(1.00811, 2.01528, 2.5608); - Pose2 actual = expmapFull(pose, Vector_(3, 0.01, -0.015, 0.99)); + Pose2 actual = expmap_default(pose, Vector_(3, 0.01, -0.015, 0.99)); EXPECT(assert_equal(expected, actual, 1e-5)); } /* ************************************************************************* */ TEST(Pose2, expmap2) { + Pose2 pose(M_PI_2, Point2(1, 2)); + Pose2 expected(1.00811, 2.01528, 2.5608); + Pose2 actual = expmap_default(pose, Vector_(3, 0.01, -0.015, 0.99)); + EXPECT(assert_equal(expected, actual, 1e-5)); +} + +/* ************************************************************************* */ +TEST(Pose2, expmap3) { // do an actual series exponential map // see e.g. http://www.cis.upenn.edu/~cis610/cis610lie1.ps Matrix A = Matrix_(3,3, @@ -114,12 +111,12 @@ TEST(Pose2, expmap2) { /* ************************************************************************* */ TEST(Pose2, expmap0) { Pose2 pose(M_PI_2, Point2(1, 2)); -#ifdef SLOW_BUT_CORRECT_EXPMAP +//#ifdef SLOW_BUT_CORRECT_EXPMAP Pose2 expected(1.01491, 2.01013, 1.5888); -#else - Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01)); -#endif - Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)); +//#else +// Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01)); +//#endif + Pose2 actual = pose * (Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018))); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -127,7 +124,7 @@ TEST(Pose2, expmap0) { TEST(Pose2, expmap0_full) { Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 expected(1.01491, 2.01013, 1.5888); - Pose2 actual = pose * Pose2::ExpmapFull(Vector_(3, 0.01, -0.015, 0.018)); + Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -135,7 +132,7 @@ TEST(Pose2, expmap0_full) { TEST(Pose2, expmap0_full2) { Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 expected(1.01491, 2.01013, 1.5888); - Pose2 actual = pose * ExpmapFull(Vector_(3, 0.01, -0.015, 0.018)); + Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -153,8 +150,8 @@ namespace screw { TEST(Pose3, expmap_c) { EXPECT(assert_equal(screw::expected, expm(screw::xi),1e-6)); - EXPECT(assert_equal(screw::expected, Pose2::Expmap(screw::xi),1e-6)); - EXPECT(assert_equal(screw::xi, Pose2::Logmap(screw::expected),1e-6)); + EXPECT(assert_equal(screw::expected, Pose2::Retract(screw::xi),1e-6)); + EXPECT(assert_equal(screw::xi, Pose2::Unretract(screw::expected),1e-6)); } #endif @@ -167,8 +164,8 @@ TEST(Pose3, expmap_c_full) Point2 expectedT(-0.0446635, 0.29552); Pose2 expected(expectedR, expectedT); EXPECT(assert_equal(expected, expm(xi),1e-6)); - EXPECT(assert_equal(expected, Pose2::ExpmapFull(xi),1e-6)); - EXPECT(assert_equal(xi, Pose2::LogmapFull(expected),1e-6)); + EXPECT(assert_equal(expected, Pose2::Expmap(xi),1e-6)); + EXPECT(assert_equal(xi, Pose2::Logmap(expected),1e-6)); } /* ************************************************************************* */ @@ -180,7 +177,7 @@ TEST(Pose2, logmap) { #else Vector expected = Vector_(3, 0.01, -0.015, 0.018); #endif - Vector actual = pose0.logmap(pose); + Vector actual = pose0.localCoordinates(pose); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -189,7 +186,7 @@ TEST(Pose2, logmap_full) { Pose2 pose0(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01)); Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018); - Vector actual = pose0.logmapFull(pose); + Vector actual = logmap_default(pose0, pose); EXPECT(assert_equal(expected, actual, 1e-5)); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index e0f62bfa3..bbe69bc9e 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -24,6 +24,9 @@ using namespace std; using namespace gtsam; +GTSAM_CONCEPT_TESTABLE_INST(Pose3) +GTSAM_CONCEPT_LIE_INST(Pose3) + static Point3 P(0.2,0.7,-2); static Rot3 R = Rot3::rodriguez(0.3,0,0); static Pose3 T(R,Point3(3.5,-8.2,4.2)); @@ -35,9 +38,9 @@ const double tol=1e-5; TEST( Pose3, equals) { Pose3 pose2 = T3; - CHECK(T3.equals(pose2)); + EXPECT(T3.equals(pose2)); Pose3 origin; - CHECK(!T3.equals(origin)); + EXPECT(!T3.equals(origin)); } /* ************************************************************************* */ @@ -46,14 +49,14 @@ TEST( Pose3, expmap_a) Pose3 id; Vector v = zero(6); v(0) = 0.3; - CHECK(assert_equal(id.expmap(v), Pose3(R, Point3()))); + EXPECT(assert_equal(id.retract(v), Pose3(R, Point3()))); #ifdef CORRECT_POSE3_EXMAP v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; #else v(3)=0.2;v(4)=0.7;v(5)=-2; #endif - CHECK(assert_equal(Pose3(R, P),id.expmap(v),1e-5)); + EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-5)); } /* ************************************************************************* */ @@ -62,9 +65,9 @@ TEST( Pose3, expmap_a_full) Pose3 id; Vector v = zero(6); v(0) = 0.3; - CHECK(assert_equal(id.expmapFull(v), Pose3(R, Point3()))); + EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3()))); v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; - CHECK(assert_equal(Pose3(R, P),id.expmapFull(v),1e-5)); + EXPECT(assert_equal(Pose3(R, P),expmap_default(id, v),1e-5)); } /* ************************************************************************* */ @@ -73,18 +76,18 @@ TEST( Pose3, expmap_a_full2) Pose3 id; Vector v = zero(6); v(0) = 0.3; - CHECK(assert_equal(expmapFull(id,v), Pose3(R, Point3()))); + EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3()))); v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; - CHECK(assert_equal(Pose3(R, P),expmapFull(id,v),1e-5)); + EXPECT(assert_equal(Pose3(R, P),expmap_default(id, v),1e-5)); } /* ************************************************************************* */ TEST(Pose3, expmap_b) { Pose3 p1(Rot3(), Point3(100, 0, 0)); - Pose3 p2 = p1.expmap(Vector_(6,0.0, 0.0, 0.1, 0.0, 0.0, 0.0)); + Pose3 p2 = p1.retract(Vector_(6,0.0, 0.0, 0.1, 0.0, 0.0, 0.0)); Pose3 expected(Rot3::rodriguez(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0)); - CHECK(assert_equal(expected, p2)); + EXPECT(assert_equal(expected, p2)); } /* ************************************************************************* */ @@ -101,25 +104,25 @@ namespace screw { /* ************************************************************************* */ TEST(Pose3, expmap_c) { - CHECK(assert_equal(screw::expected, expm(screw::xi),1e-6)); - CHECK(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6)); + EXPECT(assert_equal(screw::expected, expm(screw::xi),1e-6)); + EXPECT(assert_equal(screw::expected, Pose3::Retract(screw::xi),1e-6)); } /* ************************************************************************* */ // assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) TEST(Pose3, Adjoint) { - Pose3 expected = T * Pose3::Expmap(screw::xi) * inverse(T); + Pose3 expected = T * Pose3::Retract(screw::xi) * inverse(T); Vector xiprime = Adjoint(T, screw::xi); - CHECK(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6)); + EXPECT(assert_equal(expected, Pose3::Retract(xiprime), 1e-6)); - Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * inverse(T2); + Pose3 expected2 = T2 * Pose3::Retract(screw::xi) * inverse(T2); Vector xiprime2 = Adjoint(T2, screw::xi); - CHECK(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6)); + EXPECT(assert_equal(expected2, Pose3::Retract(xiprime2), 1e-6)); - Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * inverse(T3); + Pose3 expected3 = T3 * Pose3::Retract(screw::xi) * inverse(T3); Vector xiprime3 = Adjoint(T3, screw::xi); - CHECK(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); + EXPECT(assert_equal(expected3, Pose3::Retract(xiprime3), 1e-6)); } /* ************************************************************************* */ @@ -142,28 +145,28 @@ TEST(Pose3, expmaps_galore) { Vector xi; Pose3 actual; xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6); - actual = Pose3::Expmap(xi); - CHECK(assert_equal(expm(xi), actual,1e-6)); - CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6)); - CHECK(assert_equal(xi, logmap(actual),1e-6)); + actual = Pose3::Retract(xi); + EXPECT(assert_equal(expm(xi), actual,1e-6)); + EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); + EXPECT(assert_equal(xi, logmap(actual),1e-6)); xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6); for (double theta=1.0;0.3*theta<=M_PI;theta*=2) { Vector txi = xi*theta; - actual = Pose3::Expmap(txi); - CHECK(assert_equal(expm(txi,30), actual,1e-6)); - CHECK(assert_equal(Agrawal06iros(txi), actual,1e-6)); + actual = Pose3::Retract(txi); + EXPECT(assert_equal(expm(txi,30), actual,1e-6)); + EXPECT(assert_equal(Agrawal06iros(txi), actual,1e-6)); Vector log = logmap(actual); - CHECK(assert_equal(actual, Pose3::Expmap(log),1e-6)); - CHECK(assert_equal(txi,log,1e-6)); // not true once wraps + EXPECT(assert_equal(actual, Pose3::Retract(log),1e-6)); + EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps } // Works with large v as well, but expm needs 10 iterations! xi = Vector_(6,0.2,0.3,-0.8,100.0,120.0,-60.0); - actual = Pose3::Expmap(xi); - CHECK(assert_equal(expm(xi,10), actual,1e-5)); - CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6)); - CHECK(assert_equal(xi, logmap(actual),1e-6)); + actual = Pose3::Retract(xi); + EXPECT(assert_equal(expm(xi,10), actual,1e-5)); + EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); + EXPECT(assert_equal(xi, logmap(actual),1e-6)); } /* ************************************************************************* */ @@ -173,35 +176,35 @@ TEST(Pose3, Adjoint_compose) // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 const Pose3& T1 = T; Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8); - Pose3 expected = T1 * Pose3::Expmap(x) * T2; + Pose3 expected = T1 * Pose3::Retract(x) * T2; Vector y = Adjoint(inverse(T2), x); - Pose3 actual = T1 * T2 * Pose3::Expmap(y); - CHECK(assert_equal(expected, actual, 1e-6)); + Pose3 actual = T1 * T2 * Pose3::Retract(y); + EXPECT(assert_equal(expected, actual, 1e-6)); } #endif // SLOW_BUT_CORRECT_EXMAP /* ************************************************************************* */ TEST(Pose3, expmap_c_full) { - CHECK(assert_equal(screw::expected, expm(screw::xi),1e-6)); - CHECK(assert_equal(screw::expected, Pose3::ExpmapFull(screw::xi),1e-6)); + EXPECT(assert_equal(screw::expected, expm(screw::xi),1e-6)); + EXPECT(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6)); } /* ************************************************************************* */ // assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) TEST(Pose3, Adjoint_full) { - Pose3 expected = T * Pose3::ExpmapFull(screw::xi) * T.inverse(); + Pose3 expected = T * Pose3::Expmap(screw::xi) * T.inverse(); Vector xiprime = T.Adjoint(screw::xi); - CHECK(assert_equal(expected, Pose3::ExpmapFull(xiprime), 1e-6)); + EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6)); - Pose3 expected2 = T2 * Pose3::ExpmapFull(screw::xi) * T2.inverse(); + Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * T2.inverse(); Vector xiprime2 = T2.Adjoint(screw::xi); - CHECK(assert_equal(expected2, Pose3::ExpmapFull(xiprime2), 1e-6)); + EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6)); - Pose3 expected3 = T3 * Pose3::ExpmapFull(screw::xi) * T3.inverse(); + Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * T3.inverse(); Vector xiprime3 = T3.Adjoint(screw::xi); - CHECK(assert_equal(expected3, Pose3::ExpmapFull(xiprime3), 1e-6)); + EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); } /* ************************************************************************* */ @@ -224,28 +227,28 @@ TEST(Pose3, expmaps_galore_full) { Vector xi; Pose3 actual; xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6); - actual = Pose3::ExpmapFull(xi); - CHECK(assert_equal(expm(xi), actual,1e-6)); - CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6)); - CHECK(assert_equal(xi, Pose3::LogmapFull(actual),1e-6)); + actual = Pose3::Expmap(xi); + EXPECT(assert_equal(expm(xi), actual,1e-6)); + EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); + EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6)); xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6); for (double theta=1.0;0.3*theta<=M_PI;theta*=2) { Vector txi = xi*theta; - actual = Pose3::ExpmapFull(txi); - CHECK(assert_equal(expm(txi,30), actual,1e-6)); - CHECK(assert_equal(Agrawal06iros(txi), actual,1e-6)); - Vector log = Pose3::LogmapFull(actual); - CHECK(assert_equal(actual, Pose3::ExpmapFull(log),1e-6)); - CHECK(assert_equal(txi,log,1e-6)); // not true once wraps + actual = Pose3::Expmap(txi); + EXPECT(assert_equal(expm(txi,30), actual,1e-6)); + EXPECT(assert_equal(Agrawal06iros(txi), actual,1e-6)); + Vector log = Pose3::Logmap(actual); + EXPECT(assert_equal(actual, Pose3::Expmap(log),1e-6)); + EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps } // Works with large v as well, but expm needs 10 iterations! xi = Vector_(6,0.2,0.3,-0.8,100.0,120.0,-60.0); - actual = Pose3::ExpmapFull(xi); - CHECK(assert_equal(expm(xi,10), actual,1e-5)); - CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6)); - CHECK(assert_equal(xi, Pose3::LogmapFull(actual),1e-6)); + actual = Pose3::Expmap(xi); + EXPECT(assert_equal(expm(xi,10), actual,1e-5)); + EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); + EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6)); } /* ************************************************************************* */ @@ -255,10 +258,10 @@ TEST(Pose3, Adjoint_compose_full) // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 const Pose3& T1 = T; Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8); - Pose3 expected = T1 * Pose3::ExpmapFull(x) * T2; + Pose3 expected = T1 * Pose3::Expmap(x) * T2; Vector y = T2.inverse().Adjoint(x); - Pose3 actual = T1 * T2 * Pose3::ExpmapFull(y); - CHECK(assert_equal(expected, actual, 1e-6)); + Pose3 actual = T1 * T2 * Pose3::Expmap(y); + EXPECT(assert_equal(expected, actual, 1e-6)); } /* ************************************************************************* */ @@ -266,16 +269,16 @@ TEST( Pose3, compose ) { Matrix actual = (T2*T2).matrix(); Matrix expected = T2.matrix()*T2.matrix(); - CHECK(assert_equal(actual,expected,1e-8)); + EXPECT(assert_equal(actual,expected,1e-8)); Matrix actualDcompose1, actualDcompose2; T2.compose(T2, actualDcompose1, actualDcompose2); Matrix numericalH1 = numericalDerivative21(testing::compose, T2, T2); - CHECK(assert_equal(numericalH1,actualDcompose1,5e-3)); + EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3)); Matrix numericalH2 = numericalDerivative22(testing::compose, T2, T2); - CHECK(assert_equal(numericalH2,actualDcompose2)); + EXPECT(assert_equal(numericalH2,actualDcompose2)); } /* ************************************************************************* */ @@ -284,16 +287,16 @@ TEST( Pose3, compose2 ) const Pose3& T1 = T; Matrix actual = (T1*T2).matrix(); Matrix expected = T1.matrix()*T2.matrix(); - CHECK(assert_equal(actual,expected,1e-8)); + EXPECT(assert_equal(actual,expected,1e-8)); Matrix actualDcompose1, actualDcompose2; T1.compose(T2, actualDcompose1, actualDcompose2); Matrix numericalH1 = numericalDerivative21(testing::compose, T1, T2); - CHECK(assert_equal(numericalH1,actualDcompose1,5e-3)); + EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3)); Matrix numericalH2 = numericalDerivative22(testing::compose, T1, T2); - CHECK(assert_equal(numericalH2,actualDcompose2)); + EXPECT(assert_equal(numericalH2,actualDcompose2)); } /* ************************************************************************* */ @@ -302,10 +305,10 @@ TEST( Pose3, inverse) Matrix actualDinverse; Matrix actual = T.inverse(actualDinverse).matrix(); Matrix expected = inverse(T.matrix()); - CHECK(assert_equal(actual,expected,1e-8)); + EXPECT(assert_equal(actual,expected,1e-8)); Matrix numericalH = numericalDerivative11(testing::inverse, T); - CHECK(assert_equal(numericalH,actualDinverse,5e-3)); + EXPECT(assert_equal(numericalH,actualDinverse,5e-3)); } /* ************************************************************************* */ @@ -318,7 +321,7 @@ TEST( Pose3, inverseDerivatives2) Matrix numericalH = numericalDerivative11(testing::inverse, T); Matrix actualDinverse; T.inverse(actualDinverse); - CHECK(assert_equal(numericalH,actualDinverse,5e-3)); + EXPECT(assert_equal(numericalH,actualDinverse,5e-3)); } /* ************************************************************************* */ @@ -326,7 +329,7 @@ TEST( Pose3, compose_inverse) { Matrix actual = (T*T.inverse()).matrix(); Matrix expected = eye(4,4); - CHECK(assert_equal(actual,expected,1e-8)); + EXPECT(assert_equal(actual,expected,1e-8)); } /* ************************************************************************* */ @@ -336,7 +339,7 @@ TEST( Pose3, Dtransform_from1_a) Matrix actualDtransform_from1; T.transform_from(P, actualDtransform_from1, boost::none); Matrix numerical = numericalDerivative21(transform_from_,T,P); - CHECK(assert_equal(numerical,actualDtransform_from1,1e-8)); + EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8)); } TEST( Pose3, Dtransform_from1_b) @@ -345,7 +348,7 @@ TEST( Pose3, Dtransform_from1_b) Matrix actualDtransform_from1; origin.transform_from(P, actualDtransform_from1, boost::none); Matrix numerical = numericalDerivative21(transform_from_,origin,P); - CHECK(assert_equal(numerical,actualDtransform_from1,1e-8)); + EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8)); } TEST( Pose3, Dtransform_from1_c) @@ -355,7 +358,7 @@ TEST( Pose3, Dtransform_from1_c) Matrix actualDtransform_from1; T0.transform_from(P, actualDtransform_from1, boost::none); Matrix numerical = numericalDerivative21(transform_from_,T0,P); - CHECK(assert_equal(numerical,actualDtransform_from1,1e-8)); + EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8)); } TEST( Pose3, Dtransform_from1_d) @@ -368,7 +371,7 @@ TEST( Pose3, Dtransform_from1_d) //print(computed, "Dtransform_from1_d computed:"); Matrix numerical = numericalDerivative21(transform_from_,T0,P); //print(numerical, "Dtransform_from1_d numerical:"); - CHECK(assert_equal(numerical,actualDtransform_from1,1e-8)); + EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8)); } /* ************************************************************************* */ @@ -377,7 +380,7 @@ TEST( Pose3, Dtransform_from2) Matrix actualDtransform_from2; T.transform_from(P, boost::none, actualDtransform_from2); Matrix numerical = numericalDerivative22(transform_from_,T,P); - CHECK(assert_equal(numerical,actualDtransform_from2,1e-8)); + EXPECT(assert_equal(numerical,actualDtransform_from2,1e-8)); } /* ************************************************************************* */ @@ -387,7 +390,7 @@ TEST( Pose3, Dtransform_to1) Matrix computed; T.transform_to(P, computed, boost::none); Matrix numerical = numericalDerivative21(transform_to_,T,P); - CHECK(assert_equal(numerical,computed,1e-8)); + EXPECT(assert_equal(numerical,computed,1e-8)); } /* ************************************************************************* */ @@ -396,7 +399,7 @@ TEST( Pose3, Dtransform_to2) Matrix computed; T.transform_to(P, boost::none, computed); Matrix numerical = numericalDerivative22(transform_to_,T,P); - CHECK(assert_equal(numerical,computed,1e-8)); + EXPECT(assert_equal(numerical,computed,1e-8)); } /* ************************************************************************* */ @@ -406,8 +409,8 @@ TEST( Pose3, transform_to_with_derivatives) T.transform_to(P,actH1,actH2); Matrix expH1 = numericalDerivative21(transform_to_, T,P), expH2 = numericalDerivative22(transform_to_, T,P); - CHECK(assert_equal(expH1, actH1, 1e-8)); - CHECK(assert_equal(expH2, actH2, 1e-8)); + EXPECT(assert_equal(expH1, actH1, 1e-8)); + EXPECT(assert_equal(expH2, actH2, 1e-8)); } /* ************************************************************************* */ @@ -417,8 +420,8 @@ TEST( Pose3, transform_from_with_derivatives) T.transform_from(P,actH1,actH2); Matrix expH1 = numericalDerivative21(transform_from_, T,P), expH2 = numericalDerivative22(transform_from_, T,P); - CHECK(assert_equal(expH1, actH1, 1e-8)); - CHECK(assert_equal(expH2, actH2, 1e-8)); + EXPECT(assert_equal(expH1, actH1, 1e-8)); + EXPECT(assert_equal(expH2, actH2, 1e-8)); } /* ************************************************************************* */ @@ -426,7 +429,7 @@ TEST( Pose3, transform_to_translate) { Point3 actual = Pose3(Rot3(), Point3(1, 2, 3)).transform_to(Point3(10.,20.,30.)); Point3 expected(9.,18.,27.); - CHECK(assert_equal(expected, actual)); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ @@ -435,7 +438,7 @@ TEST( Pose3, transform_to_rotate) Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3()); Point3 actual = transform.transform_to(Point3(2,1,10)); Point3 expected(-1,2,10); - CHECK(assert_equal(expected, actual, 0.001)); + EXPECT(assert_equal(expected, actual, 0.001)); } /* ************************************************************************* */ @@ -444,7 +447,7 @@ TEST( Pose3, transform_to) Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3(2,4, 0)); Point3 actual = transform.transform_to(Point3(3,2,10)); Point3 expected(2,1,10); - CHECK(assert_equal(expected, actual, 0.001)); + EXPECT(assert_equal(expected, actual, 0.001)); } /* ************************************************************************* */ @@ -452,7 +455,7 @@ TEST( Pose3, transform_from) { Point3 actual = T3.transform_from(Point3()); Point3 expected = Point3(1.,2.,3.); - CHECK(assert_equal(expected, actual)); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ @@ -460,7 +463,7 @@ TEST( Pose3, transform_roundtrip) { Point3 actual = T3.transform_from(T3.transform_to(Point3(12., -0.11,7.0))); Point3 expected(12., -0.11,7.0); - CHECK(assert_equal(expected, actual)); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ @@ -468,7 +471,7 @@ TEST( Pose3, transformPose_to_origin) { // transform to origin Pose3 actual = T3.transform_to(Pose3()); - CHECK(assert_equal(T3, actual, 1e-8)); + EXPECT(assert_equal(T3, actual, 1e-8)); } /* ************************************************************************* */ @@ -476,7 +479,7 @@ TEST( Pose3, transformPose_to_itself) { // transform to itself Pose3 actual = T3.transform_to(T3); - CHECK(assert_equal(Pose3(), actual, 1e-8)); + EXPECT(assert_equal(Pose3(), actual, 1e-8)); } /* ************************************************************************* */ @@ -487,7 +490,7 @@ TEST( Pose3, transformPose_to_translation) Pose3 pose2(r, Point3(21.,32.,13.)); Pose3 actual = pose2.transform_to(Pose3(Rot3(), Point3(1,2,3))); Pose3 expected(r, Point3(20.,30.,10.)); - CHECK(assert_equal(expected, actual, 1e-8)); + EXPECT(assert_equal(expected, actual, 1e-8)); } /* ************************************************************************* */ @@ -499,7 +502,7 @@ TEST( Pose3, transformPose_to_simple_rotate) Pose3 transform(r, Point3(1,2,3)); Pose3 actual = pose2.transform_to(transform); Pose3 expected(Rot3(), Point3(-30.,20.,10.)); - CHECK(assert_equal(expected, actual, 0.001)); + EXPECT(assert_equal(expected, actual, 0.001)); } /* ************************************************************************* */ @@ -512,7 +515,7 @@ TEST( Pose3, transformPose_to) Pose3 transform(r, Point3(1,2,3)); Pose3 actual = pose2.transform_to(transform); Pose3 expected(Rot3::rodriguez(0,0,2.26892803), Point3(-30.,20.,10.)); - CHECK(assert_equal(expected, actual, 0.001)); + EXPECT(assert_equal(expected, actual, 0.001)); } /* ************************************************************************* */ @@ -522,17 +525,17 @@ TEST(Pose3, manifold) Pose3 t1 = T; Pose3 t2 = T3; Pose3 origin; - Vector d12 = t1.logmap(t2); - CHECK(assert_equal(t2, t1.expmap(d12))); + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); // todo: richard - commented out because this tests for "compose-style" (new) expmap - // CHECK(assert_equal(t2, expmap(origin,d12)*t1)); - Vector d21 = t2.logmap(t1); - CHECK(assert_equal(t1, t2.expmap(d21))); + // EXPECT(assert_equal(t2, retract(origin,d12)*t1)); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); // todo: richard - commented out because this tests for "compose-style" (new) expmap - // CHECK(assert_equal(t1, expmap(origin,d21)*t2)); + // EXPECT(assert_equal(t1, retract(origin,d21)*t2)); // Check that log(t1,t2)=-log(t2,t1) - this holds even for incorrect expmap :-) - CHECK(assert_equal(d12,-d21)); + EXPECT(assert_equal(d12,-d21)); #ifdef CORRECT_POSE3_EXMAP @@ -541,13 +544,13 @@ TEST(Pose3, manifold) // lines in canonical coordinates correspond to Abelian subgroups in SE(3) Vector d = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6); // exp(-d)=inverse(exp(d)) - CHECK(assert_equal(Pose3::Expmap(-d),inverse(Pose3::Expmap(d)))); + EXPECT(assert_equal(Pose3::Retract(-d),inverse(Pose3::Retract(d)))); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) - Pose3 T2 = Pose3::Expmap(2*d); - Pose3 T3 = Pose3::Expmap(3*d); - Pose3 T5 = Pose3::Expmap(5*d); - CHECK(assert_equal(T5,T2*T3)); - CHECK(assert_equal(T5,T3*T2)); + Pose3 T2 = Pose3::Retract(2*d); + Pose3 T3 = Pose3::Retract(3*d); + Pose3 T5 = Pose3::Retract(5*d); + EXPECT(assert_equal(T5,T2*T3)); + EXPECT(assert_equal(T5,T3*T2)); #endif } @@ -558,13 +561,13 @@ TEST( Pose3, between ) Pose3 expected = T2.inverse() * T3; Matrix actualDBetween1,actualDBetween2; Pose3 actual = T2.between(T3, actualDBetween1,actualDBetween2); - CHECK(assert_equal(expected,actual)); + EXPECT(assert_equal(expected,actual)); Matrix numericalH1 = numericalDerivative21(testing::between , T2, T3); - CHECK(assert_equal(numericalH1,actualDBetween1,5e-3)); + EXPECT(assert_equal(numericalH1,actualDBetween1,5e-3)); Matrix numericalH2 = numericalDerivative22(testing::between , T2, T3); - CHECK(assert_equal(numericalH2,actualDBetween2)); + EXPECT(assert_equal(numericalH2,actualDBetween2)); } /* ************************************************************************* */ @@ -652,9 +655,9 @@ TEST( Pose3, unicycle ) { // velocity in X should be X in inertial frame, rather than global frame Vector x_step = delta(6,3,1.0); - EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), x1.expmapFull(x_step), tol)); - EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), x2.expmapFull(x_step), tol)); - EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI_4,0,0), Point3(2,2,0)), x3.expmapFull(sqrt(2) * x_step), tol)); + EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), expmap_default(x1, x_step), tol)); + EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), expmap_default(x2, x_step), tol)); + EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI_4,0,0), Point3(2,2,0)), expmap_default(x3, sqrt(2) * x_step), tol)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 15a81d0f4..e23224898 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -22,6 +22,9 @@ using namespace gtsam; +GTSAM_CONCEPT_TESTABLE_INST(Rot2) +GTSAM_CONCEPT_LIE_INST(Rot2) + Rot2 R(Rot2::fromAngle(0.1)); Point2 P(0.2, 0.7); @@ -85,7 +88,7 @@ TEST( Rot2, equals) TEST( Rot2, expmap) { Vector v = zero(1); - CHECK(assert_equal(R.expmap(v), R)); + CHECK(assert_equal(R.retract(v), R)); } /* ************************************************************************* */ @@ -94,7 +97,7 @@ TEST(Rot2, logmap) Rot2 rot0(Rot2::fromAngle(M_PI_2)); Rot2 rot(Rot2::fromAngle(M_PI)); Vector expected = Vector_(1, M_PI_2); - Vector actual = rot0.logmap(rot); + Vector actual = rot0.localCoordinates(rot); CHECK(assert_equal(expected, actual)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 06ca2747f..ec001b465 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -25,6 +25,9 @@ using namespace gtsam; +GTSAM_CONCEPT_TESTABLE_INST(Rot3) +GTSAM_CONCEPT_LIE_INST(Rot3) + Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); Point3 P(0.2, 0.7, -2.0); double error = 1e-9, epsilon = 0.001; @@ -138,7 +141,7 @@ TEST( Rot3, rodriguez4) TEST( Rot3, expmap) { Vector v = zero(3); - CHECK(assert_equal(R.expmap(v), R)); + CHECK(assert_equal(R.retract(v), R)); } /* ************************************************************************* */ @@ -185,11 +188,11 @@ TEST(Rot3, manifold) Rot3 origin; // log behaves correctly - Vector d12 = gR1.logmap(gR2); - CHECK(assert_equal(gR2, gR1.expmap(d12))); + Vector d12 = gR1.localCoordinates(gR2); + CHECK(assert_equal(gR2, gR1.retract(d12))); CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12))); - Vector d21 = gR2.logmap(gR1); - CHECK(assert_equal(gR1, gR2.expmap(d21))); + Vector d21 = gR2.localCoordinates(gR1); + CHECK(assert_equal(gR1, gR2.retract(d21))); CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21))); // Check that log(t1,t2)=-log(t2,t1) diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index ae4248ce5..2446ff122 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -24,6 +24,9 @@ using namespace std; using namespace gtsam; +GTSAM_CONCEPT_TESTABLE_INST(StereoCamera) +GTSAM_CONCEPT_MANIFOLD_INST(StereoCamera) + /* ************************************************************************* */ TEST( StereoCamera, operators) { diff --git a/gtsam/geometry/tests/testStereoPoint2.cpp b/gtsam/geometry/tests/testStereoPoint2.cpp new file mode 100644 index 000000000..96410426d --- /dev/null +++ b/gtsam/geometry/tests/testStereoPoint2.cpp @@ -0,0 +1,27 @@ +/** + * @file testStereoPoint2.cpp + * + * @brief Tests for the StereoPoint2 class + * + * @date Nov 4, 2011 + * @author Alex Cunningham + */ + +#include + +#include +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2) +GTSAM_CONCEPT_LIE_INST(StereoPoint2) + +/* ************************************************************************* */ +TEST(testStereoPoint2, test) { + +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index aa139eaa3..823853f6d 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -43,7 +43,7 @@ namespace gtsam { // Extract the current estimate of x1,P1 from the Bayes Network VectorValues result = optimize(*linearBayesNet); - T x = linearizationPoints[lastKey].expmap(result[lastIndex]); + T x = linearizationPoints[lastKey].retract(result[lastIndex]); // Create a Jacobian Factor from the root node of the produced Bayes Net. // This will act as a prior for the next iteration. 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/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index d94865cf2..d32085eeb 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -237,7 +237,7 @@ struct _SelectiveExpmapAndClear { assert(delta[var].size() == (int)it_x->second.dim()); assert(delta[var].unaryExpr(&isfinite).all()); if(mask[var]) { - it_x->second = it_x->second.expmap(delta[var]); + it_x->second = it_x->second.retract(delta[var]); if(invalidate) (*invalidate)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits::infinity())); // Strange syntax to work with clang++ (bug in clang?) } diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index c62f843e0..4e8accd4e 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -548,7 +548,7 @@ template Values ISAM2::calculateBestEstimate() const { VectorValues delta(theta_.dims(ordering_)); optimize2(this->root(), delta); - return theta_.expmap(delta, ordering_); + return theta_.retract(delta, ordering_); } } diff --git a/gtsam/nonlinear/NonlinearConstraint.h b/gtsam/nonlinear/NonlinearConstraint.h index ce3738ed9..9478d808d 100644 --- a/gtsam/nonlinear/NonlinearConstraint.h +++ b/gtsam/nonlinear/NonlinearConstraint.h @@ -573,7 +573,7 @@ public: Vector evaluateError(const X& x1, boost::optional H1 = boost::none) const { const size_t p = X::Dim(); if (H1) *H1 = eye(p); - return value_.logmap(x1); + return value_.localCoordinates(x1); } /** Print */ @@ -628,7 +628,7 @@ public: const size_t p = X::Dim(); if (H1) *H1 = -eye(p); if (H2) *H2 = eye(p); - return x1.logmap(x2); + return x1.localCoordinates(x2); } private: diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index e36e22f1a..2b7ac2e7f 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -120,7 +120,7 @@ namespace gtsam { size_t nj = feasible_.dim(); if (allow_error_) { if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare - return xj.logmap(feasible_); + return xj.localCoordinates(feasible_); } else if (compare_(feasible_,xj)) { if (H) *H = eye(nj); return zero(nj); // set error to zero if equal diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h index eba64cf33..761f2e032 100644 --- a/gtsam/nonlinear/NonlinearISAM-inl.h +++ b/gtsam/nonlinear/NonlinearISAM-inl.h @@ -93,7 +93,7 @@ void NonlinearISAM::reorder_relinearize() { template Values NonlinearISAM::estimate() const { if(isam_.size() > 0) - return linPoint_.expmap(optimize(isam_), ordering_); + return linPoint_.retract(optimize(isam_), ordering_); else return linPoint_; } diff --git a/gtsam/nonlinear/NonlinearOptimizer-inl.h b/gtsam/nonlinear/NonlinearOptimizer-inl.h index a248f5606..52437aa61 100644 --- a/gtsam/nonlinear/NonlinearOptimizer-inl.h +++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h @@ -90,7 +90,7 @@ namespace gtsam { if (verbosity >= Parameters::DELTA) delta.print("delta"); // take old values and update it - shared_values newValues(new C(values_->expmap(delta, *ordering_))); + shared_values newValues(new C(values_->retract(delta, *ordering_))); // maybe show output if (verbosity >= Parameters::VALUES) newValues->print("newValues"); @@ -182,7 +182,7 @@ namespace gtsam { if (verbosity >= Parameters::TRYDELTA) delta.print("delta"); // update values - shared_values newValues(new T(values_->expmap(delta, *ordering_))); + shared_values newValues(new T(values_->retract(delta, *ordering_))); // create new optimization state with more adventurous lambda double error = graph_->error(*newValues); 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 977f361d3..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 @@ -205,21 +205,21 @@ namespace gtsam { } /** Expmap */ - TupleValues expmap(const VectorValues& delta, const Ordering& ordering) const { - return TupleValues(first_.expmap(delta, ordering), second_.expmap(delta, ordering)); + TupleValues retract(const VectorValues& delta, const Ordering& ordering) const { + return TupleValues(first_.retract(delta, ordering), second_.retract(delta, ordering)); } /** logmap each element */ - VectorValues logmap(const TupleValues& cp, const Ordering& ordering) const { + VectorValues localCoordinates(const TupleValues& cp, const Ordering& ordering) const { VectorValues delta(this->dims(ordering)); - logmap(cp, ordering, delta); + localCoordinates(cp, ordering, delta); return delta; } /** logmap each element */ - void logmap(const TupleValues& cp, const Ordering& ordering, VectorValues& delta) const { - first_.logmap(cp.first_, ordering, delta); - second_.logmap(cp.second_, ordering, delta); + void localCoordinates(const TupleValues& cp, const Ordering& ordering, VectorValues& delta) const { + first_.localCoordinates(cp.first_, ordering, delta); + second_.localCoordinates(cp.second_, ordering, delta); } /** @@ -318,18 +318,18 @@ namespace gtsam { size_t dim() const { return first_.dim(); } - TupleValuesEnd expmap(const VectorValues& delta, const Ordering& ordering) const { - return TupleValuesEnd(first_.expmap(delta, ordering)); + TupleValuesEnd retract(const VectorValues& delta, const Ordering& ordering) const { + return TupleValuesEnd(first_.retract(delta, ordering)); } - VectorValues logmap(const TupleValuesEnd& cp, const Ordering& ordering) const { + VectorValues localCoordinates(const TupleValuesEnd& cp, const Ordering& ordering) const { VectorValues delta(this->dims(ordering)); - logmap(cp, ordering, delta); + localCoordinates(cp, ordering, delta); return delta; } - void logmap(const TupleValuesEnd& cp, const Ordering& ordering, VectorValues& delta) const { - first_.logmap(cp.first_, ordering, delta); + void localCoordinates(const TupleValuesEnd& cp, const Ordering& ordering, VectorValues& delta) const { + first_.localCoordinates(cp.first_, ordering, delta); } /** diff --git a/gtsam/nonlinear/LieValues-inl.h b/gtsam/nonlinear/Values-inl.h similarity index 72% rename from gtsam/nonlinear/LieValues-inl.h rename to gtsam/nonlinear/Values-inl.h index 2e7f2b3f6..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,15 +136,15 @@ namespace gtsam { /* ************************************************************************* */ // todo: insert for every element is inefficient template - LieValues LieValues::expmap(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; const typename J::Value& pj = value.second; Index index; if(ordering.tryAt(j,index)) { - newValues.insert(j, pj.expmap(delta[index])); + newValues.insert(j, pj.retract(delta[index])); } else newValues.insert(j, pj); } @@ -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,19 +171,19 @@ namespace gtsam { // /* ************************************************************************* */ // // todo: insert for every element is inefficient // template -// LieValues LieValues::expmap(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_) { // const J& j = value.first; // const typename J::Value& pj = value.second; // int cur_dim = pj.dim(); -// newValues.insert(j,pj.expmap(sub(delta, delta_offset, delta_offset+cur_dim))); +// newValues.insert(j,pj.retract(sub(delta, delta_offset, delta_offset+cur_dim))); // delta_offset += cur_dim; // } // return newValues; @@ -193,19 +193,19 @@ namespace gtsam { // todo: insert for every element is inefficient // todo: currently only logmaps elements in both configs template - inline VectorValues LieValues::logmap(const LieValues& cp, const Ordering& ordering) const { + inline VectorValues Values::localCoordinates(const Values& cp, const Ordering& ordering) const { VectorValues delta(this->dims(ordering)); - logmap(cp, ordering, delta); + localCoordinates(cp, ordering, delta); return delta; } /* ************************************************************************* */ template - void LieValues::logmap(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)); - delta[ordering[value.first]] = this->at(value.first).logmap(value.second); + delta[ordering[value.first]] = this->at(value.first).localCoordinates(value.second); } } diff --git a/gtsam/nonlinear/LieValues.h b/gtsam/nonlinear/Values.h similarity index 84% rename from gtsam/nonlinear/LieValues.h rename to gtsam/nonlinear/Values.h index 97bec1f35..bb45435ce 100644 --- a/gtsam/nonlinear/LieValues.h +++ b/gtsam/nonlinear/Values.h @@ -10,16 +10,16 @@ * -------------------------------------------------------------------------- */ /** - * @file LieValues.h + * @file Values.h * @author Richard Roberts * - * @brief A templated config for Lie-group elements + * @brief A templated config for Manifold-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 - * are elements of Lie groups, not just vectors. It then, as a whole, implements a aggregate type - * which is also a Lie group, and hence supports operations dim, expmap, and logmap. + * 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. */ #pragma once @@ -29,6 +29,7 @@ #include #include #include +#include #include namespace boost { template class optional; } @@ -37,7 +38,7 @@ namespace gtsam { class VectorValues; class Ordering; } namespace gtsam { /** - * Lie type values structure + * Manifold type values structure * Takes two template types * J: a key type to look up values in the values structure (need to be sortable) * @@ -47,7 +48,7 @@ namespace gtsam { * labels (example: Pose2, Point2, etc) */ template - class LieValues { + class Values { public: @@ -66,23 +67,24 @@ namespace gtsam { /** concept check */ GTSAM_CONCEPT_TESTABLE_TYPE(Value) + GTSAM_CONCEPT_MANIFOLD_TYPE(Value) KeyValueMap values_; 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; @@ -105,9 +107,6 @@ namespace gtsam { /** whether the config is empty */ bool empty() const { return values_.empty(); } - /** The dimensionality of the tangent space */ - size_t dim() const; - /** Get a zero VectorValues of the correct structure */ VectorValues zero(const Ordering& ordering) const; @@ -116,16 +115,19 @@ namespace gtsam { iterator begin() { return values_.begin(); } iterator end() { return values_.end(); } - // Lie operations + // Manifold operations + + /** The dimensionality of the tangent space */ + size_t dim() const; /** Add a delta config to current config and returns a new config */ - LieValues expmap(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 logmap(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 logmap(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const; + void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const; // imperative methods: @@ -133,10 +135,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); @@ -156,7 +158,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 83% rename from gtsam/nonlinear/tests/testLieValues.cpp rename to gtsam/nonlinear/tests/testValues.cpp index a4e0cc698..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,17 +133,17 @@ 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)); - CHECK(assert_equal(expected, config0.expmap(increment, ordering))); + CHECK(assert_equal(expected, config0.retract(increment, ordering))); } ///* ************************************************************************* */ -//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,17 +151,17 @@ 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)); // -// CHECK(assert_equal(expected, config0.expmap(increment, ordering))); +// CHECK(assert_equal(expected, config0.retract(increment, ordering))); //} ///* ************************************************************************* */ -//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,24 +169,24 @@ 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)); // -// CHECK(assert_equal(expected, config0.expmap(increment))); +// CHECK(assert_equal(expected, config0.retract(increment))); //} /* ************************************************************************* */ -/*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/BetweenConstraint.h b/gtsam/slam/BetweenConstraint.h index 0d4ae632c..e4085f0dc 100644 --- a/gtsam/slam/BetweenConstraint.h +++ b/gtsam/slam/BetweenConstraint.h @@ -51,7 +51,7 @@ namespace gtsam { boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { X hx = x1.between(x2, H1, H2); - return measured_.logmap(hx); + return measured_.localCoordinates(hx); } inline const X measured() const { diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 339dbad3b..c12b65d50 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -83,7 +83,7 @@ namespace gtsam { boost::none) const { T hx = p1.between(p2, H1, H2); // h(x) // manifold equivalent of h(x)-z -> log(z,h(x)) - return measured_.logmap(hx); + return measured_.localCoordinates(hx); } /** return the measured */ diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 37f084033..66ec820c4 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -82,7 +82,7 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const { - Vector error = z_.logmap(camera.project(point,H1,H2)); + Vector error = z_.localCoordinates(camera.project(point,H1,H2)); // gtsam::print(error, "error"); return error; } diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h index 9b76ee5bc..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. @@ -118,6 +118,7 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const T& p, boost::optional H = boost::none) const { if (H) (*H) = zeros(this->dim(), p.dim()); + // FIXME: this was originally the generic retraction - may not produce same results Vector full_logmap = T::Logmap(p); Vector masked_logmap = zero(this->dim()); size_t masked_idx=0; diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 33f3d1aef..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 */ @@ -81,7 +81,7 @@ namespace gtsam { Vector evaluateError(const T& p, boost::optional H = boost::none) const { if (H) (*H) = eye(p.dim()); // manifold equivalent of h(x)-z -> log(z,h(x)) - return prior_.logmap(p); + return prior_.localCoordinates(p); } private: 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/dataset.cpp b/gtsam/slam/dataset.cpp index 6b247c3fe..b5322ee47 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -130,7 +130,7 @@ pair load2D(const string& filename, } if (addNoise) - l1Xl2 = l1Xl2.expmap((*model)->sample()); + l1Xl2 = l1Xl2.retract((*model)->sample()); // Insert vertices if pure odometry file if (!poses->exists(id1)) poses->insert(id1, Pose2()); 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 0934ed6b0..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 @@ -69,7 +69,7 @@ namespace gtsam { /// Evaluate error and optionally derivative Vector evaluateError(const Pose2& x, boost::optional H = boost::none) const { - return z_.logmap(prior(x, H)); + return z_.localCoordinates(prior(x, H)); } }; @@ -93,7 +93,7 @@ namespace gtsam { Vector evaluateError(const Pose2& x1, const Pose2& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return z_.logmap(odo(x1, x2, H1, H2)); + return z_.localCoordinates(odo(x1, x2, H1, H2)); } }; diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 096e0d13b..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, @@ -333,7 +321,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { skew_noise, // s trans_noise, trans_noise // ux, uy ) ; - values->insert((int)i, X[i].expmap(delta)) ; + values->insert((int)i, X[i].retract(delta)) ; } } @@ -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 afc5fa526..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, @@ -327,7 +317,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { trans_noise, trans_noise, trans_noise, // translation focal_noise, distort_noise, distort_noise // f, k1, k2 ) ; - values->insert((int)i, X[i].expmap(delta)) ; + values->insert((int)i, X[i].retract(delta)) ; } } @@ -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/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index edf90bf37..7068f99bf 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -190,8 +190,8 @@ TEST(Pose2Graph, optimizeThreePoses) { // Create initial config boost::shared_ptr initial(new Pose2Values()); initial->insert(0, p0); - initial->insert(1, hexagon[1].expmap(Vector_(3,-0.1, 0.1,-0.1))); - initial->insert(2, hexagon[2].expmap(Vector_(3, 0.1,-0.1, 0.1))); + initial->insert(1, hexagon[1].retract(Vector_(3,-0.1, 0.1,-0.1))); + initial->insert(2, hexagon[2].retract(Vector_(3, 0.1,-0.1, 0.1))); // Choose an ordering shared_ptr ordering(new Ordering); @@ -230,11 +230,11 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) { // Create initial config boost::shared_ptr initial(new Pose2Values()); initial->insert(0, p0); - initial->insert(1, hexagon[1].expmap(Vector_(3,-0.1, 0.1,-0.1))); - initial->insert(2, hexagon[2].expmap(Vector_(3, 0.1,-0.1, 0.1))); - initial->insert(3, hexagon[3].expmap(Vector_(3,-0.1, 0.1,-0.1))); - initial->insert(4, hexagon[4].expmap(Vector_(3, 0.1,-0.1, 0.1))); - initial->insert(5, hexagon[5].expmap(Vector_(3,-0.1, 0.1,-0.1))); + initial->insert(1, hexagon[1].retract(Vector_(3,-0.1, 0.1,-0.1))); + initial->insert(2, hexagon[2].retract(Vector_(3, 0.1,-0.1, 0.1))); + initial->insert(3, hexagon[3].retract(Vector_(3,-0.1, 0.1,-0.1))); + initial->insert(4, hexagon[4].retract(Vector_(3, 0.1,-0.1, 0.1))); + initial->insert(5, hexagon[5].retract(Vector_(3,-0.1, 0.1,-0.1))); // Choose an ordering shared_ptr ordering(new Ordering); @@ -371,7 +371,7 @@ TEST( Pose2Values, expmap ) delta[ordering[Key(1)]] = Vector_(3, -0.1,0.0,0.0); delta[ordering[Key(2)]] = Vector_(3, 0.0,0.1,0.0); delta[ordering[Key(3)]] = Vector_(3, 0.1,0.0,0.0); - Pose2Values actual = circle.expmap(delta, ordering); + Pose2Values actual = circle.retract(delta, ordering); CHECK(assert_equal(expected,actual)); } @@ -406,7 +406,7 @@ TEST( Pose2Prior, error ) VectorValues addition(VectorValues::Zero(x0.dims(ordering))); addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0); VectorValues plus = delta + addition; - Pose2Values x1 = x0.expmap(plus, ordering); + Pose2Values x1 = x0.retract(plus, ordering); Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); @@ -472,7 +472,7 @@ TEST( Pose2Factor, error ) // Check error after increasing p2 VectorValues plus = delta; plus[ordering["x2"]] = Vector_(3, 0.1, 0.0, 0.0); - Pose2Values x1 = x0.expmap(plus, ordering); + Pose2Values x1 = x0.retract(plus, ordering); Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 0efd2c5f5..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) { @@ -69,18 +67,18 @@ TEST(Pose3Graph, optimizeCircle) { // Create initial config boost::shared_ptr initial(new Pose3Values()); initial->insert(0, gT0); - initial->insert(1, hexagon[1].expmap(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial->insert(2, hexagon[2].expmap(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial->insert(3, hexagon[3].expmap(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial->insert(4, hexagon[4].expmap(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial->insert(5, hexagon[5].expmap(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial->insert(1, hexagon[1].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial->insert(2, hexagon[2].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); + initial->insert(3, hexagon[3].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial->insert(4, hexagon[4].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); + initial->insert(5, hexagon[5].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); // Choose an ordering and optimize 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); } @@ -140,18 +138,18 @@ TEST( Pose3Factor, error ) x.insert(1,t1); x.insert(2,t2); - // Get error h(x)-z -> logmap(z,h(x)) = logmap(z,between(t1,t2)) + // Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2)) Vector actual = factor.unwhitenedError(x); - Vector expected = z.logmap(t1.between(t2)); + Vector expected = z.localCoordinates(t1.between(t2)); CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ 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); } @@ -223,7 +221,7 @@ TEST( Pose3Values, expmap ) 0.0,0.0,0.0, 0.1, 0.0, 0.0, 0.0,0.0,0.0, 0.1, 0.0, 0.0, 0.0,0.0,0.0, 0.1, 0.0, 0.0); - Pose3Values actual = pose3SLAM::circle(4,1.0).expmap(delta, ordering); + Pose3Values actual = pose3SLAM::circle(4,1.0).retract(delta, ordering); CHECK(assert_equal(expected,actual)); } diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index 29cf4842e..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.expmap(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 9651ee4e1..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.expmap(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 50723fbe7..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 ) { @@ -232,7 +232,7 @@ public: } // Return the error between the prediction and the supplied value of p2 - return prediction.logmap(p2); + return prediction.localCoordinates(p2); } }; diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 1940a151b..ad9855fa5 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -186,7 +186,7 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const planarSLAM::Values& fu GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); // gbn.print("Expected bayesnet: "); VectorValues delta = optimize(gbn); - planarSLAM::Values expected = fullinit.expmap(delta, ordering); + planarSLAM::Values expected = fullinit.retract(delta, ordering); return assert_equal(expected, actual); } diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp index 2e7e98a2e..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); @@ -179,11 +179,11 @@ TEST(GaussianJunctionTree, slamlike) { GaussianJunctionTree gjt(linearized); VectorValues deltaactual = gjt.optimize(&EliminateQR); - planarSLAM::Values actual = init.expmap(deltaactual, ordering); + planarSLAM::Values actual = init.retract(deltaactual, ordering); GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); VectorValues delta = optimize(gbn); - planarSLAM::Values expected = init.expmap(delta, ordering); + planarSLAM::Values expected = init.retract(delta, ordering); EXPECT(assert_equal(expected, actual)); } 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 b37631d5f..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) { @@ -61,13 +61,13 @@ TEST(testNonlinearISAM, markov_chain ) { } cur_pose = cur_pose.compose(z); - new_init.insert(key2, cur_pose.expmap(sampler.sample())); + new_init.insert(key2, cur_pose.retract(sampler.sample())); expected.insert(key2, cur_pose); isam.update(new_factors, new_init); } // 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 de83e1540..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,18 +192,18 @@ 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; - expected.insert(PoseKey(1), x1.expmap(Vector_(3, 1.0, 1.1, 1.2))); - expected.insert(PoseKey(2), x2.expmap(Vector_(3, 1.3, 1.4, 1.5))); + 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.expmap(delta, o); + TestValues actual = values1.retract(delta, o); CHECK(assert_equal(expected, actual)); // Check log VectorValues expected_log = delta; - VectorValues actual_log = values1.logmap(actual, o); + VectorValues actual_log = values1.localCoordinates(actual, o); CHECK(assert_equal(expected_log, actual_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; @@ -454,13 +454,13 @@ TEST(TupleValues, expmap) delta[o["l2"]] = Vector_(2, 1.3, 1.4); ValuesA expected; - expected.insert(x1k, x1.expmap(Vector_(3, 1.0, 1.1, 1.2))); - expected.insert(x2k, x2.expmap(Vector_(3, 1.3, 1.4, 1.5))); + expected.insert(x1k, x1.retract(Vector_(3, 1.0, 1.1, 1.2))); + expected.insert(x2k, x2.retract(Vector_(3, 1.3, 1.4, 1.5))); expected.insert(l1k, Point2(5.0, 6.1)); expected.insert(l2k, Point2(10.3, 11.4)); - CHECK(assert_equal(expected, values1.expmap(delta, o))); - CHECK(assert_equal(delta, values1.logmap(expected, o))); + CHECK(assert_equal(expected, values1.retract(delta, o))); + CHECK(assert_equal(delta, values1.localCoordinates(expected, o))); } /* ************************************************************************* */ @@ -485,13 +485,13 @@ TEST(TupleValues, expmap_typedefs) delta[o["l1"]] = Vector_(2, 1.0, 1.1); delta[o["l2"]] = Vector_(2, 1.3, 1.4); - expected.insert(x1k, x1.expmap(Vector_(3, 1.0, 1.1, 1.2))); - expected.insert(x2k, x2.expmap(Vector_(3, 1.3, 1.4, 1.5))); + expected.insert(x1k, x1.retract(Vector_(3, 1.0, 1.1, 1.2))); + expected.insert(x2k, x2.retract(Vector_(3, 1.3, 1.4, 1.5))); expected.insert(l1k, Point2(5.0, 6.1)); expected.insert(l2k, Point2(10.3, 11.4)); - CHECK(assert_equal(expected, TupleValues2(values1.expmap(delta, o)))); - //CHECK(assert_equal(delta, values1.logmap(expected))); + CHECK(assert_equal(expected, TupleValues2(values1.retract(delta, o)))); + //CHECK(assert_equal(delta, values1.localCoordinates(expected))); } /* ************************************************************************* */