Merged changes from the trunk back into examples. Mostly just Vector inits and some new examples.
Frank Dellaert
parent
2778f2fe39
commit
2601b2ad00
|
|
@ -4,6 +4,16 @@ endif()
|
|||
|
||||
# Build example executables
|
||||
FILE(GLOB example_srcs "*.cpp")
|
||||
|
||||
set (excluded_examples #"")
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/DiscreteBayesNet_FG.cpp"
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/UGM_chain.cpp"
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/UGM_small.cpp"
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/elaboratePoint2KalmanFilter.cpp"
|
||||
)
|
||||
|
||||
list(REMOVE_ITEM example_srcs ${excluded_examples})
|
||||
|
||||
foreach(example_src ${example_srcs} )
|
||||
get_filename_component(example_base ${example_src} NAME_WE)
|
||||
set( example_bin ${example_base} )
|
||||
|
|
|
|||
|
|
@ -70,7 +70,7 @@ int main(int argc, char* argv[]) {
|
|||
|
||||
/* 2. add factors to the graph */
|
||||
// add measurement factors
|
||||
SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector_(2, 0.5, 0.5));
|
||||
SharedDiagonal measurementNoise = Diagonal::Sigmas((Vector(2) << 0.5, 0.5));
|
||||
boost::shared_ptr<ResectioningFactor> factor;
|
||||
graph.push_back(
|
||||
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
|
||||
|
|
|
|||
|
|
@ -0,0 +1,71 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 CreateSFMExampleData.cpp
|
||||
* @brief Create some example data that for inclusion in the data folder
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/geometry/CalibratedCamera.h>
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
|
||||
using namespace boost::assign;
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
void create5PointExample1() {
|
||||
|
||||
// Class that will gather all data
|
||||
SfM_data data;
|
||||
|
||||
// Create two cameras and corresponding essential matrix E
|
||||
Rot3 aRb = Rot3::yaw(M_PI_2);
|
||||
Point3 aTb(0.1, 0, 0);
|
||||
Pose3 identity, aPb(aRb, aTb);
|
||||
data.cameras.push_back(SfM_Camera(identity));
|
||||
data.cameras.push_back(SfM_Camera(aPb));
|
||||
|
||||
// Create test data, we need at least 5 points
|
||||
vector<Point3> P;
|
||||
P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), //
|
||||
Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5);
|
||||
|
||||
BOOST_FOREACH(const Point3& p, P) {
|
||||
|
||||
// Create the track
|
||||
SfM_Track track;
|
||||
track.p = p;
|
||||
track.r = 1;
|
||||
track.g = 1;
|
||||
track.b = 1;
|
||||
|
||||
// Project points in both cameras
|
||||
for (size_t i = 0; i < 2; i++)
|
||||
track.measurements.push_back(make_pair(i, data.cameras[i].project(p)));
|
||||
|
||||
// Add track to data
|
||||
data.tracks.push_back(track);
|
||||
}
|
||||
|
||||
// Assumes example is run in ${GTSAM_TOP}/build/examples
|
||||
const string filename = "../../examples/data/5pointExample1.txt";
|
||||
writeBAL(filename, data);
|
||||
}
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
create5PointExample1();
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -93,8 +93,8 @@ public:
|
|||
// Consequently, the Jacobians are:
|
||||
// [ derror_x/dx derror_x/dy derror_x/dtheta ] = [1 0 0]
|
||||
// [ derror_y/dx derror_y/dy derror_y/dtheta ] = [0 1 0]
|
||||
if (H) (*H) = Matrix_(2,3, 1.0,0.0,0.0, 0.0,1.0,0.0);
|
||||
return Vector_(2, q.x() - mx_, q.y() - my_);
|
||||
if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0);
|
||||
return (Vector(2) << q.x() - mx_, q.y() - my_);
|
||||
}
|
||||
|
||||
// The second is a 'clone' function that allows the factor to be copied. Under most
|
||||
|
|
@ -118,17 +118,17 @@ int main(int argc, char** argv) {
|
|||
|
||||
// 2a. Add odometry factors
|
||||
// For simplicity, we will use the same noise model for each odometry factor
|
||||
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
|
||||
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
|
||||
// Create odometry (Between) factors between consecutive poses
|
||||
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
|
||||
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise));
|
||||
|
||||
// 2b. Add "GPS-like" measurements
|
||||
// We will use our custom UnaryFactor for this.
|
||||
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // 10cm std on x,y
|
||||
graph.push_back(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
|
||||
graph.push_back(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
|
||||
graph.push_back(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
|
||||
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // 10cm std on x,y
|
||||
graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
|
||||
graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
|
||||
graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
|
||||
graph.print("\nFactor Graph:\n"); // print
|
||||
|
||||
// 3. Create the data structure to hold the initialEstimate estimate to the solution
|
||||
|
|
|
|||
|
|
@ -64,13 +64,13 @@ int main(int argc, char** argv) {
|
|||
// Add a prior on the first pose, setting it to the origin
|
||||
// A prior factor consists of a mean and a noise model (covariance matrix)
|
||||
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
|
||||
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
|
||||
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
|
||||
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
|
||||
|
||||
// Add odometry factors
|
||||
Pose2 odometry(2.0, 0.0, 0.0);
|
||||
// For simplicity, we will use the same noise model for each odometry factor
|
||||
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
|
||||
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
|
||||
// Create odometry (Between) factors between consecutive poses
|
||||
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
|
||||
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
|
||||
|
|
|
|||
|
|
@ -80,18 +80,18 @@ int main(int argc, char** argv) {
|
|||
|
||||
// Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
|
||||
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
|
||||
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
|
||||
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
|
||||
graph.add(PriorFactor<Pose2>(x1, prior, priorNoise)); // add directly to graph
|
||||
|
||||
// Add two odometry factors
|
||||
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
|
||||
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
|
||||
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
|
||||
graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
|
||||
graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
|
||||
|
||||
// Add Range-Bearing measurements to two different landmarks
|
||||
// create a noise model for the landmark measurements
|
||||
noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
|
||||
noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
|
||||
// create the measurement values - indices are (pose id, landmark id)
|
||||
Rot2 bearing11 = Rot2::fromDegrees(45),
|
||||
bearing21 = Rot2::fromDegrees(90),
|
||||
|
|
|
|||
|
|
@ -71,11 +71,11 @@ int main(int argc, char** argv) {
|
|||
|
||||
// 2a. Add a prior on the first pose, setting it to the origin
|
||||
// A prior factor consists of a mean and a noise model (covariance matrix)
|
||||
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
|
||||
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
|
||||
graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
|
||||
|
||||
// For simplicity, we will use the same noise model for odometry and loop closures
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
|
||||
|
||||
// 2b. Add odometry factors
|
||||
// Create odometry (Between) factors between consecutive poses
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Pose2SLAMExample_graph->cpp
|
||||
* @file Pose2SLAMExample_graph.cpp
|
||||
* @brief Read graph from file and perform GraphSLAM
|
||||
* @date June 3, 2012
|
||||
* @author Frank Dellaert
|
||||
|
|
@ -20,28 +20,24 @@
|
|||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/nonlinear/Marginals.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
int main (int argc, char** argv) {
|
||||
|
||||
// Read File and create graph and initial estimate
|
||||
// Read File, create graph and initial estimate
|
||||
// we are in build/examples, data is in examples/Data
|
||||
NonlinearFactorGraph::shared_ptr graph ;
|
||||
NonlinearFactorGraph::shared_ptr graph;
|
||||
Values::shared_ptr initial;
|
||||
SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.05, 0.05, 5.0*M_PI/180.0));
|
||||
boost::tie(graph,initial) = load2D("../../examples/Data/w100-odom.graph",model);
|
||||
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0));
|
||||
boost::tie(graph, initial) = load2D("../../examples/Data/w100.graph", model);
|
||||
initial->print("Initial estimate:\n");
|
||||
|
||||
// Add a Gaussian prior on first poses
|
||||
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
|
||||
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.01, 0.01, 0.01));
|
||||
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.01, 0.01, 0.01));
|
||||
graph->add(PriorFactor<Pose2>(0, priorMean, priorNoise));
|
||||
|
||||
// Single Step Optimization using Levenberg-Marquardt
|
||||
|
|
@ -53,5 +49,5 @@ int main(int argc, char** argv) {
|
|||
cout.precision(2);
|
||||
cout << "\nP3:\n" << marginals.marginalCovariance(99) << endl;
|
||||
|
||||
return 0;
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,71 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Pose2SLAMExample_graphviz.cpp
|
||||
* @brief Save factor graph as graphviz dot file
|
||||
* @date Sept 6, 2013
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/Marginals.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <fstream>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
int main (int argc, char** argv) {
|
||||
|
||||
// 1. Create a factor graph container and add factors to it
|
||||
NonlinearFactorGraph graph;
|
||||
|
||||
// 2a. Add a prior on the first pose, setting it to the origin
|
||||
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
|
||||
graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
|
||||
|
||||
// For simplicity, we will use the same noise model for odometry and loop closures
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
|
||||
|
||||
// 2b. Add odometry factors
|
||||
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
|
||||
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
|
||||
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
|
||||
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
|
||||
|
||||
// 2c. Add the loop closure constraint
|
||||
graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
|
||||
|
||||
// 3. Create the data structure to hold the initial estimate to the solution
|
||||
// For illustrative purposes, these have been deliberately set to incorrect values
|
||||
Values initial;
|
||||
initial.insert(1, Pose2(0.5, 0.0, 0.2 ));
|
||||
initial.insert(2, Pose2(2.3, 0.1, -0.2 ));
|
||||
initial.insert(3, Pose2(4.1, 0.1, M_PI_2));
|
||||
initial.insert(4, Pose2(4.0, 2.0, M_PI ));
|
||||
initial.insert(5, Pose2(2.1, 2.1, -M_PI_2));
|
||||
|
||||
// Single Step Optimization using Levenberg-Marquardt
|
||||
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
|
||||
|
||||
// save factor graph as graphviz dot file
|
||||
// Render to PDF using "fdp Pose2SLAMExample.dot -Tpdf > graph.pdf"
|
||||
ofstream os("Pose2SLAMExample.dot");
|
||||
graph.saveGraph(os, result);
|
||||
|
||||
// Also print out to console
|
||||
graph.saveGraph(cout, result);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -68,12 +68,12 @@ int main(int argc, char** argv) {
|
|||
// 2a. Add a prior on the first pose, setting it to the origin
|
||||
// A prior factor consists of a mean and a noise model (covariance matrix)
|
||||
Pose2 prior(0.0, 0.0, 0.0); // prior at origin
|
||||
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
|
||||
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
|
||||
graph.add(PriorFactor<Pose2>(1, prior, priorNoise));
|
||||
|
||||
// 2b. Add odometry factors
|
||||
// For simplicity, we will use the same noise model for each odometry factor
|
||||
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
|
||||
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
|
||||
// Create odometry (Between) factors between consecutive poses
|
||||
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
|
||||
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
|
||||
|
|
@ -85,7 +85,7 @@ int main(int argc, char** argv) {
|
|||
// these constraints may be identified in many ways, such as appearance-based techniques
|
||||
// with camera images.
|
||||
// We will use another Between Factor to enforce this constraint, with the distance set to zero,
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
|
||||
graph.add(BetweenFactor<Pose2>(5, 1, Pose2(0.0, 0.0, 0.0), model));
|
||||
graph.print("\nFactor Graph:\n"); // print
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,118 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SFMExample.cpp
|
||||
* @brief A structure-from-motion problem on a simulated dataset
|
||||
* @author Duy-Nguyen Ta
|
||||
*/
|
||||
|
||||
/**
|
||||
* A structure-from-motion example with landmarks
|
||||
* - The landmarks form a 10 meter cube
|
||||
* - The robot rotates around the landmarks, always facing towards the cube
|
||||
*/
|
||||
|
||||
// For loading the data
|
||||
#include "SFMdata.h"
|
||||
|
||||
// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
// Each variable in the system (poses and landmarks) must be identified with a unique key.
|
||||
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
|
||||
// Here we will use Symbols
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
|
||||
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
|
||||
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
|
||||
// Here we will use Projection factors to model the camera's landmark observations.
|
||||
// Also, we will initialize the robot at some location using a Prior factor.
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/ProjectionFactor.h>
|
||||
|
||||
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
|
||||
// are nonlinear factors, we will need a Nonlinear Factor Graph.
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
||||
// Finally, once all of the factors have been added to our factor graph, we will want to
|
||||
// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
|
||||
// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
|
||||
// trust-region method known as Powell's Degleg
|
||||
#include <gtsam/nonlinear/DoglegOptimizer.h>
|
||||
|
||||
// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
|
||||
// nonlinear functions around an initial linearization point, then solve the linear system
|
||||
// to update the linearization point. This happens repeatedly until the solver converges
|
||||
// to a consistent set of variable values. This requires us to specify an initial guess
|
||||
// for each variable, held in a Values container.
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
|
||||
#include <vector>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main(int argc, char* argv[]) {
|
||||
|
||||
// Define the camera calibration parameters
|
||||
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
|
||||
|
||||
// Define the camera observation noise model
|
||||
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
|
||||
|
||||
// Create the set of ground-truth landmarks
|
||||
vector<Point3> points = createPoints();
|
||||
|
||||
// Create the set of ground-truth poses
|
||||
vector<Pose3> poses = createPoses();
|
||||
|
||||
// Create a factor graph
|
||||
NonlinearFactorGraph graph;
|
||||
|
||||
// Add a prior on pose x1. This indirectly specifies where the origin is.
|
||||
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph
|
||||
|
||||
// Simulated measurements from each camera pose, adding them to the factor graph
|
||||
for (size_t i = 0; i < poses.size(); ++i) {
|
||||
for (size_t j = 0; j < points.size(); ++j) {
|
||||
SimpleCamera camera(poses[i], *K);
|
||||
Point2 measurement = camera.project(points[j]);
|
||||
graph.add(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
|
||||
}
|
||||
}
|
||||
|
||||
// Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
|
||||
// Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
|
||||
// between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
|
||||
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
|
||||
graph.print("Factor Graph:\n");
|
||||
|
||||
// Create the data structure to hold the initial estimate to the solution
|
||||
// Intentionally initialize the variables off from the ground truth
|
||||
Values initialEstimate;
|
||||
for (size_t i = 0; i < poses.size(); ++i)
|
||||
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
|
||||
for (size_t j = 0; j < points.size(); ++j)
|
||||
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
|
||||
initialEstimate.print("Initial Estimates:\n");
|
||||
|
||||
/* Optimize the graph and print results */
|
||||
Values result = DoglegOptimizer(graph, initialEstimate).optimize();
|
||||
result.print("Final results:\n");
|
||||
|
||||
return 0;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -0,0 +1,94 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SFMExample.cpp
|
||||
* @brief Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
// For an explanation of headers, see SFMExample.cpp
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/GeneralSFMFactor.h>
|
||||
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
|
||||
#include <vector>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using symbol_shorthand::C;
|
||||
using symbol_shorthand::P;
|
||||
|
||||
// We will be using a projection factor that ties a SFM_Camera to a 3D point.
|
||||
// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
|
||||
// and has a total of 9 free parameters
|
||||
typedef GeneralSFMFactor<SfM_Camera,Point3> MyFactor;
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main (int argc, char* argv[]) {
|
||||
|
||||
// Find default file, but if an argument is given, try loading a file
|
||||
string filename = findExampleDataFile("dubrovnik-3-7-pre");
|
||||
if (argc>1) filename = string(argv[1]);
|
||||
|
||||
// Load the SfM data from file
|
||||
SfM_data mydata;
|
||||
const bool success = readBAL(filename, mydata);
|
||||
assert(success);
|
||||
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
|
||||
|
||||
// Create a factor graph
|
||||
NonlinearFactorGraph graph;
|
||||
|
||||
// We share *one* noiseModel between all projection factors
|
||||
noiseModel::Isotropic::shared_ptr noise =
|
||||
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
|
||||
|
||||
// Add measurements to the factor graph
|
||||
size_t j = 0;
|
||||
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
|
||||
BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
|
||||
size_t i = m.first;
|
||||
Point2 uv = m.second;
|
||||
graph.add(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
|
||||
}
|
||||
j += 1;
|
||||
}
|
||||
|
||||
// Add a prior on pose x1. This indirectly specifies where the origin is.
|
||||
// and a prior on the position of the first landmark to fix the scale
|
||||
graph.add(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)));
|
||||
graph.add(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
|
||||
|
||||
// Create initial estimate
|
||||
Values initial;
|
||||
size_t i = 0; j = 0;
|
||||
BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
|
||||
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p);
|
||||
|
||||
/* Optimize the graph and print results */
|
||||
Values result;
|
||||
try {
|
||||
LevenbergMarquardtParams params;
|
||||
params.setVerbosity("ERROR");
|
||||
LevenbergMarquardtOptimizer lm(graph, initial, params);
|
||||
result = lm.optimize();
|
||||
} catch (exception& e) {
|
||||
cout << e.what();
|
||||
}
|
||||
cout << "final error: " << graph.error(result) << endl;
|
||||
|
||||
return 0;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -0,0 +1,68 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SFMMdata.h
|
||||
* @brief Simple example for the structure-from-motion problems
|
||||
* @author Duy-Nguyen Ta
|
||||
*/
|
||||
|
||||
/**
|
||||
* A structure-from-motion example with landmarks
|
||||
* - The landmarks form a 10 meter cube
|
||||
* - The robot rotates around the landmarks, always facing towards the cube
|
||||
*/
|
||||
|
||||
// As this is a full 3D problem, we will use Pose3 variables to represent the camera
|
||||
// positions and Point3 variables (x, y, z) to represent the landmark coordinates.
|
||||
// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
|
||||
// We will also need a camera object to hold calibration information and perform projections.
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
|
||||
// We will also need a camera object to hold calibration information and perform projections.
|
||||
#include <gtsam/geometry/SimpleCamera.h>
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::vector<gtsam::Point3> createPoints() {
|
||||
|
||||
// Create the set of ground-truth landmarks
|
||||
std::vector<gtsam::Point3> points;
|
||||
points.push_back(gtsam::Point3(10.0,10.0,10.0));
|
||||
points.push_back(gtsam::Point3(-10.0,10.0,10.0));
|
||||
points.push_back(gtsam::Point3(-10.0,-10.0,10.0));
|
||||
points.push_back(gtsam::Point3(10.0,-10.0,10.0));
|
||||
points.push_back(gtsam::Point3(10.0,10.0,-10.0));
|
||||
points.push_back(gtsam::Point3(-10.0,10.0,-10.0));
|
||||
points.push_back(gtsam::Point3(-10.0,-10.0,-10.0));
|
||||
points.push_back(gtsam::Point3(10.0,-10.0,-10.0));
|
||||
|
||||
return points;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::vector<gtsam::Pose3> createPoses() {
|
||||
|
||||
// Create the set of ground-truth poses
|
||||
std::vector<gtsam::Pose3> poses;
|
||||
double radius = 30.0;
|
||||
int i = 0;
|
||||
double theta = 0.0;
|
||||
gtsam::Point3 up(0,0,1);
|
||||
gtsam::Point3 target(0,0,0);
|
||||
for(; i < 8; ++i, theta += 2*M_PI/8) {
|
||||
gtsam::Point3 position = gtsam::Point3(radius*cos(theta), radius*sin(theta), 0.0);
|
||||
gtsam::SimpleCamera camera = gtsam::SimpleCamera::Lookat(position, target, up);
|
||||
poses.push_back(camera.pose());
|
||||
}
|
||||
return poses;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,96 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SelfCalibrationExample.cpp
|
||||
* @brief Based on VisualSLAMExample, but with unknown (yet fixed) calibration.
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
/*
|
||||
* See the detailed documentation in Visual SLAM.
|
||||
* The only documentation below with deal with the self-calibration.
|
||||
*/
|
||||
|
||||
// For loading the data
|
||||
#include "SFMdata.h"
|
||||
|
||||
// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
// Inference and optimization
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/DoglegOptimizer.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
|
||||
// SFM-specific factors
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/GeneralSFMFactor.h> // does calibration !
|
||||
|
||||
// Standard headers
|
||||
#include <vector>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main(int argc, char* argv[]) {
|
||||
|
||||
// Create the set of ground-truth
|
||||
vector<Point3> points = createPoints();
|
||||
vector<Pose3> poses = createPoses();
|
||||
|
||||
// Create the factor graph
|
||||
NonlinearFactorGraph graph;
|
||||
|
||||
// Add a prior on pose x1.
|
||||
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
|
||||
|
||||
// Simulated measurements from each camera pose, adding them to the factor graph
|
||||
Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
|
||||
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
|
||||
for (size_t i = 0; i < poses.size(); ++i) {
|
||||
for (size_t j = 0; j < points.size(); ++j) {
|
||||
SimpleCamera camera(poses[i], K);
|
||||
Point2 measurement = camera.project(points[j]);
|
||||
// The only real difference with the Visual SLAM example is that here we use a
|
||||
// different factor type, that also calculates the Jacobian with respect to calibration
|
||||
graph.add(GeneralSFMFactor2<Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0)));
|
||||
}
|
||||
}
|
||||
|
||||
// Add a prior on the position of the first landmark.
|
||||
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
|
||||
|
||||
// Add a prior on the calibration.
|
||||
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100));
|
||||
graph.add(PriorFactor<Cal3_S2>(Symbol('K', 0), K, calNoise));
|
||||
|
||||
// Create the initial estimate to the solution
|
||||
// now including an estimate on the camera calibration parameters
|
||||
Values initialEstimate;
|
||||
initialEstimate.insert(Symbol('K', 0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0));
|
||||
for (size_t i = 0; i < poses.size(); ++i)
|
||||
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
|
||||
for (size_t j = 0; j < points.size(); ++j)
|
||||
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
|
||||
|
||||
/* Optimize the graph and print results */
|
||||
Values result = DoglegOptimizer(graph, initialEstimate).optimize();
|
||||
result.print("Final results:\n");
|
||||
|
||||
return 0;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -22,14 +22,11 @@
|
|||
* - The robot rotates around the landmarks, always facing towards the cube
|
||||
*/
|
||||
|
||||
// As this is a full 3D problem, we will use Pose3 variables to represent the camera
|
||||
// positions and Point3 variables (x, y, z) to represent the landmark coordinates.
|
||||
// For loading the data
|
||||
#include "SFMdata.h"
|
||||
|
||||
// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
|
||||
// We will also need a camera object to hold calibration information and perform projections.
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/SimpleCamera.h>
|
||||
|
||||
// Each variable in the system (poses and landmarks) must be identified with a unique key.
|
||||
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
|
||||
|
|
@ -67,28 +64,10 @@ int main(int argc, char* argv[]) {
|
|||
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
|
||||
|
||||
// Create the set of ground-truth landmarks
|
||||
std::vector<gtsam::Point3> points;
|
||||
points.push_back(gtsam::Point3(10.0,10.0,10.0));
|
||||
points.push_back(gtsam::Point3(-10.0,10.0,10.0));
|
||||
points.push_back(gtsam::Point3(-10.0,-10.0,10.0));
|
||||
points.push_back(gtsam::Point3(10.0,-10.0,10.0));
|
||||
points.push_back(gtsam::Point3(10.0,10.0,-10.0));
|
||||
points.push_back(gtsam::Point3(-10.0,10.0,-10.0));
|
||||
points.push_back(gtsam::Point3(-10.0,-10.0,-10.0));
|
||||
points.push_back(gtsam::Point3(10.0,-10.0,-10.0));
|
||||
vector<Point3> points = createPoints();
|
||||
|
||||
// Create the set of ground-truth poses
|
||||
std::vector<gtsam::Pose3> poses;
|
||||
double radius = 30.0;
|
||||
int i = 0;
|
||||
double theta = 0.0;
|
||||
gtsam::Point3 up(0,0,1);
|
||||
gtsam::Point3 target(0,0,0);
|
||||
for(; i < 8; ++i, theta += 2*M_PI/8) {
|
||||
gtsam::Point3 position = Point3(radius*cos(theta), radius*sin(theta), 0.0);
|
||||
gtsam::SimpleCamera camera = SimpleCamera::Lookat(position, target, up);
|
||||
poses.push_back(camera.pose());
|
||||
}
|
||||
vector<Pose3> poses = createPoses();
|
||||
|
||||
// Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization
|
||||
// and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter
|
||||
|
|
@ -124,7 +103,7 @@ int main(int argc, char* argv[]) {
|
|||
// adding it to iSAM.
|
||||
if( i == 0) {
|
||||
// Add a prior on pose x0
|
||||
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(Vector_(6, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
|
||||
|
||||
// Add a prior on landmark l0
|
||||
|
|
|
|||
|
|
@ -22,14 +22,11 @@
|
|||
* - The robot rotates around the landmarks, always facing towards the cube
|
||||
*/
|
||||
|
||||
// As this is a full 3D problem, we will use Pose3 variables to represent the camera
|
||||
// positions and Point3 variables (x, y, z) to represent the landmark coordinates.
|
||||
// For loading the data
|
||||
#include "SFMdata.h"
|
||||
|
||||
// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
|
||||
// We will also need a camera object to hold calibration information and perform projections.
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/SimpleCamera.h>
|
||||
|
||||
// Each variable in the system (poses and landmarks) must be identified with a unique key.
|
||||
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
|
||||
|
|
@ -67,28 +64,10 @@ int main(int argc, char* argv[]) {
|
|||
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
|
||||
|
||||
// Create the set of ground-truth landmarks
|
||||
std::vector<gtsam::Point3> points;
|
||||
points.push_back(gtsam::Point3(10.0,10.0,10.0));
|
||||
points.push_back(gtsam::Point3(-10.0,10.0,10.0));
|
||||
points.push_back(gtsam::Point3(-10.0,-10.0,10.0));
|
||||
points.push_back(gtsam::Point3(10.0,-10.0,10.0));
|
||||
points.push_back(gtsam::Point3(10.0,10.0,-10.0));
|
||||
points.push_back(gtsam::Point3(-10.0,10.0,-10.0));
|
||||
points.push_back(gtsam::Point3(-10.0,-10.0,-10.0));
|
||||
points.push_back(gtsam::Point3(10.0,-10.0,-10.0));
|
||||
vector<Point3> points = createPoints();
|
||||
|
||||
// Create the set of ground-truth poses
|
||||
std::vector<gtsam::Pose3> poses;
|
||||
double radius = 30.0;
|
||||
int i = 0;
|
||||
double theta = 0.0;
|
||||
gtsam::Point3 up(0,0,1);
|
||||
gtsam::Point3 target(0,0,0);
|
||||
for(; i < 8; ++i, theta += 2*M_PI/8) {
|
||||
gtsam::Point3 position = Point3(radius*cos(theta), radius*sin(theta), 0.0);
|
||||
gtsam::SimpleCamera camera = SimpleCamera::Lookat(position, target, up);
|
||||
poses.push_back(camera.pose());
|
||||
}
|
||||
vector<Pose3> poses = createPoses();
|
||||
|
||||
// Create a NonlinearISAM object which will relinearize and reorder the variables every "relinearizeInterval" updates
|
||||
int relinearizeInterval = 3;
|
||||
|
|
@ -118,7 +97,7 @@ int main(int argc, char* argv[]) {
|
|||
// adding it to iSAM.
|
||||
if( i == 0) {
|
||||
// Add a prior on pose x0
|
||||
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(Vector_(6, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
|
||||
|
||||
// Add a prior on landmark l0
|
||||
|
|
|
|||
Loading…
Reference in New Issue