All compiled! Only SPCG and linear/SubgraphSolver are not fixed.
Duy-Nguyen Ta
parent
5b5bbfdfff
commit
2db224df3c
|
|
@ -20,7 +20,6 @@
|
|||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/SimpleCamera.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimization.h>
|
||||
|
|
@ -28,13 +27,12 @@
|
|||
using namespace gtsam;
|
||||
|
||||
typedef TypedSymbol<Pose3, 'x'> PoseKey;
|
||||
typedef Values<PoseKey> PoseValues;
|
||||
|
||||
/**
|
||||
* Unary factor for the pose.
|
||||
*/
|
||||
class ResectioningFactor: public NonlinearFactor1<PoseValues, PoseKey> {
|
||||
typedef NonlinearFactor1<PoseValues, PoseKey> Base;
|
||||
class ResectioningFactor: public NonlinearFactor1<PoseKey> {
|
||||
typedef NonlinearFactor1<PoseKey> Base;
|
||||
|
||||
shared_ptrK K_; // camera's intrinsic parameters
|
||||
Point3 P_; // 3D point on the calibration rig
|
||||
|
|
@ -46,6 +44,8 @@ public:
|
|||
Base(model, key), K_(calib), P_(P), p_(p) {
|
||||
}
|
||||
|
||||
virtual ~ResectioningFactor() {}
|
||||
|
||||
virtual Vector evaluateError(const Pose3& X, boost::optional<Matrix&> H =
|
||||
boost::none) const {
|
||||
SimpleCamera camera(*K_, X);
|
||||
|
|
@ -72,7 +72,7 @@ int main(int argc, char* argv[]) {
|
|||
PoseKey X(1);
|
||||
|
||||
/* 1. create graph */
|
||||
NonlinearFactorGraph<PoseValues> graph;
|
||||
NonlinearFactorGraph graph;
|
||||
|
||||
/* 2. add factors to the graph */
|
||||
// add measurement factors
|
||||
|
|
@ -92,14 +92,13 @@ int main(int argc, char* argv[]) {
|
|||
graph.push_back(factor);
|
||||
|
||||
/* 3. Create an initial estimate for the camera pose */
|
||||
PoseValues initial;
|
||||
DynamicValues initial;
|
||||
initial.insert(X, Pose3(Rot3(1.,0.,0.,
|
||||
0.,-1.,0.,
|
||||
0.,0.,-1.), Point3(0.,0.,2.0)));
|
||||
|
||||
/* 4. Optimize the graph using Levenberg-Marquardt*/
|
||||
PoseValues result = optimize<NonlinearFactorGraph<PoseValues> , PoseValues> (
|
||||
graph, initial);
|
||||
DynamicValues result = optimize<NonlinearFactorGraph> (graph, initial);
|
||||
result.print("Final result: ");
|
||||
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -16,8 +16,8 @@ noinst_PROGRAMS += PlanarSLAMExample_easy # Solves SLAM example from tutorial
|
|||
noinst_PROGRAMS += PlanarSLAMSelfContained_advanced # Solves SLAM example from tutorial with all typedefs in the script
|
||||
noinst_PROGRAMS += Pose2SLAMExample_easy # Solves SLAM example from tutorial by using Pose2SLAM and easy optimization interface
|
||||
noinst_PROGRAMS += Pose2SLAMExample_advanced # Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface
|
||||
noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver
|
||||
noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver
|
||||
#noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver
|
||||
#noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver
|
||||
noinst_PROGRAMS += elaboratePoint2KalmanFilter # simple linear Kalman filter on a moving 2D point, but done using factor graphs
|
||||
noinst_PROGRAMS += easyPoint2KalmanFilter # uses the cool generic templated Kalman filter class to do the same
|
||||
noinst_PROGRAMS += CameraResectioning
|
||||
|
|
|
|||
|
|
@ -86,7 +86,7 @@ int main(int argc, char** argv) {
|
|||
initialEstimate.print("initial estimate");
|
||||
|
||||
// optimize using Levenberg-Marquardt optimization with an ordering from colamd
|
||||
planarSLAM::Values result = optimize<Graph, planarSLAM::Values>(graph, initialEstimate);
|
||||
planarSLAM::Values result = optimize<Graph>(graph, initialEstimate);
|
||||
result.print("final result");
|
||||
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -34,7 +34,6 @@
|
|||
#include <gtsam/slam/BearingRangeFactor.h>
|
||||
|
||||
// implementations for structures - needed if self-contained, and these should be included last
|
||||
#include <gtsam/nonlinear/TupleValues.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||
|
|
@ -43,12 +42,9 @@
|
|||
// 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::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
|
||||
typedef gtsam::NonlinearFactorGraph Graph; // graph structure
|
||||
typedef gtsam::NonlinearOptimizer<Graph,gtsam::GaussianFactorGraph,gtsam::GaussianSequentialSolver> OptimizerSeqential; // optimization engine for this domain
|
||||
typedef gtsam::NonlinearOptimizer<Graph,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> OptimizerMultifrontal; // optimization engine for this domain
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -74,7 +70,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<PlanarValues, PoseKey> posePrior(x1, prior_measurement, prior_model); // create the factor
|
||||
PriorFactor<PoseKey> posePrior(x1, prior_measurement, prior_model); // create the factor
|
||||
graph->add(posePrior); // add the factor to the graph
|
||||
|
||||
/* add odometry */
|
||||
|
|
@ -82,8 +78,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<PlanarValues,PoseKey> odom12(x1, x2, odom_measurement, odom_model);
|
||||
BetweenFactor<PlanarValues,PoseKey> odom23(x2, x3, odom_measurement, odom_model);
|
||||
BetweenFactor<PoseKey> odom12(x1, x2, odom_measurement, odom_model);
|
||||
BetweenFactor<PoseKey> odom23(x2, x3, odom_measurement, odom_model);
|
||||
graph->add(odom12); // add both to graph
|
||||
graph->add(odom23);
|
||||
|
||||
|
|
@ -100,9 +96,9 @@ int main(int argc, char** argv) {
|
|||
range32 = 2.0;
|
||||
|
||||
// create bearing/range factors
|
||||
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);
|
||||
BearingRangeFactor<PoseKey, PointKey> meas11(x1, l1, bearing11, range11, meas_model);
|
||||
BearingRangeFactor<PoseKey, PointKey> meas21(x2, l1, bearing21, range21, meas_model);
|
||||
BearingRangeFactor<PoseKey, PointKey> meas32(x3, l2, bearing32, range32, meas_model);
|
||||
|
||||
// add the factors
|
||||
graph->add(meas11);
|
||||
|
|
@ -112,7 +108,7 @@ int main(int argc, char** argv) {
|
|||
graph->print("Full Graph");
|
||||
|
||||
// initialize to noisy points
|
||||
boost::shared_ptr<PlanarValues> initial(new PlanarValues);
|
||||
boost::shared_ptr<DynamicValues> initial(new DynamicValues);
|
||||
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));
|
||||
|
|
|
|||
|
|
@ -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<pose2SLAM::Values> initial(new pose2SLAM::Values);
|
||||
shared_ptr<DynamicValues> initial(new DynamicValues);
|
||||
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();
|
||||
|
||||
pose2SLAM::Values result = *optimizer_result.values();
|
||||
DynamicValues result = *optimizer_result.values();
|
||||
result.print("final result");
|
||||
|
||||
/* Get covariances */
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
pose2SLAM::Values initial;
|
||||
DynamicValues 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 */
|
||||
pose2SLAM::Values result = optimize<Graph, pose2SLAM::Values>(graph, initial);
|
||||
DynamicValues result = optimize<Graph>(graph, initial);
|
||||
result.print("final result");
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@ using namespace gtsam;
|
|||
using namespace pose2SLAM;
|
||||
|
||||
Graph graph;
|
||||
pose2SLAM::Values initial, result;
|
||||
DynamicValues initial, result;
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main(void) {
|
||||
|
|
|
|||
|
|
@ -23,7 +23,6 @@
|
|||
#include <gtsam/geometry/Rot2.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimization.h>
|
||||
|
||||
|
|
@ -53,8 +52,7 @@ using namespace gtsam;
|
|||
* and 2D poses.
|
||||
*/
|
||||
typedef TypedSymbol<Rot2, 'x'> Key; /// Variable labels for a specific type
|
||||
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
|
||||
typedef NonlinearFactorGraph Graph; /// Graph container for constraints - needs to know type of variables
|
||||
|
||||
const double degree = M_PI / 180;
|
||||
|
||||
|
|
@ -83,7 +81,7 @@ int main() {
|
|||
prior.print("goal angle");
|
||||
SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree);
|
||||
Key key(1);
|
||||
PriorFactor<RotValues, Key> factor(key, prior, model);
|
||||
PriorFactor<Key> factor(key, prior, model);
|
||||
|
||||
/**
|
||||
* Step 3: create a graph container and add the factor to it
|
||||
|
|
@ -114,7 +112,7 @@ int main() {
|
|||
* on the type of key used to find the appropriate value map if there
|
||||
* are multiple types of variables.
|
||||
*/
|
||||
RotValues initial;
|
||||
DynamicValues initial;
|
||||
initial.insert(key, Rot2::fromAngle(20 * degree));
|
||||
initial.print("initial estimate");
|
||||
|
||||
|
|
@ -126,7 +124,7 @@ int main() {
|
|||
* initial estimate. This will yield a new RotValues structure
|
||||
* with the final state of the optimization.
|
||||
*/
|
||||
RotValues result = optimize<Graph, RotValues>(graph, initial);
|
||||
DynamicValues result = optimize<Graph>(graph, initial);
|
||||
result.print("final result");
|
||||
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -24,7 +24,6 @@
|
|||
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
using namespace std;
|
||||
|
|
@ -32,7 +31,6 @@ using namespace gtsam;
|
|||
|
||||
// Define Types for Linear System Test
|
||||
typedef TypedSymbol<Point2, 'x'> LinearKey;
|
||||
typedef Values<LinearKey> LinearValues;
|
||||
typedef Point2 LinearMeasurement;
|
||||
|
||||
int main() {
|
||||
|
|
@ -42,7 +40,7 @@ int main() {
|
|||
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
|
||||
|
||||
// Create an ExtendedKalmanFilter object
|
||||
ExtendedKalmanFilter<LinearValues,LinearKey> ekf(x_initial, P_initial);
|
||||
ExtendedKalmanFilter<LinearKey> ekf(x_initial, P_initial);
|
||||
|
||||
|
||||
|
||||
|
|
@ -67,7 +65,7 @@ int main() {
|
|||
// Predict delta based on controls
|
||||
Point2 difference(1,0);
|
||||
// Create Factor
|
||||
BetweenFactor<LinearValues,LinearKey> factor1(x0, x1, difference, Q);
|
||||
BetweenFactor<LinearKey> factor1(x0, x1, difference, Q);
|
||||
|
||||
// Predict the new value with the EKF class
|
||||
Point2 x1_predict = ekf.predict(factor1);
|
||||
|
|
@ -88,7 +86,7 @@ int main() {
|
|||
|
||||
// This simple measurement can be modeled with a PriorFactor
|
||||
Point2 z1(1.0, 0.0);
|
||||
PriorFactor<LinearValues,LinearKey> factor2(x1, z1, R);
|
||||
PriorFactor<LinearKey> factor2(x1, z1, R);
|
||||
|
||||
// Update the Kalman Filter with the measurement
|
||||
Point2 x1_update = ekf.update(factor2);
|
||||
|
|
@ -100,13 +98,13 @@ int main() {
|
|||
// Predict
|
||||
LinearKey x2(2);
|
||||
difference = Point2(1,0);
|
||||
BetweenFactor<LinearValues,LinearKey> factor3(x1, x2, difference, Q);
|
||||
BetweenFactor<LinearKey> factor3(x1, x2, difference, Q);
|
||||
Point2 x2_predict = ekf.predict(factor1);
|
||||
x2_predict.print("X2 Predict");
|
||||
|
||||
// Update
|
||||
Point2 z2(2.0, 0.0);
|
||||
PriorFactor<LinearValues,LinearKey> factor4(x2, z2, R);
|
||||
PriorFactor<LinearKey> factor4(x2, z2, R);
|
||||
Point2 x2_update = ekf.update(factor4);
|
||||
x2_update.print("X2 Update");
|
||||
|
||||
|
|
@ -116,13 +114,13 @@ int main() {
|
|||
// Predict
|
||||
LinearKey x3(3);
|
||||
difference = Point2(1,0);
|
||||
BetweenFactor<LinearValues,LinearKey> factor5(x2, x3, difference, Q);
|
||||
BetweenFactor<LinearKey> factor5(x2, x3, difference, Q);
|
||||
Point2 x3_predict = ekf.predict(factor5);
|
||||
x3_predict.print("X3 Predict");
|
||||
|
||||
// Update
|
||||
Point2 z3(3.0, 0.0);
|
||||
PriorFactor<LinearValues,LinearKey> factor6(x3, z3, R);
|
||||
PriorFactor<LinearKey> factor6(x3, z3, R);
|
||||
Point2 x3_update = ekf.update(factor6);
|
||||
x3_update.print("X3 Update");
|
||||
|
||||
|
|
|
|||
|
|
@ -22,7 +22,6 @@
|
|||
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||
|
|
@ -35,7 +34,6 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
|
||||
typedef TypedSymbol<Point2, 'x'> Key; /// Variable labels for a specific type
|
||||
typedef Values<Key> KalmanValues; /// Class to store values - acts as a state for the system
|
||||
|
||||
int main() {
|
||||
|
||||
|
|
@ -48,7 +46,7 @@ int main() {
|
|||
Ordering::shared_ptr ordering(new Ordering);
|
||||
|
||||
// Create a structure to hold the linearization points
|
||||
KalmanValues linearizationPoints;
|
||||
DynamicValues linearizationPoints;
|
||||
|
||||
// Ground truth example
|
||||
// Start at origin, move to the right (x-axis): 0,0 0,1 0,2
|
||||
|
|
@ -64,7 +62,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<KalmanValues, Key> factor1(x0, x_initial, P_initial);
|
||||
PriorFactor<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 +93,7 @@ int main() {
|
|||
|
||||
Point2 difference(1,0);
|
||||
SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
|
||||
BetweenFactor<KalmanValues, Key> factor2(x0, x1, difference, Q);
|
||||
BetweenFactor<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));
|
||||
|
|
@ -173,7 +171,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<KalmanValues, Key> factor4(x1, z1, R1);
|
||||
PriorFactor<Key> factor4(x1, z1, R1);
|
||||
// Linearize the factor and add it to the linear factor graph
|
||||
linearFactorGraph->push_back(factor4.linearize(linearizationPoints, *ordering));
|
||||
|
||||
|
|
@ -225,7 +223,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<KalmanValues, Key> factor6(x1, x2, difference, Q);
|
||||
BetweenFactor<Key> factor6(x1, x2, difference, Q);
|
||||
|
||||
// Linearize the factor and add it to the linear factor graph
|
||||
linearizationPoints.insert(x2, x1_update);
|
||||
|
|
@ -263,7 +261,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<KalmanValues, Key> factor8(x2, z2, R2);
|
||||
PriorFactor<Key> factor8(x2, z2, R2);
|
||||
|
||||
// Linearize the factor and add it to the linear factor graph
|
||||
linearFactorGraph->push_back(factor8.linearize(linearizationPoints, *ordering));
|
||||
|
|
@ -314,7 +312,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<KalmanValues, Key> factor10(x2, x3, difference, Q);
|
||||
BetweenFactor<Key> factor10(x2, x3, difference, Q);
|
||||
|
||||
// Linearize the factor and add it to the linear factor graph
|
||||
linearizationPoints.insert(x3, x2_update);
|
||||
|
|
@ -352,7 +350,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<KalmanValues, Key> factor12(x3, z3, R3);
|
||||
PriorFactor<Key> factor12(x3, z3, R3);
|
||||
|
||||
// Linearize the factor and add it to the linear factor graph
|
||||
linearFactorGraph->push_back(factor12.linearize(linearizationPoints, *ordering));
|
||||
|
|
|
|||
|
|
@ -80,7 +80,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<visualSLAM::Values>& initialValues,
|
||||
void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<DynamicValues>& initialValues,
|
||||
int pose_id, const Pose3& pose, const std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
|
||||
|
||||
// Create a graph of newFactors with new measurements
|
||||
|
|
@ -101,10 +101,10 @@ void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<visualSLA
|
|||
}
|
||||
|
||||
// Create initial values for all nodes in the newFactors
|
||||
initialValues = shared_ptr<visualSLAM::Values> (new visualSLAM::Values());
|
||||
initialValues->insert(pose_id, pose);
|
||||
initialValues = shared_ptr<DynamicValues> (new DynamicValues());
|
||||
initialValues->insert(PoseKey(pose_id), pose);
|
||||
for (size_t i = 0; i < measurements.size(); i++) {
|
||||
initialValues->insert(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]);
|
||||
initialValues->insert(PointKey(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -124,7 +124,7 @@ int main(int argc, char* argv[]) {
|
|||
|
||||
// Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates
|
||||
int relinearizeInterval = 3;
|
||||
NonlinearISAM<visualSLAM::Values> isam(relinearizeInterval);
|
||||
NonlinearISAM<> isam(relinearizeInterval);
|
||||
|
||||
// At each frame (poseId) with new camera pose and set of associated measurements,
|
||||
// create a graph of new factors and update ISAM
|
||||
|
|
@ -132,12 +132,12 @@ int main(int argc, char* argv[]) {
|
|||
BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) {
|
||||
const int poseId = features.first;
|
||||
shared_ptr<Graph> newFactors;
|
||||
shared_ptr<visualSLAM::Values> initialValues;
|
||||
shared_ptr<DynamicValues> initialValues;
|
||||
createNewFactors(newFactors, initialValues, poseId, g_poses[poseId],
|
||||
features.second, measurementSigma, g_calib);
|
||||
|
||||
isam.update(*newFactors, *initialValues);
|
||||
visualSLAM::Values currentEstimate = isam.estimate();
|
||||
DynamicValues currentEstimate = isam.estimate();
|
||||
cout << "****************************************************" << endl;
|
||||
currentEstimate.print("Current estimate: ");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -103,17 +103,17 @@ 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.
|
||||
*/
|
||||
visualSLAM::Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
|
||||
DynamicValues initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
|
||||
|
||||
visualSLAM::Values initValues;
|
||||
DynamicValues initValues;
|
||||
|
||||
// Initialize landmarks 3D positions.
|
||||
for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
|
||||
initValues.insert(lmit->first, lmit->second);
|
||||
initValues.insert(PointKey(lmit->first), lmit->second);
|
||||
|
||||
// Initialize camera poses.
|
||||
for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++)
|
||||
initValues.insert(poseit->first, poseit->second);
|
||||
initValues.insert(PoseKey(poseit->first), poseit->second);
|
||||
|
||||
return initValues;
|
||||
}
|
||||
|
|
@ -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<visualSLAM::Values> initialEstimates(new visualSLAM::Values(initialize(g_landmarks, g_poses)));
|
||||
boost::shared_ptr<DynamicValues> initialEstimates(new DynamicValues(initialize(g_landmarks, g_poses)));
|
||||
cout << "*******************************************************" << endl;
|
||||
initialEstimates->print("INITIAL ESTIMATES: ");
|
||||
|
||||
|
|
|
|||
|
|
@ -20,8 +20,8 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
template<class GRAPH, class LINEAR, class VALUES>
|
||||
void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
|
||||
template<class GRAPH, class LINEAR, class KEY>
|
||||
void SubgraphSolver<GRAPH,LINEAR,KEY>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
|
||||
|
||||
GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>();
|
||||
GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared<GaussianFactorGraph>();
|
||||
|
|
@ -46,8 +46,8 @@ void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::
|
|||
Ab1->dynamicCastFactors<FactorGraph<JacobianFactor> >(), Ab2->dynamicCastFactors<FactorGraph<JacobianFactor> >(),Rc1,xbar);
|
||||
}
|
||||
|
||||
template<class GRAPH, class LINEAR, class VALUES>
|
||||
VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,VALUES>::optimize() const {
|
||||
template<class GRAPH, class LINEAR, class KEY>
|
||||
VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,KEY>::optimize() const {
|
||||
|
||||
// preconditioned conjugate gradient
|
||||
VectorValues zeros = pc_->zero();
|
||||
|
|
@ -59,18 +59,18 @@ VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,VALUES>::optimize() const {
|
|||
return xbar;
|
||||
}
|
||||
|
||||
template<class GRAPH, class LINEAR, class VALUES>
|
||||
void SubgraphSolver<GRAPH,LINEAR,VALUES>::initialize(const GRAPH& G, const VALUES& theta0) {
|
||||
template<class GRAPH, class LINEAR, class KEY>
|
||||
void SubgraphSolver<GRAPH,LINEAR,KEY>::initialize(const GRAPH& G, const DynamicValues& theta0) {
|
||||
// generate spanning tree
|
||||
PredecessorMap<Key> tree_ = gtsam::findMinimumSpanningTree<GRAPH, Key, Constraint>(G);
|
||||
PredecessorMap<KEY> tree_ = gtsam::findMinimumSpanningTree<GRAPH, KEY, Constraint>(G);
|
||||
|
||||
// make the ordering
|
||||
list<Key> keys = predecessorMap2Keys(tree_);
|
||||
list<KEY> keys = predecessorMap2Keys(tree_);
|
||||
ordering_ = boost::make_shared<Ordering>(list<Symbol>(keys.begin(), keys.end()));
|
||||
|
||||
// build factor pairs
|
||||
pairs_.clear();
|
||||
typedef pair<Key,Key> EG ;
|
||||
typedef pair<KEY,KEY> EG ;
|
||||
BOOST_FOREACH( const EG &eg, tree_ ) {
|
||||
Symbol key1 = Symbol(eg.first),
|
||||
key2 = Symbol(eg.second) ;
|
||||
|
|
|
|||
|
|
@ -16,6 +16,7 @@
|
|||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/IterativeSolver.h>
|
||||
#include <gtsam/linear/SubgraphPreconditioner.h>
|
||||
#include <gtsam/nonlinear/DynamicValues.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -31,11 +32,11 @@ bool split(const std::map<Index, Index> &M,
|
|||
* linearize: G * T -> L
|
||||
* solve : L -> VectorValues
|
||||
*/
|
||||
template<class GRAPH, class LINEAR, class VALUES>
|
||||
template<class GRAPH, class LINEAR, class KEY>
|
||||
class SubgraphSolver : public IterativeSolver {
|
||||
|
||||
private:
|
||||
typedef typename VALUES::Key Key;
|
||||
// typedef typename VALUES::Key Key;
|
||||
typedef typename GRAPH::Pose Pose;
|
||||
typedef typename GRAPH::Constraint Constraint;
|
||||
|
||||
|
|
@ -43,7 +44,7 @@ private:
|
|||
typedef boost::shared_ptr<Ordering> shared_ordering ;
|
||||
typedef boost::shared_ptr<GRAPH> shared_graph ;
|
||||
typedef boost::shared_ptr<LINEAR> shared_linear ;
|
||||
typedef boost::shared_ptr<VALUES> shared_values ;
|
||||
typedef boost::shared_ptr<DynamicValues> shared_values ;
|
||||
typedef boost::shared_ptr<SubgraphPreconditioner> shared_preconditioner ;
|
||||
typedef std::map<Index,Index> mapPairIndex ;
|
||||
|
||||
|
|
@ -61,7 +62,7 @@ private:
|
|||
|
||||
public:
|
||||
|
||||
SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters ¶meters = Parameters(), bool useQR = false):
|
||||
SubgraphSolver(const GRAPH& G, const DynamicValues& theta0, const Parameters ¶meters = Parameters(), bool useQR = false):
|
||||
IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); }
|
||||
|
||||
SubgraphSolver(const LINEAR& GFG) {
|
||||
|
|
@ -89,7 +90,7 @@ public:
|
|||
shared_ordering ordering() const { return ordering_; }
|
||||
|
||||
protected:
|
||||
void initialize(const GRAPH& G, const VALUES& theta0);
|
||||
void initialize(const GRAPH& G, const DynamicValues& theta0);
|
||||
|
||||
private:
|
||||
SubgraphSolver():IterativeSolver(){}
|
||||
|
|
|
|||
|
|
@ -27,10 +27,10 @@
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES, class KEY>
|
||||
typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::solve_(
|
||||
template<class KEY>
|
||||
typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::solve_(
|
||||
const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering,
|
||||
const VALUES& linearizationPoints, const KEY& lastKey,
|
||||
const DynamicValues& linearizationPoints, const KEY& lastKey,
|
||||
JacobianFactor::shared_ptr& newPrior) const {
|
||||
|
||||
// Extract the Index of the provided last key
|
||||
|
|
@ -66,8 +66,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES, class KEY>
|
||||
ExtendedKalmanFilter<VALUES, KEY>::ExtendedKalmanFilter(T x_initial,
|
||||
template<class KEY>
|
||||
ExtendedKalmanFilter<KEY>::ExtendedKalmanFilter(T x_initial,
|
||||
noiseModel::Gaussian::shared_ptr P_initial) {
|
||||
|
||||
// Set the initial linearization point to the provided mean
|
||||
|
|
@ -82,8 +82,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES, class KEY>
|
||||
typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::predict(
|
||||
template<class KEY>
|
||||
typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::predict(
|
||||
const MotionFactor& motionFactor) {
|
||||
|
||||
// TODO: This implementation largely ignores the actual factor symbols.
|
||||
|
|
@ -100,7 +100,7 @@ namespace gtsam {
|
|||
ordering.insert(x1, 1);
|
||||
|
||||
// Create a set of linearization points
|
||||
VALUES linearizationPoints;
|
||||
DynamicValues linearizationPoints;
|
||||
linearizationPoints.insert(x0, x_);
|
||||
linearizationPoints.insert(x1, x_); // TODO should this really be x_ ?
|
||||
|
||||
|
|
@ -122,8 +122,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES, class KEY>
|
||||
typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::update(
|
||||
template<class KEY>
|
||||
typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::update(
|
||||
const MeasurementFactor& measurementFactor) {
|
||||
|
||||
// TODO: This implementation largely ignores the actual factor symbols.
|
||||
|
|
@ -138,7 +138,7 @@ namespace gtsam {
|
|||
ordering.insert(x0, 0);
|
||||
|
||||
// Create a set of linearization points
|
||||
VALUES linearizationPoints;
|
||||
DynamicValues linearizationPoints;
|
||||
linearizationPoints.insert(x0, x_);
|
||||
|
||||
// Create a Gaussian Factor Graph
|
||||
|
|
|
|||
|
|
@ -39,21 +39,21 @@ namespace gtsam {
|
|||
* TODO: a "predictAndUpdate" that combines both steps for some computational savings.
|
||||
*/
|
||||
|
||||
template<class VALUES, class KEY>
|
||||
template<class KEY>
|
||||
class ExtendedKalmanFilter {
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<ExtendedKalmanFilter<VALUES, KEY> > shared_ptr;
|
||||
typedef boost::shared_ptr<ExtendedKalmanFilter<KEY> > shared_ptr;
|
||||
typedef typename KEY::Value T;
|
||||
typedef NonlinearFactor2<VALUES, KEY, KEY> MotionFactor;
|
||||
typedef NonlinearFactor1<VALUES, KEY> MeasurementFactor;
|
||||
typedef NonlinearFactor2<KEY, KEY> MotionFactor;
|
||||
typedef NonlinearFactor1<KEY> MeasurementFactor;
|
||||
|
||||
protected:
|
||||
T x_; // linearization point
|
||||
JacobianFactor::shared_ptr priorFactor_; // density
|
||||
|
||||
T solve_(const GaussianFactorGraph& linearFactorGraph,
|
||||
const Ordering& ordering, const VALUES& linearizationPoints,
|
||||
const Ordering& ordering, const DynamicValues& linearizationPoints,
|
||||
const KEY& x, JacobianFactor::shared_ptr& newPrior) const;
|
||||
|
||||
public:
|
||||
|
|
|
|||
|
|
@ -29,9 +29,9 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES, class GRAPH>
|
||||
void NonlinearISAM<VALUES,GRAPH>::update(const Factors& newFactors,
|
||||
const Values& initialValues) {
|
||||
template<class GRAPH>
|
||||
void NonlinearISAM<GRAPH>::update(const Factors& newFactors,
|
||||
const DynamicValues& initialValues) {
|
||||
|
||||
if(newFactors.size() > 0) {
|
||||
|
||||
|
|
@ -62,14 +62,14 @@ void NonlinearISAM<VALUES,GRAPH>::update(const Factors& newFactors,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES, class GRAPH>
|
||||
void NonlinearISAM<VALUES,GRAPH>::reorder_relinearize() {
|
||||
template<class GRAPH>
|
||||
void NonlinearISAM<GRAPH>::reorder_relinearize() {
|
||||
|
||||
// cout << "Reordering, relinearizing..." << endl;
|
||||
|
||||
if(factors_.size() > 0) {
|
||||
// Obtain the new linearization point
|
||||
const Values newLinPoint = estimate();
|
||||
const DynamicValues newLinPoint = estimate();
|
||||
|
||||
isam_.clear();
|
||||
|
||||
|
|
@ -89,8 +89,8 @@ void NonlinearISAM<VALUES,GRAPH>::reorder_relinearize() {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES, class GRAPH>
|
||||
VALUES NonlinearISAM<VALUES,GRAPH>::estimate() const {
|
||||
template<class GRAPH>
|
||||
DynamicValues NonlinearISAM<GRAPH>::estimate() const {
|
||||
if(isam_.size() > 0)
|
||||
return linPoint_.retract(optimize(isam_), ordering_);
|
||||
else
|
||||
|
|
@ -98,14 +98,14 @@ VALUES NonlinearISAM<VALUES,GRAPH>::estimate() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES, class GRAPH>
|
||||
Matrix NonlinearISAM<VALUES,GRAPH>::marginalCovariance(const Symbol& key) const {
|
||||
template<class GRAPH>
|
||||
Matrix NonlinearISAM<GRAPH>::marginalCovariance(const Symbol& key) const {
|
||||
return isam_.marginalCovariance(ordering_[key]);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES, class GRAPH>
|
||||
void NonlinearISAM<VALUES,GRAPH>::print(const std::string& s) const {
|
||||
template<class GRAPH>
|
||||
void NonlinearISAM<GRAPH>::print(const std::string& s) const {
|
||||
cout << "ISAM - " << s << ":" << endl;
|
||||
cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl;
|
||||
isam_.print("GaussianISAM");
|
||||
|
|
|
|||
|
|
@ -24,11 +24,10 @@ namespace gtsam {
|
|||
/**
|
||||
* Wrapper class to manage ISAM in a nonlinear context
|
||||
*/
|
||||
template<class VALUES, class GRAPH = gtsam::NonlinearFactorGraph<VALUES> >
|
||||
template<class GRAPH = gtsam::NonlinearFactorGraph >
|
||||
class NonlinearISAM {
|
||||
public:
|
||||
|
||||
typedef VALUES Values;
|
||||
typedef GRAPH Factors;
|
||||
|
||||
protected:
|
||||
|
|
@ -37,7 +36,7 @@ protected:
|
|||
gtsam::GaussianISAM isam_;
|
||||
|
||||
/** The current linearization point */
|
||||
Values linPoint_;
|
||||
DynamicValues linPoint_;
|
||||
|
||||
/** The ordering */
|
||||
gtsam::Ordering ordering_;
|
||||
|
|
@ -61,10 +60,10 @@ public:
|
|||
NonlinearISAM(int reorderInterval = 1) : reorderInterval_(reorderInterval), reorderCounter_(0) {}
|
||||
|
||||
/** Add new factors along with their initial linearization points */
|
||||
void update(const Factors& newFactors, const Values& initialValues);
|
||||
void update(const Factors& newFactors, const DynamicValues& initialValues);
|
||||
|
||||
/** Return the current solution estimate */
|
||||
Values estimate() const;
|
||||
DynamicValues estimate() const;
|
||||
|
||||
/** Relinearization and reordering of variables */
|
||||
void reorder_relinearize();
|
||||
|
|
@ -84,7 +83,7 @@ public:
|
|||
const GaussianISAM& bayesTree() const { return isam_; }
|
||||
|
||||
/** Return the current linearization point */
|
||||
const Values& getLinearizationPoint() const { return linPoint_; }
|
||||
const DynamicValues& getLinearizationPoint() const { return linPoint_; }
|
||||
|
||||
/** Get the ordering */
|
||||
const gtsam::Ordering& getOrdering() const { return ordering_; }
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@ namespace gtsam {
|
|||
Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
|
||||
|
||||
// Create an nonlinear Optimizer that uses a Sequential Solver
|
||||
typedef NonlinearOptimizer<G, DynamicValues, GaussianFactorGraph, GaussianSequentialSolver> Optimizer;
|
||||
typedef NonlinearOptimizer<G, GaussianFactorGraph, GaussianSequentialSolver> Optimizer;
|
||||
Optimizer optimizer(boost::make_shared<const G>(graph),
|
||||
boost::make_shared<const DynamicValues>(initialEstimate), ordering,
|
||||
boost::make_shared<NonlinearOptimizationParameters>(parameters));
|
||||
|
|
@ -62,7 +62,7 @@ namespace gtsam {
|
|||
Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
|
||||
|
||||
// Create an nonlinear Optimizer that uses a Multifrontal Solver
|
||||
typedef NonlinearOptimizer<G, DynamicValues, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
|
||||
typedef NonlinearOptimizer<G, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
|
||||
Optimizer optimizer(boost::make_shared<const G>(graph),
|
||||
boost::make_shared<const DynamicValues>(initialEstimate), ordering,
|
||||
boost::make_shared<NonlinearOptimizationParameters>(parameters));
|
||||
|
|
@ -85,7 +85,7 @@ namespace gtsam {
|
|||
// initial optimization state is the same in both cases tested
|
||||
typedef SubgraphSolver<G,GaussianFactorGraph,DynamicValues> Solver;
|
||||
typedef boost::shared_ptr<Solver> shared_Solver;
|
||||
typedef NonlinearOptimizer<G, DynamicValues, GaussianFactorGraph, Solver> SPCGOptimizer;
|
||||
typedef NonlinearOptimizer<G, GaussianFactorGraph, Solver> SPCGOptimizer;
|
||||
shared_Solver solver = boost::make_shared<Solver>(
|
||||
graph, initialEstimate, IterativeOptimizationParameters());
|
||||
SPCGOptimizer optimizer(
|
||||
|
|
@ -111,10 +111,10 @@ namespace gtsam {
|
|||
const NonlinearOptimizationMethod& nonlinear_method) {
|
||||
switch (solver) {
|
||||
case SEQUENTIAL:
|
||||
return optimizeSequential<G,DynamicValues>(graph, initialEstimate, parameters,
|
||||
return optimizeSequential<G>(graph, initialEstimate, parameters,
|
||||
nonlinear_method == LM);
|
||||
case MULTIFRONTAL:
|
||||
return optimizeMultiFrontal<G,DynamicValues>(graph, initialEstimate, parameters,
|
||||
return optimizeMultiFrontal<G>(graph, initialEstimate, parameters,
|
||||
nonlinear_method == LM);
|
||||
case SPCG:
|
||||
// return optimizeSPCG<G,DynamicValues>(graph, initialEstimate, parameters,
|
||||
|
|
|
|||
|
|
@ -74,10 +74,10 @@ namespace gtsam {
|
|||
};
|
||||
|
||||
/// The sequential optimizer
|
||||
typedef NonlinearOptimizer<Graph, DynamicValues, GaussianFactorGraph, GaussianSequentialSolver> OptimizerSequential;
|
||||
typedef NonlinearOptimizer<Graph, GaussianFactorGraph, GaussianSequentialSolver> OptimizerSequential;
|
||||
|
||||
/// The multifrontal optimizer
|
||||
typedef NonlinearOptimizer<Graph, DynamicValues, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
|
||||
typedef NonlinearOptimizer<Graph, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
|
||||
|
||||
} // pose2SLAM
|
||||
|
||||
|
|
|
|||
|
|
@ -44,7 +44,7 @@ int main(int argc, char *argv[]) {
|
|||
else if (argc == 4)
|
||||
nrTrials = strtoul(argv[3], NULL, 10);
|
||||
|
||||
pair<shared_ptr<Pose2Graph>, shared_ptr<Pose2Values> > data = load2D(dataset(datasetname));
|
||||
pair<shared_ptr<Pose2Graph>, shared_ptr<DynamicValues> > data = load2D(dataset(datasetname));
|
||||
|
||||
// Add a prior on the first pose
|
||||
if (soft_prior)
|
||||
|
|
|
|||
|
|
@ -44,7 +44,7 @@ int main(int argc, char *argv[]) {
|
|||
else if (argc == 4)
|
||||
nrTrials = strtoul(argv[3], NULL, 10);
|
||||
|
||||
pair<shared_ptr<Pose2Graph>, shared_ptr<Pose2Values> > data = load2D(dataset(datasetname));
|
||||
pair<shared_ptr<Pose2Graph>, shared_ptr<DynamicValues> > data = load2D(dataset(datasetname));
|
||||
|
||||
// Add a prior on the first pose
|
||||
if (soft_prior)
|
||||
|
|
|
|||
Loading…
Reference in New Issue