add better visualSLAM examples
Duy-Nguyen Ta
parent
be7828b8cf
commit
4eee4b72f4
|
|
@ -0,0 +1,91 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 VisualSLAMSimulatedData.cpp
|
||||
* @brief Generate ground-truth simulated data for VisualSLAM examples (SFM and ISAM2)
|
||||
* @author Duy-Nguyen Ta
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <gtsam/geometry/SimpleCamera.h>
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* Simulated data for the example:
|
||||
* - 8 Landmarks: (10,10,10) (-10,10,10) (-10,-10,10) (10,-10,10)
|
||||
* (10,10,-10) (-10,10,-10) (-10,-10,-10) (10,-10,-10)
|
||||
* - n 90-deg-FoV cameras with the same calibration parameters:
|
||||
* f = 50.0, Image: 100x100, center: 50.0, 50.0
|
||||
* and ground-truth poses on a circle around the landmarks looking at the world's origin:
|
||||
* Rot3(-sin(theta), 0, -cos(theta),
|
||||
* cos(theta), 0, -sin(theta),
|
||||
* 0, -1, 0 ),
|
||||
* Point3(r*cos(theta), r*sin(theta), 0.0)
|
||||
* (theta += 2*pi/N)
|
||||
* - Measurement noise: 1 pix sigma
|
||||
*/
|
||||
struct VisualSLAMExampleData {
|
||||
gtsam::shared_ptrK sK; // camera calibration parameters
|
||||
std::vector<gtsam::Pose3> poses; // ground-truth camera poses
|
||||
gtsam::Pose3 odometry; // ground-truth odometry between 2 consecutive poses (simulated data for iSAM)
|
||||
std::vector<gtsam::Point3> landmarks; // ground-truth landmarks
|
||||
std::map<int, vector<gtsam::Point2> > z; // 2D measurements of landmarks in each camera frame
|
||||
gtsam::SharedDiagonal noiseZ; // measurement noise (noiseModel::Isotropic::Sigma(2, 5.0f));
|
||||
gtsam::SharedDiagonal noiseX; // noise for camera poses
|
||||
gtsam::SharedDiagonal noiseL; // noise for landmarks
|
||||
|
||||
static const VisualSLAMExampleData generate() {
|
||||
VisualSLAMExampleData data;
|
||||
// Landmarks (ground truth)
|
||||
data.landmarks.push_back(gtsam::Point3(10.0,10.0,10.0));
|
||||
data.landmarks.push_back(gtsam::Point3(-10.0,10.0,10.0));
|
||||
data.landmarks.push_back(gtsam::Point3(-10.0,-10.0,10.0));
|
||||
data.landmarks.push_back(gtsam::Point3(10.0,-10.0,10.0));
|
||||
data.landmarks.push_back(gtsam::Point3(10.0,10.0,-10.0));
|
||||
data.landmarks.push_back(gtsam::Point3(-10.0,10.0,-10.0));
|
||||
data.landmarks.push_back(gtsam::Point3(-10.0,-10.0,-10.0));
|
||||
data.landmarks.push_back(gtsam::Point3(10.0,-10.0,-10.0));
|
||||
|
||||
// Camera calibration parameters
|
||||
data.sK = gtsam::shared_ptrK(new gtsam::Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
|
||||
|
||||
// n camera poses
|
||||
int n = 8;
|
||||
double theta = 0.0;
|
||||
double r = 30.0;
|
||||
for (int i=0; i<n; ++i) {
|
||||
theta += 2*M_PI/n;
|
||||
data.poses.push_back(gtsam::Pose3(
|
||||
gtsam::Rot3(-sin(theta), 0.0, -cos(theta),
|
||||
cos(theta), 0.0, -sin(theta),
|
||||
0.0, -1.0, 0.0),
|
||||
gtsam::Point3(r*cos(theta), r*sin(theta), 0.0)));
|
||||
}
|
||||
data.odometry = data.poses[0].between(data.poses[1]);
|
||||
|
||||
// Simulated measurements with Gaussian noise
|
||||
data.noiseZ = gtsam::sharedSigma(2, 1.0);
|
||||
for (size_t i=0; i<data.poses.size(); ++i) {
|
||||
for (size_t j=0; j<data.landmarks.size(); ++j) {
|
||||
gtsam::SimpleCamera camera(data.poses[i], *data.sK);
|
||||
data.z[i].push_back(camera.project(data.landmarks[j]) + gtsam::Point2(data.noiseZ->sample()));
|
||||
}
|
||||
}
|
||||
data.noiseX = gtsam::sharedSigmas(gtsam::Vector_(6, 0.01, 0.01, 0.01, 0.1, 0.1, 0.1));
|
||||
data.noiseL = gtsam::sharedSigma(3, 0.1);
|
||||
|
||||
return data;
|
||||
}
|
||||
};
|
||||
|
|
@ -0,0 +1,64 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 VisualSLAMforSFMExample.cpp
|
||||
* @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset
|
||||
* @author Duy-Nguyen Ta
|
||||
*/
|
||||
|
||||
#include <gtsam/slam/visualSLAM.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
#include "VisualSLAMExampleData.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// Convenience for named keys
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::L;
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main(int argc, char* argv[]) {
|
||||
|
||||
VisualSLAMExampleData data = VisualSLAMExampleData::generate();
|
||||
|
||||
/* 1. Create graph *///using the 2D measurements (features) and the calibration data
|
||||
visualSLAM::Graph graph;
|
||||
|
||||
/* 2. Add factors to the graph */
|
||||
// 2a. Measurement factors
|
||||
for (size_t i=0; i<data.poses.size(); ++i) {
|
||||
for (size_t j=0; j<data.landmarks.size(); ++j)
|
||||
graph.addMeasurement(data.z[i][j], data.noiseZ, X(i), L(j), data.sK);
|
||||
}
|
||||
// 2b. Prior factor for the first pose to resolve ambiguity (not needed for LevenbergMarquardtOptimizer)
|
||||
graph.addPosePrior(X(0), data.poses[0], data.noiseX);
|
||||
|
||||
/* 3. Initial estimates for variable nodes, simulated by Gaussian noises */
|
||||
Values initial;
|
||||
for (size_t i=0; i<data.poses.size(); ++i)
|
||||
initial.insert(X(i), data.poses[i]*Pose3::Expmap(data.noiseX->sample()));
|
||||
for (size_t j=0; j<data.landmarks.size(); ++j)
|
||||
initial.insert(L(j), data.landmarks[j] + Point3(data.noiseL->sample()));
|
||||
initial.print("Intial Estimates: ");
|
||||
|
||||
/* 4. Optimize the graph and print results */
|
||||
visualSLAM::Values result = GaussNewtonOptimizer(graph, initial).optimize();
|
||||
// visualSLAM::Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
|
||||
result.print("Final results: ");
|
||||
|
||||
return 0;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -0,0 +1,113 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 VisualSLAMwISAM2Example.cpp
|
||||
* @brief An ISAM example for synthesis sequence, single camera
|
||||
* @author Duy-Nguyen Ta
|
||||
*/
|
||||
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/nonlinear/NonlinearISAM.h>
|
||||
#include <gtsam/slam/visualSLAM.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include "VisualSLAMExampleData.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// Convenience for named keys
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::L;
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main(int argc, char* argv[]) {
|
||||
|
||||
VisualSLAMExampleData data = VisualSLAMExampleData::generate();
|
||||
|
||||
/* 1. Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates */
|
||||
int relinearizeInterval = 3;
|
||||
NonlinearISAM<> isam(relinearizeInterval);
|
||||
|
||||
/* 2. At each frame (poseId) with new camera pose and set of associated measurements,
|
||||
* create a graph of new factors and update ISAM */
|
||||
|
||||
// Store the current best estimate from ISAM
|
||||
Values currentEstimate;
|
||||
|
||||
// First two frames:
|
||||
// Add factors and initial values for the first two poses and landmarks then update ISAM.
|
||||
// Note: measurements from the first pose only are not enough to update ISAM:
|
||||
// the system is underconstrained.
|
||||
{
|
||||
visualSLAM::Graph newFactors;
|
||||
|
||||
// First pose with prior factor
|
||||
newFactors.addPosePrior(X(0), data.poses[0], data.noiseX);
|
||||
|
||||
// Second pose with odometry measurement, simulated by adding Gaussian noise to the ground-truth.
|
||||
Pose3 odoMeasurement = data.odometry*Pose3::Expmap(data.noiseX->sample());
|
||||
newFactors.push_back( boost::shared_ptr<BetweenFactor<Pose3> >(
|
||||
new BetweenFactor<Pose3>(X(0), X(1), odoMeasurement, data.noiseX)));
|
||||
|
||||
// Visual measurements at both poses
|
||||
for (size_t i=0; i<2; ++i) {
|
||||
for (size_t j=0; j<data.z[i].size(); ++j) {
|
||||
newFactors.addMeasurement(data.z[i][j], data.noiseZ, X(i), L(j), data.sK);
|
||||
}
|
||||
}
|
||||
|
||||
// Initial values for the first two poses, simulated with Gaussian noise
|
||||
Values initials;
|
||||
Pose3 pose0Init = data.poses[0]*Pose3::Expmap(data.noiseX->sample());
|
||||
initials.insert(X(0), pose0Init);
|
||||
initials.insert(X(1), pose0Init*odoMeasurement);
|
||||
|
||||
// Initial values for the landmarks, simulated with Gaussian noise
|
||||
for (size_t j=0; j<data.landmarks.size(); ++j)
|
||||
initials.insert(L(j), data.landmarks[j] + Point3(data.noiseL->sample()));
|
||||
|
||||
// Update ISAM the first time and obtain the current estimate
|
||||
isam.update(newFactors, initials);
|
||||
currentEstimate = isam.estimate();
|
||||
cout << "Pose 0 and 1: " << endl;
|
||||
currentEstimate.print("Current estimate: ");
|
||||
}
|
||||
|
||||
// Subsequent frames: Add new odometry and measurement factors and initial values,
|
||||
// then update ISAM at each frame
|
||||
for (size_t i=2; i<data.poses.size(); ++i) {
|
||||
visualSLAM::Graph newFactors;
|
||||
// Factor for odometry measurements, simulated by adding Gaussian noise to the ground-truth.
|
||||
Pose3 odoMeasurement = data.odometry*Pose3::Expmap(data.noiseX->sample());
|
||||
newFactors.push_back( boost::shared_ptr<BetweenFactor<Pose3> >(
|
||||
new BetweenFactor<Pose3>(X(i-1), X(i), odoMeasurement, data.noiseX)));
|
||||
// Factors for visual measurements
|
||||
for (size_t j=0; j<data.z[i].size(); ++j) {
|
||||
newFactors.addMeasurement(data.z[i][j], data.noiseZ, X(i), L(j), data.sK);
|
||||
}
|
||||
|
||||
// Initial estimates for the new node Xi, simulated by Gaussian noises
|
||||
Values initials;
|
||||
initials.insert(X(i), currentEstimate.at<Pose3>(X(i-1))*odoMeasurement);
|
||||
|
||||
// update ISAM
|
||||
isam.update(newFactors, initials);
|
||||
currentEstimate = isam.estimate();
|
||||
cout << "****************************************************" << endl;
|
||||
cout << "Pose " << i << ": " << endl;
|
||||
currentEstimate.print("Current estimate: ");
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
Loading…
Reference in New Issue