Reformatted some and use of auto
Frank dellaert
parent
ff5a14831b
commit
0cd1e777bc
|
|
@ -66,12 +66,11 @@ using namespace gtsam;
|
|||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
|
||||
class UnaryFactor: public NoiseModelFactor1<Pose2> {
|
||||
|
||||
// The factor will hold a measurement consisting of an (X,Y) location
|
||||
// We could this with a Point2 but here we just use two doubles
|
||||
double mx_, my_;
|
||||
|
||||
public:
|
||||
public:
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<UnaryFactor> shared_ptr;
|
||||
|
||||
|
|
@ -85,15 +84,15 @@ public:
|
|||
// The first is the 'evaluateError' function. This function implements the desired measurement
|
||||
// function, returning a vector of errors when evaluated at the provided variable value. It
|
||||
// must also calculate the Jacobians for this measurement function, if requested.
|
||||
Vector evaluateError(const Pose2& q, boost::optional<Matrix&> H = boost::none) const
|
||||
{
|
||||
Vector evaluateError(const Pose2& q,
|
||||
boost::optional<Matrix&> H = boost::none) const {
|
||||
// The measurement function for a GPS-like measurement is simple:
|
||||
// error_x = pose.x - measurement.x
|
||||
// error_y = pose.y - measurement.y
|
||||
// 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).finished();
|
||||
if (H) (*H) = (Matrix(2, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0).finished();
|
||||
return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
|
||||
}
|
||||
|
||||
|
|
@ -107,25 +106,24 @@ public:
|
|||
// Additionally, we encourage you the use of unit testing your custom factors,
|
||||
// (as all GTSAM factors are), in which you would need an equals and print, to satisfy the
|
||||
// GTSAM_CONCEPT_TESTABLE_INST(T) defined in Testable.h, but these are not needed below.
|
||||
|
||||
}; // UnaryFactor
|
||||
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
// 1. Create a factor graph container and add factors to it
|
||||
NonlinearFactorGraph graph;
|
||||
|
||||
// 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(Vector3(0.2, 0.2, 0.1));
|
||||
auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
|
||||
// Create odometry (Between) factors between consecutive poses
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise);
|
||||
graph.emplace_shared<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(Vector2(0.1, 0.1)); // 10cm std on x,y
|
||||
auto unaryNoise =
|
||||
noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
|
||||
graph.emplace_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise);
|
||||
graph.emplace_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise);
|
||||
graph.emplace_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise);
|
||||
|
|
|
|||
|
|
@ -11,7 +11,8 @@
|
|||
|
||||
/**
|
||||
* @file METISOrdering.cpp
|
||||
* @brief Simple robot motion example, with prior and two odometry measurements, using a METIS ordering
|
||||
* @brief Simple robot motion example, with prior and two odometry measurements,
|
||||
* using a METIS ordering
|
||||
* @author Frank Dellaert
|
||||
* @author Andrew Melim
|
||||
*/
|
||||
|
|
@ -22,11 +23,11 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/Marginals.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -35,11 +36,11 @@ int main(int argc, char** argv) {
|
|||
NonlinearFactorGraph graph;
|
||||
|
||||
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
|
||||
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
|
||||
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
|
||||
graph.addPrior(1, priorMean, priorNoise);
|
||||
|
||||
Pose2 odometry(2.0, 0.0, 0.0);
|
||||
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
|
||||
auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
|
||||
graph.print("\nFactor Graph:\n"); // print
|
||||
|
|
@ -52,8 +53,8 @@ int main(int argc, char** argv) {
|
|||
|
||||
// optimize using Levenberg-Marquardt optimization
|
||||
LevenbergMarquardtParams params;
|
||||
// In order to specify the ordering type, we need to se the NonlinearOptimizerParameter "orderingType"
|
||||
// By default this parameter is set to OrderingType::COLAMD
|
||||
// In order to specify the ordering type, we need to se the "orderingType". By
|
||||
// default this parameter is set to OrderingType::COLAMD
|
||||
params.orderingType = Ordering::METIS;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, initial, params);
|
||||
Values result = optimizer.optimize();
|
||||
|
|
|
|||
|
|
@ -56,20 +56,19 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
// Create an empty nonlinear factor graph
|
||||
NonlinearFactorGraph graph;
|
||||
|
||||
// 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(Vector3(0.3, 0.3, 0.1));
|
||||
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
|
||||
graph.addPrior(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(Vector3(0.2, 0.2, 0.1));
|
||||
auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
|
||||
// Create odometry (Between) factors between consecutive poses
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
|
||||
|
|
|
|||
|
|
@ -69,35 +69,36 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
// Create a factor graph
|
||||
NonlinearFactorGraph graph;
|
||||
|
||||
// Create the keys we need for this simple example
|
||||
static Symbol x1('x',1), x2('x',2), x3('x',3);
|
||||
static Symbol l1('l',1), l2('l',2);
|
||||
static Symbol x1('x', 1), x2('x', 2), x3('x', 3);
|
||||
static Symbol l1('l', 1), l2('l', 2);
|
||||
|
||||
// Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
|
||||
// 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(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
|
||||
auto priorNoise = noiseModel::Diagonal::Sigmas(
|
||||
Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
|
||||
graph.addPrior(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(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
|
||||
Pose2 odometry(2.0, 0.0, 0.0);
|
||||
// create a measurement for both factors (the same in this case)
|
||||
auto odometryNoise = noiseModel::Diagonal::Sigmas(
|
||||
Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(x1, x2, odometry, odometryNoise);
|
||||
graph.emplace_shared<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(Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
|
||||
auto measurementNoise = noiseModel::Diagonal::Sigmas(
|
||||
Vector2(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),
|
||||
Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90),
|
||||
bearing32 = Rot2::fromDegrees(90);
|
||||
double range11 = std::sqrt(4.0+4.0),
|
||||
range21 = 2.0,
|
||||
range32 = 2.0;
|
||||
double range11 = std::sqrt(4.0 + 4.0), range21 = 2.0, range32 = 2.0;
|
||||
|
||||
// Add Bearing-Range factors
|
||||
graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x1, l1, bearing11, range11, measurementNoise);
|
||||
|
|
@ -110,7 +111,7 @@ int main(int argc, char** argv) {
|
|||
// Create (deliberately inaccurate) initial estimate
|
||||
Values initialEstimate;
|
||||
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
|
||||
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
|
||||
initialEstimate.insert(x2, Pose2(2.3, 0.1, -0.2));
|
||||
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
|
||||
initialEstimate.insert(l1, Point2(1.8, 2.1));
|
||||
initialEstimate.insert(l2, Point2(4.1, 1.8));
|
||||
|
|
@ -138,4 +139,3 @@ int main(int argc, char** argv) {
|
|||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -64,21 +64,20 @@ 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
|
||||
// A prior factor consists of a mean and a noise model (covariance matrix)
|
||||
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
|
||||
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
|
||||
graph.addPrior(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(Vector3(0.2, 0.2, 0.1));
|
||||
auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
|
||||
|
||||
// 2b. Add odometry factors
|
||||
// Create odometry (Between) factors between consecutive poses
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0 ), model);
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0), model);
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
|
||||
|
|
@ -93,10 +92,10 @@ int main(int argc, char** argv) {
|
|||
// 3. Create the data structure to hold the initialEstimate estimate to the solution
|
||||
// For illustrative purposes, these have been deliberately set to incorrect values
|
||||
Values initialEstimate;
|
||||
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 ));
|
||||
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 ));
|
||||
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
|
||||
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
|
||||
initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
|
||||
initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI ));
|
||||
initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI));
|
||||
initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
|
||||
initialEstimate.print("\nInitial Estimate:\n"); // print
|
||||
|
||||
|
|
|
|||
|
|
@ -30,7 +30,6 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
// 1. Create a factor graph container and add factors to it
|
||||
ExpressionFactorGraph graph;
|
||||
|
||||
|
|
@ -38,29 +37,30 @@ int main(int argc, char** argv) {
|
|||
Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5);
|
||||
|
||||
// 2a. Add a prior on the first pose, setting it to the origin
|
||||
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
|
||||
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
|
||||
graph.addExpressionFactor(x1, 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(Vector3(0.2, 0.2, 0.1));
|
||||
// For simplicity, we use the same noise model for odometry and loop closures
|
||||
auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
|
||||
|
||||
// 2b. Add odometry factors
|
||||
graph.addExpressionFactor(between(x1,x2), Pose2(2, 0, 0 ), model);
|
||||
graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model);
|
||||
graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model);
|
||||
graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model);
|
||||
graph.addExpressionFactor(between(x1, x2), Pose2(2, 0, 0), model);
|
||||
graph.addExpressionFactor(between(x2, x3), Pose2(2, 0, M_PI_2), model);
|
||||
graph.addExpressionFactor(between(x3, x4), Pose2(2, 0, M_PI_2), model);
|
||||
graph.addExpressionFactor(between(x4, x5), Pose2(2, 0, M_PI_2), model);
|
||||
|
||||
// 2c. Add the loop closure constraint
|
||||
graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model);
|
||||
graph.addExpressionFactor(between(x5, x2), Pose2(2, 0, M_PI_2), model);
|
||||
graph.print("\nFactor Graph:\n"); // print
|
||||
|
||||
// 3. Create the data structure to hold the initialEstimate estimate to the solution
|
||||
// For illustrative purposes, these have been deliberately set to incorrect values
|
||||
// 3. Create the data structure to hold the initialEstimate estimate to the
|
||||
// solution For illustrative purposes, these have been deliberately set to
|
||||
// incorrect values
|
||||
Values initialEstimate;
|
||||
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 ));
|
||||
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 ));
|
||||
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
|
||||
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
|
||||
initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
|
||||
initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI ));
|
||||
initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI));
|
||||
initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
|
||||
initialEstimate.print("\nInitial Estimate:\n"); // print
|
||||
|
||||
|
|
|
|||
|
|
@ -28,20 +28,18 @@ using namespace gtsam;
|
|||
|
||||
// HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber)
|
||||
int main(const int argc, const char *argv[]) {
|
||||
|
||||
string kernelType = "none";
|
||||
int maxIterations = 100; // default
|
||||
string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default
|
||||
|
||||
// Parse user's inputs
|
||||
if (argc > 1){
|
||||
if (argc > 1) {
|
||||
g2oFile = argv[1]; // input dataset filename
|
||||
// outputFile = g2oFile = argv[2]; // done later
|
||||
}
|
||||
if (argc > 3){
|
||||
if (argc > 3) {
|
||||
maxIterations = atoi(argv[3]); // user can specify either tukey or huber
|
||||
}
|
||||
if (argc > 4){
|
||||
if (argc > 4) {
|
||||
kernelType = argv[4]; // user can specify either tukey or huber
|
||||
}
|
||||
|
||||
|
|
@ -49,20 +47,22 @@ int main(const int argc, const char *argv[]) {
|
|||
NonlinearFactorGraph::shared_ptr graph;
|
||||
Values::shared_ptr initial;
|
||||
bool is3D = false;
|
||||
if(kernelType.compare("none") == 0){
|
||||
boost::tie(graph, initial) = readG2o(g2oFile,is3D);
|
||||
if (kernelType.compare("none") == 0) {
|
||||
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
|
||||
}
|
||||
if(kernelType.compare("huber") == 0){
|
||||
if (kernelType.compare("huber") == 0) {
|
||||
std::cout << "Using robust kernel: huber " << std::endl;
|
||||
boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeHUBER);
|
||||
boost::tie(graph, initial) =
|
||||
readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
|
||||
}
|
||||
if(kernelType.compare("tukey") == 0){
|
||||
if (kernelType.compare("tukey") == 0) {
|
||||
std::cout << "Using robust kernel: tukey " << std::endl;
|
||||
boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY);
|
||||
boost::tie(graph, initial) =
|
||||
readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
|
||||
}
|
||||
|
||||
// Add prior on the pose having index (key) = 0
|
||||
noiseModel::Diagonal::shared_ptr priorModel = //
|
||||
auto priorModel = //
|
||||
noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
|
||||
graph->addPrior(0, Pose2(), priorModel);
|
||||
std::cout << "Adding prior on pose 0 " << std::endl;
|
||||
|
|
@ -71,7 +71,8 @@ int main(const int argc, const char *argv[]) {
|
|||
params.setVerbosity("TERMINATION");
|
||||
if (argc > 3) {
|
||||
params.maxIterations = maxIterations;
|
||||
std::cout << "User required to perform maximum " << params.maxIterations << " iterations "<< std::endl;
|
||||
std::cout << "User required to perform maximum " << params.maxIterations
|
||||
<< " iterations " << std::endl;
|
||||
}
|
||||
|
||||
std::cout << "Optimizing the factor graph" << std::endl;
|
||||
|
|
@ -79,8 +80,8 @@ int main(const int argc, const char *argv[]) {
|
|||
Values result = optimizer.optimize();
|
||||
std::cout << "Optimization complete" << std::endl;
|
||||
|
||||
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
|
||||
std::cout << "final error=" <<graph->error(result)<< std::endl;
|
||||
std::cout << "initial error=" << graph->error(*initial) << std::endl;
|
||||
std::cout << "final error=" << graph->error(result) << std::endl;
|
||||
|
||||
if (argc < 3) {
|
||||
result.print("result");
|
||||
|
|
|
|||
|
|
@ -25,20 +25,20 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
int main (int argc, char** argv) {
|
||||
|
||||
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(Vector3(0.3, 0.3, 0.1));
|
||||
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
|
||||
graph.addPrior(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(Vector3(0.2, 0.2, 0.1));
|
||||
// For simplicity, we will use the same noise model for odometry and loop
|
||||
// closures
|
||||
auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
|
||||
|
||||
// 2b. Add odometry factors
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0 ), model);
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0), model);
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
|
||||
|
|
@ -49,10 +49,10 @@ int main (int argc, char** argv) {
|
|||
// 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(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(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
|
||||
|
|
|
|||
|
|
@ -28,7 +28,6 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
|
||||
int main(const int argc, const char *argv[]) {
|
||||
|
||||
// Read graph from file
|
||||
string g2oFile;
|
||||
if (argc < 2)
|
||||
|
|
@ -41,8 +40,7 @@ int main(const int argc, const char *argv[]) {
|
|||
boost::tie(graph, initial) = readG2o(g2oFile);
|
||||
|
||||
// Add prior on the pose having index (key) = 0
|
||||
noiseModel::Diagonal::shared_ptr priorModel = //
|
||||
noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
|
||||
auto priorModel = noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
|
||||
graph->addPrior(0, Pose2(), priorModel);
|
||||
graph->print();
|
||||
|
||||
|
|
|
|||
|
|
@ -28,35 +28,35 @@ 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
|
||||
Pose2 prior(0.0, 0.0, 0.0); // prior at origin
|
||||
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
|
||||
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
|
||||
graph.addPrior(1, prior, priorNoise);
|
||||
|
||||
// 2b. Add odometry factors
|
||||
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
|
||||
auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
|
||||
|
||||
// 2c. Add the loop closure constraint
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(5, 1, Pose2(0.0, 0.0, 0.0), model);
|
||||
auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(5, 1, Pose2(0.0, 0.0, 0.0),
|
||||
model);
|
||||
graph.print("\nFactor Graph:\n"); // print
|
||||
|
||||
|
||||
// 3. Create the data structure to hold the initialEstimate estimate to the solution
|
||||
// 3. Create the data structure to hold the initialEstimate estimate to the
|
||||
// solution
|
||||
Values initialEstimate;
|
||||
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
|
||||
initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1));
|
||||
initialEstimate.insert(3, Pose2(2.1, 1.9, 2.8));
|
||||
initialEstimate.insert(4, Pose2(-.3, 2.5, 4.2));
|
||||
initialEstimate.insert(5, Pose2(0.1,-0.7, 5.8));
|
||||
initialEstimate.insert(5, Pose2(0.1, -0.7, 5.8));
|
||||
initialEstimate.print("\nInitial Estimate:\n"); // print
|
||||
|
||||
// 4. Single Step Optimization using Levenberg-Marquardt
|
||||
|
|
@ -64,9 +64,9 @@ int main(int argc, char** argv) {
|
|||
parameters.verbosity = NonlinearOptimizerParams::ERROR;
|
||||
parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
|
||||
|
||||
// LM is still the outer optimization loop, but by specifying "Iterative" below
|
||||
// We indicate that an iterative linear solver should be used.
|
||||
// In addition, the *type* of the iterativeParams decides on the type of
|
||||
// LM is still the outer optimization loop, but by specifying "Iterative"
|
||||
// below We indicate that an iterative linear solver should be used. In
|
||||
// addition, the *type* of the iterativeParams decides on the type of
|
||||
// iterative solver, in this case the SPCG (subgraph PCG)
|
||||
parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
|
||||
parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();
|
||||
|
|
|
|||
|
|
@ -10,9 +10,9 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Pose3SLAMExample_initializePose3.cpp
|
||||
* @file Pose3Localization.cpp
|
||||
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
|
||||
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
|
||||
* Syntax for the script is ./Pose3Localization input.g2o output.g2o
|
||||
* @date Aug 25, 2014
|
||||
* @author Luca Carlone
|
||||
*/
|
||||
|
|
@ -26,8 +26,7 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
int main(const int argc, const char *argv[]) {
|
||||
|
||||
int main(const int argc, const char* argv[]) {
|
||||
// Read graph from file
|
||||
string g2oFile;
|
||||
if (argc < 2)
|
||||
|
|
@ -41,10 +40,10 @@ int main(const int argc, const char *argv[]) {
|
|||
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
|
||||
|
||||
// Add prior on the first key
|
||||
noiseModel::Diagonal::shared_ptr priorModel = //
|
||||
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
|
||||
auto priorModel = noiseModel::Diagonal::Variances(
|
||||
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
|
||||
Key firstKey = 0;
|
||||
for(const Values::ConstKeyValuePair& key_value: *initial) {
|
||||
for (const Values::ConstKeyValuePair& key_value : *initial) {
|
||||
std::cout << "Adding prior to g2o file " << std::endl;
|
||||
firstKey = key_value.key;
|
||||
graph->addPrior(firstKey, Pose3(), priorModel);
|
||||
|
|
@ -53,13 +52,13 @@ int main(const int argc, const char *argv[]) {
|
|||
|
||||
std::cout << "Optimizing the factor graph" << std::endl;
|
||||
GaussNewtonParams params;
|
||||
params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
|
||||
params.setVerbosity("TERMINATION"); // show info about stopping conditions
|
||||
GaussNewtonOptimizer optimizer(*graph, *initial, params);
|
||||
Values result = optimizer.optimize();
|
||||
std::cout << "Optimization complete" << std::endl;
|
||||
|
||||
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
|
||||
std::cout << "final error=" <<graph->error(result)<< std::endl;
|
||||
std::cout << "initial error=" << graph->error(*initial) << std::endl;
|
||||
std::cout << "final error=" << graph->error(result) << std::endl;
|
||||
|
||||
if (argc < 3) {
|
||||
result.print("result");
|
||||
|
|
@ -75,7 +74,7 @@ int main(const int argc, const char *argv[]) {
|
|||
|
||||
// Calculate and print marginal covariances for all variables
|
||||
Marginals marginals(*graph, result);
|
||||
for(const auto& key_value: result) {
|
||||
for (const auto& key_value : result) {
|
||||
auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
|
||||
if (!p) continue;
|
||||
std::cout << marginals.marginalCovariance(key_value.key) << endl;
|
||||
|
|
|
|||
|
|
@ -10,9 +10,9 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Pose3SLAMExample_initializePose3.cpp
|
||||
* @file Pose3SLAMExample_g2o.cpp
|
||||
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
|
||||
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
|
||||
* Syntax for the script is ./Pose3SLAMExample_g2o input.g2o output.g2o
|
||||
* @date Aug 25, 2014
|
||||
* @author Luca Carlone
|
||||
*/
|
||||
|
|
@ -25,8 +25,7 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
int main(const int argc, const char *argv[]) {
|
||||
|
||||
int main(const int argc, const char* argv[]) {
|
||||
// Read graph from file
|
||||
string g2oFile;
|
||||
if (argc < 2)
|
||||
|
|
@ -40,10 +39,10 @@ int main(const int argc, const char *argv[]) {
|
|||
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
|
||||
|
||||
// Add prior on the first key
|
||||
noiseModel::Diagonal::shared_ptr priorModel = //
|
||||
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
|
||||
auto priorModel = noiseModel::Diagonal::Variances(
|
||||
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
|
||||
Key firstKey = 0;
|
||||
for(const Values::ConstKeyValuePair& key_value: *initial) {
|
||||
for (const Values::ConstKeyValuePair& key_value : *initial) {
|
||||
std::cout << "Adding prior to g2o file " << std::endl;
|
||||
firstKey = key_value.key;
|
||||
graph->addPrior(firstKey, Pose3(), priorModel);
|
||||
|
|
@ -52,13 +51,13 @@ int main(const int argc, const char *argv[]) {
|
|||
|
||||
std::cout << "Optimizing the factor graph" << std::endl;
|
||||
GaussNewtonParams params;
|
||||
params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
|
||||
params.setVerbosity("TERMINATION"); // show info about stopping conditions
|
||||
GaussNewtonOptimizer optimizer(*graph, *initial, params);
|
||||
Values result = optimizer.optimize();
|
||||
std::cout << "Optimization complete" << std::endl;
|
||||
|
||||
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
|
||||
std::cout << "final error=" <<graph->error(result)<< std::endl;
|
||||
std::cout << "initial error=" << graph->error(*initial) << std::endl;
|
||||
std::cout << "final error=" << graph->error(result) << std::endl;
|
||||
|
||||
if (argc < 3) {
|
||||
result.print("result");
|
||||
|
|
|
|||
|
|
@ -10,9 +10,9 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Pose3SLAMExample_initializePose3.cpp
|
||||
* @file Pose3SLAMExample_initializePose3Chordal.cpp
|
||||
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
|
||||
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
|
||||
* Syntax for the script is ./Pose3SLAMExample_initializePose3Chordal input.g2o output.g2o
|
||||
* @date Aug 25, 2014
|
||||
* @author Luca Carlone
|
||||
*/
|
||||
|
|
@ -25,8 +25,7 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
int main(const int argc, const char *argv[]) {
|
||||
|
||||
int main(const int argc, const char* argv[]) {
|
||||
// Read graph from file
|
||||
string g2oFile;
|
||||
if (argc < 2)
|
||||
|
|
@ -40,10 +39,10 @@ int main(const int argc, const char *argv[]) {
|
|||
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
|
||||
|
||||
// Add prior on the first key
|
||||
noiseModel::Diagonal::shared_ptr priorModel = //
|
||||
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
|
||||
auto priorModel = noiseModel::Diagonal::Variances(
|
||||
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
|
||||
Key firstKey = 0;
|
||||
for(const Values::ConstKeyValuePair& key_value: *initial) {
|
||||
for (const Values::ConstKeyValuePair& key_value : *initial) {
|
||||
std::cout << "Adding prior to g2o file " << std::endl;
|
||||
firstKey = key_value.key;
|
||||
graph->addPrior(firstKey, Pose3(), priorModel);
|
||||
|
|
|
|||
|
|
@ -10,9 +10,9 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Pose3SLAMExample_initializePose3.cpp
|
||||
* @file Pose3SLAMExample_initializePose3Gradient.cpp
|
||||
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
|
||||
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
|
||||
* Syntax for the script is ./Pose3SLAMExample_initializePose3Gradient input.g2o output.g2o
|
||||
* @date Aug 25, 2014
|
||||
* @author Luca Carlone
|
||||
*/
|
||||
|
|
@ -25,8 +25,7 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
int main(const int argc, const char *argv[]) {
|
||||
|
||||
int main(const int argc, const char* argv[]) {
|
||||
// Read graph from file
|
||||
string g2oFile;
|
||||
if (argc < 2)
|
||||
|
|
@ -40,10 +39,10 @@ int main(const int argc, const char *argv[]) {
|
|||
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
|
||||
|
||||
// Add prior on the first key
|
||||
noiseModel::Diagonal::shared_ptr priorModel = //
|
||||
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
|
||||
auto priorModel = noiseModel::Diagonal::Variances(
|
||||
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
|
||||
Key firstKey = 0;
|
||||
for(const Values::ConstKeyValuePair& key_value: *initial) {
|
||||
for (const Values::ConstKeyValuePair& key_value : *initial) {
|
||||
std::cout << "Adding prior to g2o file " << std::endl;
|
||||
firstKey = key_value.key;
|
||||
graph->addPrior(firstKey, Pose3(), priorModel);
|
||||
|
|
@ -52,11 +51,13 @@ int main(const int argc, const char *argv[]) {
|
|||
|
||||
std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
|
||||
bool useGradient = true;
|
||||
Values initialization = InitializePose3::initialize(*graph, *initial, useGradient);
|
||||
Values initialization =
|
||||
InitializePose3::initialize(*graph, *initial, useGradient);
|
||||
std::cout << "done!" << std::endl;
|
||||
|
||||
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
|
||||
std::cout << "initialization error=" <<graph->error(initialization)<< std::endl;
|
||||
std::cout << "initial error=" << graph->error(*initial) << std::endl;
|
||||
std::cout << "initialization error=" << graph->error(initialization)
|
||||
<< std::endl;
|
||||
|
||||
if (argc < 3) {
|
||||
initialization.print("initialization");
|
||||
|
|
|
|||
|
|
@ -56,12 +56,12 @@ 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
|
||||
auto measurementNoise =
|
||||
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
|
||||
|
||||
// Create the set of ground-truth landmarks
|
||||
vector<Point3> points = createPoints();
|
||||
|
|
@ -73,32 +73,45 @@ int main(int argc, char* argv[]) {
|
|||
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.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
auto poseNoise = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
|
||||
.finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
graph.addPrior(Symbol('x', 0), poses[0], poseNoise); // add directly to graph
|
||||
|
||||
// Simulated measurements from each camera pose, adding them to the factor graph
|
||||
// Simulated measurements from each camera pose, adding them to the factor
|
||||
// graph
|
||||
for (size_t i = 0; i < poses.size(); ++i) {
|
||||
PinholeCamera<Cal3_S2> camera(poses[i], *K);
|
||||
for (size_t j = 0; j < points.size(); ++j) {
|
||||
Point2 measurement = camera.project(points[j]);
|
||||
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
|
||||
graph.emplace_shared<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.addPrior(Symbol('l', 0), points[0], pointNoise); // add directly to graph
|
||||
// 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.
|
||||
auto pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
graph.addPrior(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::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
|
||||
for (size_t j = 0; j < points.size(); ++j)
|
||||
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
|
||||
for (size_t i = 0; i < poses.size(); ++i) {
|
||||
auto corrupted_pose = poses[i].compose(
|
||||
Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)));
|
||||
initialEstimate.insert(
|
||||
Symbol('x', i), corrupted_pose);
|
||||
}
|
||||
for (size_t j = 0; j < points.size(); ++j) {
|
||||
auto corrupted_point = points[j] + Point3(-0.25, 0.20, 0.15);
|
||||
initialEstimate.insert<Point3>(Symbol('l', j), corrupted_point);
|
||||
}
|
||||
initialEstimate.print("Initial Estimates:\n");
|
||||
|
||||
/* Optimize the graph and print results */
|
||||
|
|
|
|||
|
|
@ -40,20 +40,16 @@ using symbol_shorthand::P;
|
|||
// An SfmCamera is defined in datase.h as a camera with unknown Cal3Bundler calibration
|
||||
// and has a total of 9 free parameters
|
||||
|
||||
/* ************************************************************************* */
|
||||
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]);
|
||||
if (argc > 1) filename = string(argv[1]);
|
||||
|
||||
// Load the SfM data from file
|
||||
SfmData mydata;
|
||||
readBAL(filename, mydata);
|
||||
cout
|
||||
<< boost::format("read %1% tracks on %2% cameras\n")
|
||||
% mydata.number_tracks() % mydata.number_cameras();
|
||||
cout << boost::format("read %1% tracks on %2% cameras\n") %
|
||||
mydata.number_tracks() % mydata.number_cameras();
|
||||
|
||||
// Create a factor graph
|
||||
ExpressionFactorGraph graph;
|
||||
|
|
@ -73,15 +69,15 @@ int main(int argc, char* argv[]) {
|
|||
noiseModel::Isotropic::Sigma(3, 0.1));
|
||||
|
||||
// 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
|
||||
auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
|
||||
|
||||
// Simulated measurements from each camera pose, adding them to the factor graph
|
||||
// Simulated measurements from each camera pose, adding them to the factor
|
||||
// graph
|
||||
size_t j = 0;
|
||||
for(const SfmTrack& track: mydata.tracks) {
|
||||
for (const SfmTrack& track : mydata.tracks) {
|
||||
// Leaf expression for j^th point
|
||||
Point3_ point_('p', j);
|
||||
for(const SfmMeasurement& m: track.measurements) {
|
||||
for (const SfmMeasurement& m : track.measurements) {
|
||||
size_t i = m.first;
|
||||
Point2 uv = m.second;
|
||||
// Leaf expression for i^th camera
|
||||
|
|
@ -98,10 +94,8 @@ int main(int argc, char* argv[]) {
|
|||
Values initial;
|
||||
size_t i = 0;
|
||||
j = 0;
|
||||
for(const SfmCamera& camera: mydata.cameras)
|
||||
initial.insert(C(i++), camera);
|
||||
for(const SfmTrack& track: mydata.tracks)
|
||||
initial.insert(P(j++), track.p);
|
||||
for (const SfmCamera& camera : mydata.cameras) initial.insert(C(i++), camera);
|
||||
for (const SfmTrack& track : mydata.tracks) initial.insert(P(j++), track.p);
|
||||
|
||||
/* Optimize the graph and print results */
|
||||
Values result;
|
||||
|
|
@ -117,5 +111,3 @@ int main(int argc, char* argv[]) {
|
|||
|
||||
return 0;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -44,7 +44,7 @@ int main(int argc, char* argv[]) {
|
|||
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 =
|
||||
auto measurementNoise =
|
||||
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
|
||||
|
||||
// Create the set of ground-truth landmarks and poses
|
||||
|
|
@ -80,7 +80,7 @@ int main(int argc, char* argv[]) {
|
|||
|
||||
// Add a prior on pose x0. This indirectly specifies where the origin is.
|
||||
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
|
||||
auto noise = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
|
||||
graph.addPrior(0, poses[0], noise);
|
||||
|
||||
|
|
|
|||
|
|
@ -35,12 +35,11 @@ typedef PinholePose<Cal3_S2> Camera;
|
|||
|
||||
/* ************************************************************************* */
|
||||
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 =
|
||||
auto measurementNoise =
|
||||
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
|
||||
|
||||
// Create the set of ground-truth landmarks and poses
|
||||
|
|
@ -52,17 +51,16 @@ int main(int argc, char* argv[]) {
|
|||
|
||||
// Simulated measurements from each camera pose, adding them to the factor graph
|
||||
for (size_t j = 0; j < points.size(); ++j) {
|
||||
|
||||
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
|
||||
// every landmark represent a single landmark, we use shared pointer to init
|
||||
// the factor, and then insert measurements.
|
||||
SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
|
||||
|
||||
for (size_t i = 0; i < poses.size(); ++i) {
|
||||
|
||||
// generate the 2D measurement
|
||||
Camera camera(poses[i], K);
|
||||
Point2 measurement = camera.project(points[j]);
|
||||
|
||||
// call add() function to add measurement into a single factor, here we need to add:
|
||||
// call add() function to add measurement into a single factor
|
||||
smartfactor->add(measurement, i);
|
||||
}
|
||||
|
||||
|
|
@ -72,7 +70,7 @@ int main(int argc, char* argv[]) {
|
|||
|
||||
// Add a prior on pose x0. This indirectly specifies where the origin is.
|
||||
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
|
||||
auto noise = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
|
||||
graph.addPrior(0, poses[0], noise);
|
||||
|
||||
|
|
@ -85,9 +83,9 @@ int main(int argc, char* argv[]) {
|
|||
for (size_t i = 0; i < poses.size(); ++i)
|
||||
initialEstimate.insert(i, poses[i].compose(delta));
|
||||
|
||||
// We will use LM in the outer optimization loop, but by specifying "Iterative" below
|
||||
// We indicate that an iterative linear solver should be used.
|
||||
// In addition, the *type* of the iterativeParams decides on the type of
|
||||
// We will use LM in the outer optimization loop, but by specifying
|
||||
// "Iterative" below We indicate that an iterative linear solver should be
|
||||
// used. In addition, the *type* of the iterativeParams decides on the type of
|
||||
// iterative solver, in this case the SPCG (subgraph PCG)
|
||||
LevenbergMarquardtParams parameters;
|
||||
parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
|
||||
|
|
@ -110,8 +108,7 @@ int main(int argc, char* argv[]) {
|
|||
result.print("Final results:\n");
|
||||
Values landmark_result;
|
||||
for (size_t j = 0; j < points.size(); ++j) {
|
||||
SmartFactor::shared_ptr smart = //
|
||||
boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
|
||||
auto smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
|
||||
if (smart) {
|
||||
boost::optional<Point3> point = smart->point(result);
|
||||
if (point) // ignore if boost::optional return nullptr
|
||||
|
|
|
|||
|
|
@ -49,7 +49,7 @@ int main (int argc, char* argv[]) {
|
|||
NonlinearFactorGraph graph;
|
||||
|
||||
// We share *one* noiseModel between all projection factors
|
||||
noiseModel::Isotropic::shared_ptr noise =
|
||||
auto noise =
|
||||
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
|
||||
|
||||
// Add measurements to the factor graph
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file SFMExample.cpp
|
||||
* @file SFMExample_bal_COLAMD_METIS.cpp
|
||||
* @brief This file is to compare the ordering performance for COLAMD vs METIS.
|
||||
* Example problem is to solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file.
|
||||
* @author Frank Dellaert, Zhaoyang Lv
|
||||
|
|
@ -34,36 +34,36 @@ 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<SfmCamera,Point3> MyFactor;
|
||||
// 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<SfmCamera, Point3> MyFactor;
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main (int argc, char* argv[]) {
|
||||
|
||||
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]);
|
||||
if (argc > 1) filename = string(argv[1]);
|
||||
|
||||
// Load the SfM data from file
|
||||
SfmData mydata;
|
||||
readBAL(filename, mydata);
|
||||
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
|
||||
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
|
||||
auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
|
||||
|
||||
// Add measurements to the factor graph
|
||||
size_t j = 0;
|
||||
for(const SfmTrack& track: mydata.tracks) {
|
||||
for(const SfmMeasurement& m: track.measurements) {
|
||||
for (const SfmTrack& track : mydata.tracks) {
|
||||
for (const SfmMeasurement& m : track.measurements) {
|
||||
size_t i = m.first;
|
||||
Point2 uv = m.second;
|
||||
graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
|
||||
graph.emplace_shared<MyFactor>(
|
||||
uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
|
||||
}
|
||||
j += 1;
|
||||
}
|
||||
|
|
@ -71,13 +71,15 @@ int main (int argc, char* argv[]) {
|
|||
// 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.addPrior(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
|
||||
graph.addPrior(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
|
||||
graph.addPrior(P(0), mydata.tracks[0].p,
|
||||
noiseModel::Isotropic::Sigma(3, 0.1));
|
||||
|
||||
// Create initial estimate
|
||||
Values initial;
|
||||
size_t i = 0; j = 0;
|
||||
for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera);
|
||||
for(const SfmTrack& track: mydata.tracks) initial.insert(P(j++), track.p);
|
||||
size_t i = 0;
|
||||
j = 0;
|
||||
for (const SfmCamera& camera : mydata.cameras) initial.insert(C(i++), camera);
|
||||
for (const SfmTrack& track : mydata.tracks) initial.insert(P(j++), track.p);
|
||||
|
||||
/** --------------- COMPARISON -----------------------**/
|
||||
/** ----------------------------------------------------**/
|
||||
|
|
@ -98,7 +100,7 @@ int main (int argc, char* argv[]) {
|
|||
}
|
||||
|
||||
// expect they have different ordering results
|
||||
if(params_using_COLAMD.ordering == params_using_METIS.ordering) {
|
||||
if (params_using_COLAMD.ordering == params_using_METIS.ordering) {
|
||||
cout << "COLAMD and METIS produce the same ordering. "
|
||||
<< "Problem here!!!" << endl;
|
||||
}
|
||||
|
|
@ -120,7 +122,6 @@ int main (int argc, char* argv[]) {
|
|||
cout << e.what();
|
||||
}
|
||||
|
||||
|
||||
{ // printing the result
|
||||
|
||||
cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl;
|
||||
|
|
@ -129,15 +130,13 @@ int main (int argc, char* argv[]) {
|
|||
cout << endl << endl;
|
||||
|
||||
cout << "Time comparison by solving " << filename << " results:" << endl;
|
||||
cout << boost::format("%1% point tracks and %2% cameras\n") \
|
||||
% mydata.number_tracks() % mydata.number_cameras() \
|
||||
cout << boost::format("%1% point tracks and %2% cameras\n") %
|
||||
mydata.number_tracks() % mydata.number_cameras()
|
||||
<< endl;
|
||||
|
||||
tictoc_print_();
|
||||
}
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -41,9 +41,7 @@
|
|||
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();
|
||||
|
|
@ -52,28 +50,38 @@ int main(int argc, char* argv[]) {
|
|||
NonlinearFactorGraph graph;
|
||||
|
||||
// Add a prior on pose x1.
|
||||
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
auto poseNoise = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
|
||||
.finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
graph.addPrior(Symbol('x', 0), poses[0], poseNoise);
|
||||
|
||||
// Simulated measurements from each camera pose, adding them to the factor graph
|
||||
// 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);
|
||||
auto 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) {
|
||||
PinholeCamera<Cal3_S2> 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.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0));
|
||||
// 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.emplace_shared<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.addPrior(Symbol('l', 0), points[0], pointNoise); // add directly to graph
|
||||
auto pointNoise =
|
||||
noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
graph.addPrior(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).finished());
|
||||
auto calNoise = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(5) << 500, 500, 0.1, 100, 100).finished());
|
||||
graph.addPrior(Symbol('K', 0), K, calNoise);
|
||||
|
||||
// Create the initial estimate to the solution
|
||||
|
|
@ -81,9 +89,12 @@ int main(int argc, char* argv[]) {
|
|||
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::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
|
||||
initialEstimate.insert(
|
||||
Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25),
|
||||
Point3(0.05, -0.10, 0.20))));
|
||||
for (size_t j = 0; j < points.size(); ++j)
|
||||
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
|
||||
initialEstimate.insert<Point3>(Symbol('l', j),
|
||||
points[j] + Point3(-0.25, 0.20, 0.15));
|
||||
|
||||
/* Optimize the graph and print results */
|
||||
Values result = DoglegOptimizer(graph, initialEstimate).optimize();
|
||||
|
|
@ -91,5 +102,4 @@ int main(int argc, char* argv[]) {
|
|||
|
||||
return 0;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -59,7 +59,6 @@ using namespace gtsam;
|
|||
const double degree = M_PI / 180;
|
||||
|
||||
int main() {
|
||||
|
||||
/**
|
||||
* Step 1: Create a factor to express a unary constraint
|
||||
* The "prior" in this case is the measurement from a sensor,
|
||||
|
|
@ -76,8 +75,8 @@ int main() {
|
|||
*/
|
||||
Rot2 prior = Rot2::fromAngle(30 * degree);
|
||||
prior.print("goal angle");
|
||||
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree);
|
||||
Symbol key('x',1);
|
||||
auto model = noiseModel::Isotropic::Sigma(1, 1 * degree);
|
||||
Symbol key('x', 1);
|
||||
|
||||
/**
|
||||
* Step 2: Create a graph container and add the factor to it
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file SteroVOExample.cpp
|
||||
* @file StereoVOExample.cpp
|
||||
* @brief A stereo visual odometry example
|
||||
* @date May 25, 2014
|
||||
* @author Stephen Camp
|
||||
|
|
@ -34,17 +34,17 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
int main(int argc, char** argv){
|
||||
|
||||
//create graph object, add first pose at origin with key '1'
|
||||
int main(int argc, char** argv) {
|
||||
// create graph object, add first pose at origin with key '1'
|
||||
NonlinearFactorGraph graph;
|
||||
Pose3 first_pose;
|
||||
graph.emplace_shared<NonlinearEquality<Pose3> >(1, Pose3());
|
||||
|
||||
//create factor noise model with 3 sigmas of value 1
|
||||
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
|
||||
//create stereo camera calibration object with .2m between cameras
|
||||
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
|
||||
// create factor noise model with 3 sigmas of value 1
|
||||
const auto model = noiseModel::Isotropic::Sigma(3, 1);
|
||||
// create stereo camera calibration object with .2m between cameras
|
||||
const Cal3_S2Stereo::shared_ptr K(
|
||||
new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
|
||||
|
||||
//create and add stereo factors between first pose (key value 1) and the three landmarks
|
||||
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(520, 480, 440), model, 1, 3, K);
|
||||
|
|
@ -56,17 +56,18 @@ int main(int argc, char** argv){
|
|||
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(70, 20, 490), model, 2, 4, K);
|
||||
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(320, 270, 115), model, 2, 5, K);
|
||||
|
||||
//create Values object to contain initial estimates of camera poses and landmark locations
|
||||
// create Values object to contain initial estimates of camera poses and
|
||||
// landmark locations
|
||||
Values initial_estimate;
|
||||
|
||||
//create and add iniital estimates
|
||||
// create and add iniital estimates
|
||||
initial_estimate.insert(1, first_pose);
|
||||
initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1)));
|
||||
initial_estimate.insert(3, Point3(1, 1, 5));
|
||||
initial_estimate.insert(4, Point3(-1, 1, 5));
|
||||
initial_estimate.insert(5, Point3(0, -0.5, 5));
|
||||
|
||||
//create Levenberg-Marquardt optimizer for resulting factor graph, optimize
|
||||
// create Levenberg-Marquardt optimizer for resulting factor graph, optimize
|
||||
LevenbergMarquardtOptimizer optimizer(graph, initial_estimate);
|
||||
Values result = optimizer.optimize();
|
||||
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file SteroVOExample.cpp
|
||||
* @file StereoVOExample_large.cpp
|
||||
* @brief A stereo visual odometry example
|
||||
* @date May 25, 2014
|
||||
* @author Stephen Camp
|
||||
|
|
@ -41,31 +41,31 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
int main(int argc, char** argv){
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
Values initial_estimate;
|
||||
NonlinearFactorGraph graph;
|
||||
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
|
||||
const auto model = noiseModel::Isotropic::Sigma(3, 1);
|
||||
|
||||
string calibration_loc = findExampleDataFile("VO_calibration.txt");
|
||||
string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
|
||||
string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
|
||||
|
||||
//read camera calibration info from file
|
||||
// read camera calibration info from file
|
||||
// focal lengths fx, fy, skew s, principal point u0, v0, baseline b
|
||||
double fx, fy, s, u0, v0, b;
|
||||
ifstream calibration_file(calibration_loc.c_str());
|
||||
cout << "Reading calibration info" << endl;
|
||||
calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
|
||||
|
||||
//create stereo camera calibration object
|
||||
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b));
|
||||
// create stereo camera calibration object
|
||||
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, s, u0, v0, b));
|
||||
|
||||
ifstream pose_file(pose_loc.c_str());
|
||||
cout << "Reading camera poses" << endl;
|
||||
int pose_id;
|
||||
MatrixRowMajor m(4,4);
|
||||
//read camera pose parameters and use to make initial estimates of camera poses
|
||||
MatrixRowMajor m(4, 4);
|
||||
// read camera pose parameters and use to make initial estimates of camera
|
||||
// poses
|
||||
while (pose_file >> pose_id) {
|
||||
for (int i = 0; i < 16; i++) {
|
||||
pose_file >> m.data()[i];
|
||||
|
|
@ -76,33 +76,37 @@ int main(int argc, char** argv){
|
|||
// camera and landmark keys
|
||||
size_t x, l;
|
||||
|
||||
// pixel coordinates uL, uR, v (same for left/right images due to rectification)
|
||||
// landmark coordinates X, Y, Z in camera frame, resulting from triangulation
|
||||
// pixel coordinates uL, uR, v (same for left/right images due to
|
||||
// rectification) landmark coordinates X, Y, Z in camera frame, resulting from
|
||||
// triangulation
|
||||
double uL, uR, v, X, Y, Z;
|
||||
ifstream factor_file(factor_loc.c_str());
|
||||
cout << "Reading stereo factors" << endl;
|
||||
//read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
|
||||
// read stereo measurement details from file and use to create and add
|
||||
// GenericStereoFactor objects to the graph representation
|
||||
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
|
||||
graph.emplace_shared<
|
||||
GenericStereoFactor<Pose3, Point3> >(StereoPoint2(uL, uR, v), model,
|
||||
Symbol('x', x), Symbol('l', l), K);
|
||||
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
|
||||
graph.emplace_shared<GenericStereoFactor<Pose3, Point3> >(
|
||||
StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K);
|
||||
// if the landmark variable included in this factor has not yet been added
|
||||
// to the initial variable value estimate, add it
|
||||
if (!initial_estimate.exists(Symbol('l', l))) {
|
||||
Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
|
||||
//transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space
|
||||
// transformFrom() transforms the input Point3 from the camera pose space,
|
||||
// camPose, to the global space
|
||||
Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z));
|
||||
initial_estimate.insert(Symbol('l', l), worldPoint);
|
||||
}
|
||||
}
|
||||
|
||||
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
|
||||
//constrain the first pose such that it cannot change from its original value during optimization
|
||||
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x', 1));
|
||||
// constrain the first pose such that it cannot change from its original value
|
||||
// during optimization
|
||||
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
|
||||
// QR is much slower than Cholesky, but numerically more stable
|
||||
graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x',1),first_pose);
|
||||
graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x', 1), first_pose);
|
||||
|
||||
cout << "Optimizing" << endl;
|
||||
//create Levenberg-Marquardt optimizer to optimize the factor graph
|
||||
// create Levenberg-Marquardt optimizer to optimize the factor graph
|
||||
LevenbergMarquardtParams params;
|
||||
params.orderingType = Ordering::METIS;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, initial_estimate, params);
|
||||
|
|
|
|||
|
|
@ -56,13 +56,11 @@ 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 noise = //
|
||||
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
|
||||
auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
|
||||
|
||||
// Create the set of ground-truth landmarks
|
||||
vector<Point3> points = createPoints();
|
||||
|
|
@ -81,7 +79,6 @@ int main(int argc, char* argv[]) {
|
|||
|
||||
// Loop over the different poses, adding the observations to iSAM incrementally
|
||||
for (size_t i = 0; i < poses.size(); ++i) {
|
||||
|
||||
// Add factors for each landmark observation
|
||||
for (size_t j = 0; j < points.size(); ++j) {
|
||||
// Create ground truth measurement
|
||||
|
|
@ -105,12 +102,12 @@ int main(int argc, char* argv[]) {
|
|||
// adding it to iSAM.
|
||||
if (i == 0) {
|
||||
// Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
|
||||
auto poseNoise = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
|
||||
graph.addPrior(Symbol('x', 0), poses[0], poseNoise);
|
||||
|
||||
// Add a prior on landmark l0
|
||||
noiseModel::Isotropic::shared_ptr pointNoise =
|
||||
auto pointNoise =
|
||||
noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
graph.addPrior(Symbol('l', 0), points[0], pointNoise);
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue