12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Renamed LieValues, changed Lie interface

release/4.3a0
Alex Cunningham 2011-11-07 20:23:20 +00:00
parent d57feb54b4 d03fa3c294
commit 8592e6b2c6
112 changed files with 2360 additions and 2238 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
.cproject
View File

@ -681,6 +681,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1385,6 +1401,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build_retract" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

4
examples/CameraResectioning.cpp
View File

@ -20,7 +20,7 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/nonlinear/LieValues.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
@ -28,7 +28,7 @@
using namespace gtsam;
typedef TypedSymbol<Pose3, 'x'> PoseKey;
typedef LieValues<PoseKey> PoseValues;
typedef Values<PoseKey> PoseValues;
/**
* Unary factor for the pose.

4
examples/PlanarSLAMExample_easy.cpp
View File

@ -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, Values>(graph, initialEstimate);
planarSLAM::Values result = optimize<Graph, planarSLAM::Values>(graph, initialEstimate);
result.print("final result");
return 0;

26
examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -43,12 +43,12 @@
// Main typedefs
typedef gtsam::TypedSymbol<gtsam::Pose2, 'x'> PoseKey; // Key for poses, with type included
typedef gtsam::TypedSymbol<gtsam::Point2,'l'> PointKey; // Key for points, with type included
typedef gtsam::LieValues<PoseKey> PoseValues; // config type for poses
typedef gtsam::LieValues<PointKey> PointValues; // config type for points
typedef gtsam::TupleValues2<PoseValues, PointValues> Values; // main config with two variable classes
typedef gtsam::NonlinearFactorGraph<Values> Graph; // graph structure
typedef gtsam::NonlinearOptimizer<Graph,Values,gtsam::GaussianFactorGraph,gtsam::GaussianSequentialSolver> OptimizerSeqential; // optimization engine for this domain
typedef gtsam::NonlinearOptimizer<Graph,Values,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> OptimizerMultifrontal; // optimization engine for this domain
typedef gtsam::Values<PoseKey> PoseValues; // config type for poses
typedef gtsam::Values<PointKey> PointValues; // config type for points
typedef gtsam::TupleValues2<PoseValues, PointValues> PlanarValues; // main config with two variable classes
typedef gtsam::NonlinearFactorGraph<PlanarValues> Graph; // graph structure
typedef gtsam::NonlinearOptimizer<Graph,PlanarValues,gtsam::GaussianFactorGraph,gtsam::GaussianSequentialSolver> OptimizerSeqential; // optimization engine for this domain
typedef gtsam::NonlinearOptimizer<Graph,PlanarValues,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> 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<Values, PoseKey> posePrior(x1, prior_measurement, prior_model); // create the factor
PriorFactor<PlanarValues, PoseKey> 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<Values,PoseKey> odom12(x1, x2, odom_measurement, odom_model);
BetweenFactor<Values,PoseKey> odom23(x2, x3, odom_measurement, odom_model);
BetweenFactor<PlanarValues,PoseKey> odom12(x1, x2, odom_measurement, odom_model);
BetweenFactor<PlanarValues,PoseKey> 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<Values, PoseKey, PointKey> meas11(x1, l1, bearing11, range11, meas_model);
BearingRangeFactor<Values, PoseKey, PointKey> meas21(x2, l1, bearing21, range21, meas_model);
BearingRangeFactor<Values, PoseKey, PointKey> meas32(x3, l2, bearing32, range32, meas_model);
BearingRangeFactor<PlanarValues, PoseKey, PointKey> meas11(x1, l1, bearing11, range11, meas_model);
BearingRangeFactor<PlanarValues, PoseKey, PointKey> meas21(x2, l1, bearing21, range21, meas_model);
BearingRangeFactor<PlanarValues, PoseKey, PointKey> 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<Values> initial(new Values);
boost::shared_ptr<PlanarValues> 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));

4
examples/Pose2SLAMExample_advanced.cpp
View File

@ -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<Values> initial(new Values);
shared_ptr<pose2SLAM::Values> 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 */

4
examples/Pose2SLAMExample_easy.cpp
View File

@ -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, Values>(graph, initial);
pose2SLAM::Values result = optimize<Graph, pose2SLAM::Values>(graph, initial);
result.print("final result");

10
examples/Pose2SLAMwSPCG_advanced.cpp
View File

@ -27,17 +27,17 @@ using namespace gtsam;
using namespace pose2SLAM;
typedef boost::shared_ptr<Graph> sharedGraph ;
typedef boost::shared_ptr<Values> sharedValue ;
typedef boost::shared_ptr<pose2SLAM::Values> sharedValue ;
//typedef NonlinearOptimizer<Graph, Values, SubgraphPreconditioner, SubgraphSolver<Graph,Values> > SPCGOptimizer;
typedef SubgraphSolver<Graph, GaussianFactorGraph, Values> Solver;
typedef SubgraphSolver<Graph, GaussianFactorGraph, pose2SLAM::Values> Solver;
typedef boost::shared_ptr<Solver> sharedSolver ;
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, Solver> SPCGOptimizer;
typedef NonlinearOptimizer<Graph, pose2SLAM::Values, GaussianFactorGraph, Solver> 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<Graph>() ;
initial = boost::make_shared<Values>() ;
initial = boost::make_shared<pose2SLAM::Values>() ;
// create a 3 by 3 grid
// x3 x6 x9

3
examples/Pose2SLAMwSPCG_easy.cpp
View File

@ -27,8 +27,7 @@ using namespace gtsam;
using namespace pose2SLAM;
Graph graph;
Values initial;
Values result;
pose2SLAM::Values initial, result;
/* ************************************************************************* */
int main(void) {

28
examples/SimpleRotation.cpp
View File

@ -23,12 +23,12 @@
#include <gtsam/geometry/Rot2.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
/*
* 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<Rot2, 'x'> Key; /// Variable labels for a specific type
typedef LieValues<Key> Values; /// Class to store values - acts as a state for the system
typedef NonlinearFactorGraph<Values> Graph; /// Graph container for constraints - needs to know type of variables
typedef Values<Key> RotValues; /// Class to store values - acts as a state for the system
typedef NonlinearFactorGraph<RotValues> 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<Values, Key> factor(key, prior, model);
PriorFactor<RotValues, Key> 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, Values>(graph, initial);
RotValues result = optimize<Graph, RotValues>(graph, initial);
result.print("final result");
return 0;

4
examples/easyPoint2KalmanFilter.cpp
View File

@ -24,7 +24,7 @@
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/geometry/Point2.h>
using namespace std;
@ -32,7 +32,7 @@ using namespace gtsam;
// Define Types for Linear System Test
typedef TypedSymbol<Point2, 'x'> LinearKey;
typedef LieValues<LinearKey> LinearValues;
typedef Values<LinearKey> LinearValues;
typedef Point2 LinearMeasurement;
int main() {

32
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -22,7 +22,7 @@
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
@ -35,7 +35,7 @@ using namespace std;
using namespace gtsam;
typedef TypedSymbol<Point2, 'x'> Key; /// Variable labels for a specific type
typedef LieValues<Key> Values; /// Class to store values - acts as a state for the system
typedef Values<Key> 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<Values, Key> factor1(x0, x_initial, P_initial);
PriorFactor<KalmanValues, Key> 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<Values, Key> factor2(x0, x1, difference, Q);
BetweenFactor<KalmanValues, Key> 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<Values, Key> factor4(x1, z1, R1);
PriorFactor<KalmanValues, Key> 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<Values, Key> factor6(x1, x2, difference, Q);
BetweenFactor<KalmanValues, Key> 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<Values, Key> factor8(x2, z2, R2);
PriorFactor<KalmanValues, Key> 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<Values, Key> factor10(x2, x3, difference, Q);
BetweenFactor<KalmanValues, Key> 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<Values, Key> factor12(x3, z3, R3);
PriorFactor<KalmanValues, Key> 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

10
examples/vSLAMexample/vISAMexample.cpp
View File

@ -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<Graph>& newFactors, boost::shared_ptr<Values>& initialValues,
void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<visualSLAM::Values>& initialValues,
int pose_id, Pose3& pose, std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
// Create a graph of newFactors with new measurements
@ -102,7 +102,7 @@ void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>&
}
// Create initial values for all nodes in the newFactors
initialValues = shared_ptr<Values> (new Values());
initialValues = shared_ptr<visualSLAM::Values> (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<Values> isam(relinearizeInterval);
NonlinearISAM<visualSLAM::Values> 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<Graph> newFactors;
shared_ptr<Values> initialValues;
shared_ptr<visualSLAM::Values> 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: ");
}

8
examples/vSLAMexample/vSFMexample.cpp
View File

@ -103,9 +103,9 @@ Graph setupGraph(std::vector<Feature2D>& 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<int, Point3> landmarks, std::map<int, Pose3> poses) {
visualSLAM::Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
Values initValues;
visualSLAM::Values initValues;
// Initialize landmarks 3D positions.
for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
@ -136,7 +136,7 @@ int main(int argc, char* argv[]) {
boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));
// Create an initial Values structure using groundtruth values as the initial estimates
boost::shared_ptr<Values> initialEstimates(new Values(initialize(g_landmarks, g_poses)));
boost::shared_ptr<visualSLAM::Values> 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;

47
gtsam/base/Group.h Normal file
View File

@ -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 T>
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<T>;
#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::GroupConcept<T> _gtsam_GroupConcept_##T;

6
gtsam/base/Lie-inl.h
View File

@ -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<T>;
template T expmap_default(const T&, const Vector&);

113
gtsam/base/Lie.h
View File

@ -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 <string>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/Group.h>
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 T>
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<class T>
inline void print(const T& object, const std::string& s = "") {
object.print(s);
}
/** Call equal on the object */
template<class T>
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<class T>
inline bool equal(const T& obj1, const T& obj2) {
return obj1.equals(obj2);
}
/**
* Three term approximation of the Baker<EFBFBD>Campbell<EFBFBD>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<65>Campbell<6C>Hausdorff_formula
*/
/// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere?
template<class T>
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<class T>
T ExpmapFull(const Vector& xi) { return T::Expmap(xi); }
template<class T>
Vector LogmapFull(const T& p) { return T::Logmap(p); }
/** binary versions */
template<class T>
T expmapFull(const T& t, const Vector& v) { return t.expmap(v); }
template<class T>
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<T>; \
template class gtsam::GroupConcept<T>; \
template class gtsam::LieConcept<T>;
#define GTSAM_CONCEPT_LIE_TYPE(T) \
typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T; \
typedef gtsam::GroupConcept<T> _gtsam_GroupConcept_##T; \
typedef gtsam::LieConcept<T> _gtsam_LieConcept_##T;

62
gtsam/base/LieScalar.h
View File

@ -24,10 +24,10 @@ namespace gtsam {
/**
* LieScalar is a wrapper around double to allow it to be a Lie type
*/
struct LieScalar : public Lie<LieScalar> {
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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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_;

43
gtsam/base/LieVector.h
View File

@ -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<LieVector> {
struct LieVector : public Vector {
/** default constructor - should be unnecessary */
LieVector() {}
@ -57,31 +57,25 @@ struct LieVector : public Vector, public Lie<LieVector> {
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<LieVector> {
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

3
gtsam/base/Makefile.am
View File

@ -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

87
gtsam/base/Manifold.h Normal file
View File

@ -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 <string>
#include <gtsam/base/Matrix.h>
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 T>
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<T>;
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T;

19
gtsam/base/Testable.h
View File

@ -63,6 +63,24 @@ namespace gtsam {
}
};
/** Call print on the object */
template<class T>
inline void print(const T& object, const std::string& s = "") {
object.print(s);
}
/** Call equal on the object */
template<class T>
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<class T>
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<T>;
#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::TestableConcept<T> _gtsam_TestableConcept_##T;

12
gtsam/base/lieProxies.h
View File

@ -36,13 +36,6 @@ namespace testing {
template<class T>
T compose(const T& t1, const T& t2) { return t1.compose(t2); }
/** expmap and logmap */
template<class T>
Vector logmap(const T& t1, const T& t2) { return t1.logmap(t2); }
template<class T>
T expmap(const T& t1, const Vector& t2) { return t1.expmap(t2); }
/** unary functions */
template<class T>
T inverse(const T& t) { return t.inverse(); }
@ -54,6 +47,7 @@ namespace testing {
template<class T, class P>
P unrotate(const T& r, const P& pt) { return r.unrotate(pt); }
}
}
} // \namespace testing
} // \namespace gtsam

28
gtsam/base/numericalDerivative.h
View File

@ -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<n;j++) {
d(j) += delta; double hxplus = h(x.expmap(d));
d(j) -= 2*delta; double hxmin = h(x.expmap(d));
d(j) += delta; double hxplus = h(x.retract(d));
d(j) -= 2*delta; double hxmin = h(x.retract(d));
d(j) += delta; g(j) = (hxplus-hxmin)*factor;
}
return g;
@ -97,8 +97,8 @@ namespace gtsam {
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x.expmap(d)));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x.expmap(d)));
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x.retract(d)));
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x.retract(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -151,8 +151,8 @@ namespace gtsam {
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1.expmap(d),x2));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x1.expmap(d),x2));
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1.retract(d),x2));
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1.retract(d),x2));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -215,8 +215,8 @@ namespace gtsam {
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1,x2.expmap(d)));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x1,x2.expmap(d)));
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1,x2.retract(d)));
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1,x2.retract(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -281,8 +281,8 @@ namespace gtsam {
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1.expmap(d),x2,x3));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x1.expmap(d),x2,x3));
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1.retract(d),x2,x3));
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1.retract(d),x2,x3));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -346,8 +346,8 @@ namespace gtsam {
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1, x2.expmap(d),x3));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x1, x2.expmap(d),x3));
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1, x2.retract(d),x3));
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1, x2.retract(d),x3));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -411,8 +411,8 @@ namespace gtsam {
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1, x2, x3.expmap(d)));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x1, x2, x3.expmap(d)));
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1, x2, x3.retract(d)));
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1, x2, x3.retract(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}

6
gtsam/base/tests/testLieScalar.cpp
View File

@ -17,11 +17,13 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/LieScalar.h>
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)));
}
/* ************************************************************************* */

2
gtsam/base/tests/testLieVector.cpp
View File

@ -17,11 +17,13 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/LieVector.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(LieVector)
GTSAM_CONCEPT_LIE_INST(LieVector)
/* ************************************************************************* */
TEST( testLieVector, construction ) {

27
gtsam/geometry/Cal3Bundler.cpp
View File

@ -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<Matrix&> H1,
boost::optional<Matrix&> 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(); }
}

11
gtsam/geometry/Cal3Bundler.h
View File

@ -44,18 +44,15 @@ public:
bool equals(const Cal3Bundler& K, double tol = 10e-9) const;
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const ;
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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; }

25
gtsam/geometry/Cal3DS2.cpp
View File

@ -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<Matrix&> H1,
boost::optional<Matrix&> 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(); }
}
/* ************************************************************************* */

107
gtsam/geometry/Cal3DS2.h
View File

@ -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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const ;
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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<class Archive>
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<class Archive>
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_);
}
};
};
}

6
gtsam/geometry/Cal3_S2.h
View File

@ -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();
}

122
gtsam/geometry/CalibratedCamera.cpp
View File

@ -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<Matrix&> 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<Matrix&> 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<Matrix&> D_intrinsic_pose,
boost::optional<Matrix&> 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<Matrix&> D_intrinsic_pose,
boost::optional<Matrix&> 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()) ;
}
/* ************************************************************************* */
}

14
gtsam/geometry/CalibratedCamera.h
View File

@ -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 ; }

9
gtsam/geometry/CalibratedCameraT.h
View File

@ -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 <typename Calibration>
@ -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 {

281
gtsam/geometry/GeneralCameraT.h
View File

@ -32,181 +32,164 @@ namespace gtsam {
template <typename Camera, typename Calibration>
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<Point2,bool> projectSafe(
const Point3& P,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
std::pair<Point2,bool> projectSafe(
const Point3& P,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
Point3 cameraPoint = calibrated_.pose().transform_to(P);
return std::pair<Point2, bool>(
project(P,H1,H2),
cameraPoint.z() > 0);
Point3 cameraPoint = calibrated_.pose().transform_to(P);
return std::pair<Point2, bool>(
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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(calibrated_);
ar & BOOST_SERIALIZATION_NVP(calibration_);
}
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(calibrated_);
ar & BOOST_SERIALIZATION_NVP(calibration_);
}
};
};
typedef GeneralCameraT<CalibratedCamera,Cal3Bundler> Cal3BundlerCamera;
typedef GeneralCameraT<CalibratedCamera,Cal3DS2> Cal3DS2Camera;

2
gtsam/geometry/Makefile.am
View File

@ -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

28
gtsam/geometry/Point2.cpp
View File

@ -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

184
gtsam/geometry/Point2.h
View File

@ -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<Point2> {
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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<class ARCHIVE>
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<class ARCHIVE>
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;}
}

118
gtsam/geometry/Point3.cpp
View File

@ -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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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

16
gtsam/geometry/Point3.h
View File

@ -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,

521
gtsam/geometry/Pose2.cpp
View File

@ -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<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1,
boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1,
boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1,
boost::optional<Matrix&> 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<Pose2> align(const vector<Point2Pair>& 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<Matrix&> H1,
boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1,
boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1,
boost::optional<Matrix&> 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<Pose2> align(const vector<Point2Pair>& 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

307
gtsam/geometry/Pose2.h
View File

@ -27,148 +27,155 @@
namespace gtsam {
/**
* A 2D pose (Point2,Rot2)
* @ingroup geometry
*/
class Pose2: public Lie<Pose2> {
/**
* 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<Matrix&> H1=boost::none) const;
/// inverse transformation with derivatives
Pose2 inverse(boost::optional<Matrix&> H1=boost::none) const;
/**
* compose this transformation onto another (first *this and then p2)
* With optional derivatives
*/
Pose2 compose(const Pose2& p2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
/// compose this transformation onto another (first *this and then p2)
Pose2 compose(const Pose2& p2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
/// MATLAB version returns shared pointer
boost::shared_ptr<Pose2> compose_(const Pose2& p2) {
return boost::shared_ptr<Pose2>(new Pose2(compose(p2)));
}
/// MATLAB version returns shared pointer
boost::shared_ptr<Pose2> compose_(const Pose2& p2) {
return boost::shared_ptr<Pose2>(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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Pose2> between_(const Pose2& p2) {
return boost::shared_ptr<Pose2>(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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/// MATLAB version returns shared pointer
boost::shared_ptr<Pose2> between_(const Pose2& p2) {
return boost::shared_ptr<Pose2>(new Pose2(between(p2)));
}
/**
* Return point coordinates in pose coordinate frame
*/
Point2 transform_to(const Point2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Return point coordinates in global frame
*/
Point2 transform_from(const Point2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<class Archive>
void serialize(Archive & ar, const unsigned int version) {
private:
// Serialization function
friend class boost::serialization::access;
template<class Archive>
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<Pose2>(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<Pose2>(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<Point2,Point2> Point2Pair;
boost::optional<Pose2> align(const std::vector<Point2Pair>& 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<Pose2>(const Vector& xi) { return Pose2::ExpmapFull(xi); }
template<>
inline Vector LogmapFull<Pose2>(const Pose2& p) { return Pose2::LogmapFull(p); }
/** binary versions */
template<>
inline Pose2 expmapFull<Pose2>(const Pose2& t, const Vector& v) { return t.expmapFull(v); }
template<>
inline Vector logmapFull<Pose2>(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<Point2,Point2> Point2Pair;
boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
} // namespace gtsam

78
gtsam/geometry/Pose3.cpp
View File

@ -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);
}

277
gtsam/geometry/Pose3.h
View File

@ -23,149 +23,142 @@
namespace gtsam {
/**
* A 3D pose (R,t) : (Rot3,Point3)
* @ingroup geometry
*/
class Pose3 : public Lie<Pose3> {
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<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Derivative of inverse
*/
Pose3 inverse(boost::optional<Matrix&> H1=boost::none) const;
/** receives the point in Pose coordinates and transforms it to world coordinates */
Point3 transform_from(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/**
* composes two poses (first (*this) then p2)
* with optional derivatives
*/
Pose3 compose(const Pose3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** receives the point in world coordinates and transforms it to Pose coordinates */
Point3 transform_to(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
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<class Archive>
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<Pose3>(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<Pose3>(const Vector& xi) { return Pose3::ExpmapFull(xi); }
template<>
inline Vector LogmapFull<Pose3>(const Pose3& p) { return Pose3::LogmapFull(p); }
/** binary versions */
template<>
inline Pose3 expmapFull<Pose3>(const Pose3& t, const Vector& v) { return t.expmapFull(v); }
template<>
inline Vector logmapFull<Pose3>(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<Pose3>(const Vector& xi) {
return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5));
}
} // namespace gtsam

22
gtsam/geometry/Rot2.h
View File

@ -28,7 +28,7 @@ namespace gtsam {
* NOTE: the angle theta is in radians unless explicitly stated
* @ingroup geometry
*/
class Rot2: public Lie<Rot2> {
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<Matrix&> H1 =
boost::none, boost::optional<Matrix&> 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<Matrix&> H1 =

415
gtsam/geometry/Rot3.h
View File

@ -28,245 +28,256 @@ namespace gtsam {
typedef Eigen::Quaterniond Quaternion;
/**
* @brief 3D Rotations represented as rotation matrices
* @ingroup geometry
*/
class Rot3: public Lie<Rot3> {
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<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** Compose two rotations i.e., R= (*this) * R2
*/
Rot3 compose(const Rot3& R2,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> 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<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3()
inline Rot3 inverse(boost::optional<Matrix&> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
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<Matrix,Vector> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/**
* rotate point from world to rotated
* frame = R'*p
*/
Point3 unrotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
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<Matrix,Vector> RQ(const Matrix& A);
}

195
gtsam/geometry/StereoCamera.h
View File

@ -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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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<Matrix&> H1,
boost::optional<Matrix&> H1_k,
boost::optional<Matrix&> 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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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<Matrix&> H1,
boost::optional<Matrix&> H1_k,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<class Archive>
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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(leftCamPose_);
ar & BOOST_SERIALIZATION_NVP(K_);
}
};
};
}

21
gtsam/geometry/StereoPoint2.h
View File

@ -26,7 +26,7 @@ namespace gtsam {
* A 2D stereo point, v will be same for rectified images
* @ingroup geometry
*/
class StereoPoint2: Lie<StereoPoint2> {
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);

3
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -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);

3
gtsam/geometry/tests/testCal3DS2.cpp
View File

@ -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);

1
gtsam/geometry/tests/testCal3_S2.cpp
View File

@ -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);

2
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -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.,

2
gtsam/geometry/tests/testHomography2.cpp
View File

@ -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 ?
}
}

9
gtsam/geometry/tests/testPoint2.cpp
View File

@ -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));
}

7
gtsam/geometry/tests/testPoint3.cpp
View File

@ -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)));
}
/* ************************************************************************* */

67
gtsam/geometry/tests/testPose2.cpp
View File

@ -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<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99));
Pose2 actual = expmap_default<Pose2>(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<Pose2>(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<Pose2>(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<Pose2>(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<Pose2>(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<Pose2>(pose0, pose);
EXPECT(assert_equal(expected, actual, 1e-5));
}

221
gtsam/geometry/tests/testPose3.cpp
View File

@ -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<Pose3>(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<Pose3>(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<Pose3>(id,v), Pose3(R, Point3())));
EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3())));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
CHECK(assert_equal(Pose3(R, P),expmapFull<Pose3>(id,v),1e-5));
EXPECT(assert_equal(Pose3(R, P),expmap_default<Pose3>(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<Pose3>(screw::xi),1e-6));
CHECK(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6));
EXPECT(assert_equal(screw::expected, expm<Pose3>(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<Pose3>(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<Pose3>(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<Pose3>(txi,30), actual,1e-6));
CHECK(assert_equal(Agrawal06iros(txi), actual,1e-6));
actual = Pose3::Retract(txi);
EXPECT(assert_equal(expm<Pose3>(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<Pose3>(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<Pose3>(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<Pose3>(screw::xi),1e-6));
CHECK(assert_equal(screw::expected, Pose3::ExpmapFull(screw::xi),1e-6));
EXPECT(assert_equal(screw::expected, expm<Pose3>(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<Pose3>(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<Pose3>(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<Pose3>(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<Pose3>(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<Pose3>(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<Pose3>(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<Pose3>, T2, T2);
CHECK(assert_equal(numericalH1,actualDcompose1,5e-3));
EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, 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<Pose3>, T1, T2);
CHECK(assert_equal(numericalH1,actualDcompose1,5e-3));
EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, 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<Pose3>, 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<Pose3>, 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<Pose3> , T2, T3);
CHECK(assert_equal(numericalH1,actualDBetween1,5e-3));
EXPECT(assert_equal(numericalH1,actualDBetween1,5e-3));
Matrix numericalH2 = numericalDerivative22(testing::between<Pose3> , 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<Pose3>(x1, x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), expmap_default<Pose3>(x2, x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI_4,0,0), Point3(2,2,0)), expmap_default<Pose3>(x3, sqrt(2) * x_step), tol));
}
/* ************************************************************************* */

7
gtsam/geometry/tests/testRot2.cpp
View File

@ -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));
}

13
gtsam/geometry/tests/testRot3.cpp
View File

@ -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)

3
gtsam/geometry/tests/testStereoCamera.cpp
View File

@ -24,6 +24,9 @@
using namespace std;
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(StereoCamera)
GTSAM_CONCEPT_MANIFOLD_INST(StereoCamera)
/* ************************************************************************* */
TEST( StereoCamera, operators)
{

27
gtsam/geometry/tests/testStereoPoint2.cpp Normal file
View File

@ -0,0 +1,27 @@
/**
* @file testStereoPoint2.cpp
*
* @brief Tests for the StereoPoint2 class
*
* @date Nov 4, 2011
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/StereoPoint2.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2)
GTSAM_CONCEPT_LIE_INST(StereoPoint2)
/* ************************************************************************* */
TEST(testStereoPoint2, test) {
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

2
gtsam/nonlinear/ExtendedKalmanFilter-inl.h
View File

@ -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.

2
gtsam/nonlinear/GaussianISAM2.h
View File

@ -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 <class VALUES>

2
gtsam/nonlinear/ISAM2-impl-inl.h
View File

@ -237,7 +237,7 @@ struct _SelectiveExpmapAndClear {
assert(delta[var].size() == (int)it_x->second.dim());
assert(delta[var].unaryExpr(&isfinite<double>).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<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?)
}

2
gtsam/nonlinear/ISAM2-inl.h
View File

@ -548,7 +548,7 @@ template<class Conditional, class Values>
Values ISAM2<Conditional, Values>::calculateBestEstimate() const {
VectorValues delta(theta_.dims(ordering_));
optimize2(this->root(), delta);
return theta_.expmap(delta, ordering_);
return theta_.retract(delta, ordering_);
}
}

4
gtsam/nonlinear/NonlinearConstraint.h
View File

@ -573,7 +573,7 @@ public:
Vector evaluateError(const X& x1, boost::optional<Matrix&> 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:

2
gtsam/nonlinear/NonlinearEquality.h
View File

@ -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

2
gtsam/nonlinear/NonlinearISAM-inl.h
View File

@ -93,7 +93,7 @@ void NonlinearISAM<Values>::reorder_relinearize() {
template<class Values>
Values NonlinearISAM<Values>::estimate() const {
if(isam_.size() > 0)
return linPoint_.expmap(optimize(isam_), ordering_);
return linPoint_.retract(optimize(isam_), ordering_);
else
return linPoint_;
}

4
gtsam/nonlinear/NonlinearOptimizer-inl.h
View File

@ -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);

2
gtsam/nonlinear/TupleValues-inl.h
View File

@ -18,7 +18,7 @@
#pragma once
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/TupleValues.h>
// TupleValues instantiations for N = 1-6

32
gtsam/nonlinear/TupleValues.h
View File

@ -16,7 +16,7 @@
* @author Alex Cunningham
*/
#include <gtsam/nonlinear/LieValues.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/VectorValues.h>
#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<VALUES1, VALUES2> expmap(const VectorValues& delta, const Ordering& ordering) const {
return TupleValues(first_.expmap(delta, ordering), second_.expmap(delta, ordering));
TupleValues<VALUES1, VALUES2> 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<VALUES1, VALUES2>& cp, const Ordering& ordering) const {
VectorValues localCoordinates(const TupleValues<VALUES1, VALUES2>& 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<VALUES1, VALUES2>& cp, const Ordering& ordering, VectorValues& delta) const {
first_.logmap(cp.first_, ordering, delta);
second_.logmap(cp.second_, ordering, delta);
void localCoordinates(const TupleValues<VALUES1, VALUES2>& 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<VALUES> expmap(const VectorValues& delta, const Ordering& ordering) const {
return TupleValuesEnd(first_.expmap(delta, ordering));
TupleValuesEnd<VALUES> retract(const VectorValues& delta, const Ordering& ordering) const {
return TupleValuesEnd(first_.retract(delta, ordering));
}
VectorValues logmap(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering) const {
VectorValues localCoordinates(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering) const {
VectorValues delta(this->dims(ordering));
logmap(cp, ordering, delta);
localCoordinates(cp, ordering, delta);
return delta;
}
void logmap(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering, VectorValues& delta) const {
first_.logmap(cp.first_, ordering, delta);
void localCoordinates(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering, VectorValues& delta) const {
first_.localCoordinates(cp.first_, ordering, delta);
}
/**

62
gtsam/nonlinear/LieValues-inl.h → gtsam/nonlinear/Values-inl.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file LieValues.cpp
* @file Values.cpp
* @author Richard Roberts
*/
@ -28,9 +28,9 @@
#include <gtsam/base/Lie-inl.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/LieValues.h>
#include <gtsam/nonlinear/Values.h>
#define INSTANTIATE_LIE_VALUES(J) template class LieValues<J>;
#define INSTANTIATE_VALUES(J) template class Values<J>;
using namespace std;
@ -38,8 +38,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
void LieValues<J>::print(const string &s) const {
cout << "LieValues " << s << ", size " << values_.size() << "\n";
void Values<J>::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<class J>
bool LieValues<J>::equals(const LieValues<J>& expected, double tol) const {
bool Values<J>::equals(const Values<J>& 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<class J>
const typename J::Value& LieValues<J>::at(const J& j) const {
const typename J::Value& Values<J>::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<class J>
size_t LieValues<J>::dim() const {
size_t Values<J>::dim() const {
size_t n = 0;
BOOST_FOREACH(const KeyValuePair& value, values_)
n += value.second.dim();
@ -76,26 +76,26 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
VectorValues LieValues<J>::zero(const Ordering& ordering) const {
VectorValues Values<J>::zero(const Ordering& ordering) const {
return VectorValues::Zero(this->dims(ordering));
}
/* ************************************************************************* */
template<class J>
void LieValues<J>::insert(const J& name, const typename J::Value& val) {
void Values<J>::insert(const J& name, const typename J::Value& val) {
values_.insert(make_pair(name, val));
}
/* ************************************************************************* */
template<class J>
void LieValues<J>::insert(const LieValues<J>& cfg) {
void Values<J>::insert(const Values<J>& cfg) {
BOOST_FOREACH(const KeyValuePair& v, cfg.values_)
insert(v.first, v.second);
}
/* ************************************************************************* */
template<class J>
void LieValues<J>::update(const LieValues<J>& cfg) {
void Values<J>::update(const Values<J>& cfg) {
BOOST_FOREACH(const KeyValuePair& v, values_) {
boost::optional<typename J::Value> t = cfg.exists_(v.first);
if (t) values_[v.first] = *t;
@ -104,13 +104,13 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
void LieValues<J>::update(const J& j, const typename J::Value& val) {
void Values<J>::update(const J& j, const typename J::Value& val) {
values_[j] = val;
}
/* ************************************************************************* */
template<class J>
std::list<J> LieValues<J>::keys() const {
std::list<J> Values<J>::keys() const {
std::list<J> ret;
BOOST_FOREACH(const KeyValuePair& v, values_)
ret.push_back(v.first);
@ -119,16 +119,16 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
void LieValues<J>::erase(const J& j) {
void Values<J>::erase(const J& j) {
size_t dim; // unused
erase(j, dim);
}
/* ************************************************************************* */
template<class J>
void LieValues<J>::erase(const J& j, size_t& dim) {
void Values<J>::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<class J>
LieValues<J> LieValues<J>::expmap(const VectorValues& delta, const Ordering& ordering) const {
LieValues<J> newValues;
Values<J> Values<J>::retract(const VectorValues& delta, const Ordering& ordering) const {
Values<J> newValues;
typedef pair<J,typename J::Value> 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<class J>
std::vector<size_t> LieValues<J>::dims(const Ordering& ordering) const {
std::vector<size_t> Values<J>::dims(const Ordering& ordering) const {
_ValuesDimensionCollector dimCollector(ordering);
this->apply(dimCollector);
return dimCollector.dimensions;
@ -161,7 +161,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
Ordering::shared_ptr LieValues<J>::orderingArbitrary(Index firstVar) const {
Ordering::shared_ptr Values<J>::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<class J>
// LieValues<J> LieValues<J>::expmap(const Vector& delta) const {
// Values<J> Values<J>::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<J> newValues;
// Values<J> newValues;
// int delta_offset = 0;
// typedef pair<J,typename J::Value> 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<class J>
inline VectorValues LieValues<J>::logmap(const LieValues<J>& cp, const Ordering& ordering) const {
inline VectorValues Values<J>::localCoordinates(const Values<J>& cp, const Ordering& ordering) const {
VectorValues delta(this->dims(ordering));
logmap(cp, ordering, delta);
localCoordinates(cp, ordering, delta);
return delta;
}
/* ************************************************************************* */
template<class J>
void LieValues<J>::logmap(const LieValues<J>& cp, const Ordering& ordering, VectorValues& delta) const {
void Values<J>::localCoordinates(const Values<J>& cp, const Ordering& ordering, VectorValues& delta) const {
typedef pair<J,typename J::Value> 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);
}
}

46
gtsam/nonlinear/LieValues.h → gtsam/nonlinear/Values.h
View File

@ -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 <gtsam/base/Testable.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/nonlinear/Ordering.h>
namespace boost { template<class T> 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 J>
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<class J_ALT>
LieValues(const LieValues<J_ALT>& other) {} // do nothing when initializing with wrong type
virtual ~LieValues() {}
Values(const Values<J_ALT>& 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<J> keys() const;
/** Replace all keys and variables */
LieValues& operator=(const LieValues& rhs) {
Values& operator=(const Values& rhs) {
values_ = rhs.values_;
return (*this);
}

86
gtsam/nonlinear/tests/testLieValues.cpp → gtsam/nonlinear/tests/testValues.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file testLieValues.cpp
* @file testValues.cpp
* @author Richard Roberts
*/
@ -22,32 +22,32 @@ using namespace boost::assign;
#include <gtsam/base/Testable.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
using namespace gtsam;
using namespace std;
static double inf = std::numeric_limits<double>::infinity();
typedef TypedSymbol<LieVector, 'v'> VecKey;
typedef LieValues<VecKey> Values;
typedef Values<VecKey> 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<string,Pose2> poseconfig;
TestValues<string,Pose2> 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<Pose2, 'x'> PoseKey;
LieValues<PoseKey, Pose2> config;
TestValues<PoseKey, Pose2> 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<LieVector, 'z'> Key;
typedef LieValues<Key> Values1;
typedef Values<Key> 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());

2
gtsam/slam/BetweenConstraint.h
View File

@ -51,7 +51,7 @@ namespace gtsam {
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
X hx = x1.between(x2, H1, H2);
return measured_.logmap(hx);
return measured_.localCoordinates(hx);
}
inline const X measured() const {

2
gtsam/slam/BetweenFactor.h
View File

@ -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 */

2
gtsam/slam/GeneralSFMFactor.h
View File

@ -82,7 +82,7 @@ namespace gtsam {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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;
}

3
gtsam/slam/PartialPriorFactor.h
View File

@ -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<Key> or a TupleValues<...>
* Values where the T's are stored, typically Values<Key> 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<Matrix&> 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;

4
gtsam/slam/PriorFactor.h
View File

@ -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<Key> or a TupleValues<...>
* Values where the T's are stored, typically Values<Key> 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<Matrix&> 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:

7
gtsam/slam/Simulated3D.cpp
View File

@ -16,14 +16,15 @@
**/
#include <gtsam/slam/Simulated3D.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
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 {

4
gtsam/slam/Simulated3D.h
View File

@ -40,8 +40,8 @@ namespace simulated3D {
typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
typedef LieValues<PoseKey> PoseValues;
typedef LieValues<PointKey> PointValues;
typedef Values<PoseKey> PoseValues;
typedef Values<PointKey> PointValues;
typedef TupleValues2<PoseValues, PointValues> Values;
/**

2
gtsam/slam/dataset.cpp
View File

@ -130,7 +130,7 @@ pair<sharedPose2Graph, sharedPose2Values> 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());

9
gtsam/slam/planarSLAM.cpp
View File

@ -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 {

8
gtsam/slam/planarSLAM.h
View File

@ -41,11 +41,11 @@ namespace gtsam {
/// Typedef for a PointKey with Point2 data and 'l' symbol
typedef TypedSymbol<Point2, 'l'> PointKey;
/// Typedef for LieValues structure with PoseKey type
typedef LieValues<PoseKey> PoseValues;
/// Typedef for Values structure with PoseKey type
typedef Values<PoseKey> PoseValues;
/// Typedef for LieValues structure with PointKey type
typedef LieValues<PointKey> PointValues;
/// Typedef for Values structure with PointKey type
typedef Values<PointKey> PointValues;
/// Values class, inherited from TupleValues2, using PoseKeys and PointKeys
struct Values: public TupleValues2<PoseValues, PointValues> {

11
gtsam/slam/pose2SLAM.cpp
View File

@ -16,18 +16,17 @@
**/
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
// 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<Graph, Values, GaussianFactorGraph, GaussianSequentialSolver>;
INSTANTIATE_VALUES(pose2SLAM::Key)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(pose2SLAM::Values)
INSTANTIATE_NONLINEAR_OPTIMIZER(pose2SLAM::Graph, pose2SLAM::Values)
template class NonlinearOptimizer<pose2SLAM::Graph, pose2SLAM::Values, GaussianFactorGraph, GaussianSequentialSolver>;
namespace pose2SLAM {

6
gtsam/slam/pose2SLAM.h
View File

@ -19,7 +19,7 @@
#include <gtsam/base/LieVector.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LieValues.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Key.h>
@ -37,8 +37,8 @@ namespace gtsam {
/// Keys with Pose2 and symbol 'x'
typedef TypedSymbol<Pose2, 'x'> Key;
/// LieValues with type 'Key'
typedef LieValues<Key> Values;
/// Values with type 'Key'
typedef Values<Key> Values;
/**
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)

9
gtsam/slam/pose3SLAM.cpp
View File

@ -16,17 +16,16 @@
**/
#include <gtsam/slam/pose3SLAM.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
// 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 {

6
gtsam/slam/pose3SLAM.h
View File

@ -18,7 +18,7 @@
#pragma once
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/LieValues.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Key.h>
@ -33,8 +33,8 @@ namespace gtsam {
/// Creates a Key with data Pose3 and symbol 'x'
typedef TypedSymbol<Pose3, 'x'> Key;
/// Creates a LieValues structure with type 'Key'
typedef LieValues<Key> Values;
/// Creates a Values structure with type 'Key'
typedef Values<Key> Values;
/**
* Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)

5
gtsam/slam/simulated2D.cpp
View File

@ -16,14 +16,15 @@
*/
#include <gtsam/slam/simulated2D.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
namespace gtsam {
INSTANTIATE_VALUES(simulated2D::PoseKey)
using namespace simulated2D;
INSTANTIATE_LIE_VALUES(PoseKey)
INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues)
namespace simulated2D {

4
gtsam/slam/simulated2D.h
View File

@ -31,8 +31,8 @@ namespace gtsam {
// Simulated2D robots have no orientation, just a position
typedef TypedSymbol<Point2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef LieValues<PoseKey> PoseValues;
typedef LieValues<PointKey> PointValues;
typedef Values<PoseKey> PoseValues;
typedef Values<PointKey> PointValues;
/**
* Custom Values class that holds poses and points

8
gtsam/slam/simulated2DOriented.h
View File

@ -31,8 +31,8 @@ namespace gtsam {
// The types that take an oriented pose2 rather than point2
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef LieValues<PoseKey> PoseValues;
typedef LieValues<PointKey> PointValues;
typedef Values<PoseKey> PoseValues;
typedef Values<PointKey> PointValues;
typedef TupleValues2<PoseValues, PointValues> 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<Matrix&> 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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
return z_.logmap(odo(x1, x2, H1, H2));
return z_.localCoordinates(odo(x1, x2, H1, H2));
}
};

70
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -33,14 +33,14 @@ typedef Cal3_S2Camera GeneralCamera;
//typedef Cal3BundlerCamera GeneralCamera;
typedef TypedSymbol<GeneralCamera, 'x'> CameraKey;
typedef TypedSymbol<Point3, 'l'> PointKey;
typedef LieValues<CameraKey> CameraConfig;
typedef LieValues<PointKey> PointConfig;
typedef TupleValues2<CameraConfig, PointConfig> Values;
typedef GeneralSFMFactor<Values, CameraKey, PointKey> Projection;
typedef NonlinearEquality<Values, CameraKey> CameraConstraint;
typedef NonlinearEquality<Values, PointKey> Point3Constraint;
typedef Values<CameraKey> CameraConfig;
typedef Values<PointKey> PointConfig;
typedef TupleValues2<CameraConfig, PointConfig> VisualValues;
typedef GeneralSFMFactor<VisualValues, CameraKey, PointKey> Projection;
typedef NonlinearEquality<VisualValues, CameraKey> CameraConstraint;
typedef NonlinearEquality<VisualValues, PointKey> Point3Constraint;
class Graph: public NonlinearFactorGraph<Values> {
class Graph: public NonlinearFactorGraph<VisualValues> {
public:
void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, i, j));
@ -73,7 +73,7 @@ double getGaussian()
return sqrt(-2.0f * (double)log(S) / S) * V1;
}
typedef NonlinearOptimizer<Graph,Values> Optimizer;
typedef NonlinearOptimizer<Graph,VisualValues> Optimizer;
const SharedNoiseModel sigma1(noiseModel::Unit::Create(1));
@ -90,7 +90,7 @@ TEST( GeneralSFMFactor, equals )
boost::shared_ptr<Projection>
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<Projection>
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<Point3> genPoint3() {
const double z = 5;
vector<Point3> L ;
@ -174,7 +174,7 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// add initial
const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values);
boost::shared_ptr<VisualValues> 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> 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<Point3> L = genPoint3();
@ -220,7 +213,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
// add initial
const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values);
boost::shared_ptr<VisualValues> 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<Point3> L = genPoint3();
@ -268,7 +259,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<Values> values(new Values);
boost::shared_ptr<VisualValues> 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<Point3> L = genPoint3();
@ -314,7 +302,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<Values> values(new Values);
boost::shared_ptr<VisualValues> 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> 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> values(new Values);
boost::shared_ptr<VisualValues> 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); }
/* ************************************************************************* */

70
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -32,14 +32,15 @@ using namespace gtsam;
typedef Cal3BundlerCamera GeneralCamera;
typedef TypedSymbol<GeneralCamera, 'x'> CameraKey;
typedef TypedSymbol<Point3, 'l'> PointKey;
typedef LieValues<CameraKey> CameraConfig;
typedef LieValues<PointKey> PointConfig;
typedef TupleValues2<CameraConfig, PointConfig> Values;
typedef GeneralSFMFactor<Values, CameraKey, PointKey> Projection;
typedef NonlinearEquality<Values, CameraKey> CameraConstraint;
typedef NonlinearEquality<Values, PointKey> Point3Constraint;
typedef Values<CameraKey> CameraConfig;
typedef Values<PointKey> PointConfig;
typedef TupleValues2<CameraConfig, PointConfig> VisualValues;
typedef GeneralSFMFactor<VisualValues, CameraKey, PointKey> Projection;
typedef NonlinearEquality<VisualValues, CameraKey> CameraConstraint;
typedef NonlinearEquality<VisualValues, PointKey> Point3Constraint;
class Graph: public NonlinearFactorGraph<Values> {
/* ************************************************************************* */
class Graph: public NonlinearFactorGraph<VisualValues> {
public:
void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, i, j));
@ -72,7 +73,7 @@ double getGaussian()
return sqrt(-2.0f * (double)log(S) / S) * V1;
}
typedef NonlinearOptimizer<Graph,Values> Optimizer;
typedef NonlinearOptimizer<Graph,VisualValues> Optimizer;
const SharedNoiseModel sigma1(noiseModel::Unit::Create(1));
@ -89,7 +90,7 @@ TEST( GeneralSFMFactor, equals )
boost::shared_ptr<Projection>
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<Projection>
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<Point3> genPoint3() {
const double z = 5;
vector<Point3> L ;
@ -172,7 +174,7 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// add initial
const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values);
boost::shared_ptr<VisualValues> 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> 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<Point3> L = genPoint3();
@ -218,7 +213,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
// add initial
const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values);
boost::shared_ptr<VisualValues> 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<Point3> L = genPoint3();
@ -266,7 +259,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<Values> values(new Values);
boost::shared_ptr<VisualValues> 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<Point3> L = genPoint3();
@ -312,7 +302,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<Values> values(new Values);
boost::shared_ptr<VisualValues> 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> 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> values(new Values);
boost::shared_ptr<VisualValues> 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); }
/* ************************************************************************* */

20
gtsam/slam/tests/testPose2SLAM.cpp
View File

@ -190,8 +190,8 @@ TEST(Pose2Graph, optimizeThreePoses) {
// Create initial config
boost::shared_ptr<Pose2Values> 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> ordering(new Ordering);
@ -230,11 +230,11 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) {
// Create initial config
boost::shared_ptr<Pose2Values> 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> 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)));

42
gtsam/slam/tests/testPose3SLAM.cpp
View File

@ -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<Pose3Values> 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> 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<Values, Key> Partial;
typedef PartialPriorFactor<pose3SLAM::Values, pose3SLAM::Key> 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<Partial>(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<Values, Key> Partial;
typedef PartialPriorFactor<pose3SLAM::Values, pose3SLAM::Key> 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));
}

21
gtsam/slam/tests/testProjectionFactor.cpp
View File

@ -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<ProjectionFactor>
factor(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
boost::shared_ptr<visualSLAM::ProjectionFactor>
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<GaussianFactor> 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<ProjectionFactor>
factor1(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
boost::shared_ptr<visualSLAM::ProjectionFactor>
factor1(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
boost::shared_ptr<ProjectionFactor>
factor2(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
boost::shared_ptr<visualSLAM::ProjectionFactor>
factor2(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
CHECK(assert_equal(*factor1, *factor2));
}

23
gtsam/slam/tests/testSimulated2D.cpp
View File

@ -23,27 +23,26 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/slam/simulated2D.h>
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));
}
/* ************************************************************************* */

30
gtsam/slam/tests/testSimulated2DOriented.cpp
View File

@ -24,26 +24,16 @@
#include <gtsam/slam/simulated2D.h>
#include <gtsam/slam/simulated2DOriented.h>
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<GaussianFactor> lf = factor.linearize(config, *config.orderingArbitrary());
}

17
gtsam/slam/tests/testSimulated3D.cpp
View File

@ -24,14 +24,13 @@
#include <gtsam/slam/Simulated3D.h>
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<Point3, Point3>(boost::bind(prior, _1, boost::none),x);
Matrix numerical = numericalDerivative11<Point3, Point3>(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<Point3, Point3, Point3>(boost::bind(odo, _1, _2, boost::none, boost::none),x1,x2);
simulated3D::odo(x1,x2,H1,H2);
Matrix A1 = numericalDerivative21<Point3, Point3, Point3>(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2);
EXPECT(assert_equal(A1,H1,1e-9));
Matrix A2 = numericalDerivative22<Point3, Point3, Point3>(boost::bind(odo, _1, _2, boost::none, boost::none),x1,x2);
Matrix A2 = numericalDerivative22<Point3, Point3, Point3>(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2);
EXPECT(assert_equal(A2,H2,1e-9));
}

12
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -29,9 +29,7 @@
using namespace std;
using namespace gtsam;
using namespace gtsam::visualSLAM;
using namespace boost;
typedef NonlinearOptimizer<Graph, Values> 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<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5));
boost::shared_ptr<Graph> graph(new Graph());
boost::shared_ptr<visualSLAM::Graph> 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> values(new Values());
boost::shared_ptr<visualSLAM::Values> 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<Graph,Values,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> Optimizer; // optimization engine for this domain
typedef gtsam::NonlinearOptimizer<visualSLAM::Graph,visualSLAM::Values,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> Optimizer; // optimization engine for this domain
double absoluteThreshold = 1e-9;
double relativeThreshold = 1e-5;

37
gtsam/slam/tests/testVSLAM.cpp
View File

@ -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> graph(new Graph(testGraph()));
shared_ptr<visualSLAM::Graph> 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<Values> initialEstimate(new Values);
boost::shared_ptr<visualSLAM::Values> 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> graph(new Graph(testGraph()));
shared_ptr<visualSLAM::Graph> 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<Values> initialEstimate(new Values);
boost::shared_ptr<visualSLAM::Values> 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> graph(new Graph(testGraph()));
shared_ptr<visualSLAM::Graph> 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<Values> initialEstimate(new Values);
boost::shared_ptr<visualSLAM::Values> 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));
}

Some files were not shown because too many files have changed in this diff Show More