IncrementalFixedLagSmootherExample.cpp File Reference

#include <iostream>
#include <string>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>

Include dependency graph for IncrementalFixedLagSmootherExample.cpp:

Go to the source code of this file.

Typedefs
typedef Eigen::Matrix< double, 6, 6 >	Matrix6

Enumerations
enum	FactorType { PRIOR = 0, BETWEEN = 1 }

Functions
Pose3	createPose (double x, double y, double z, double roll, double pitch, double yaw)
	Create a Pose3 object from individual pose parameters. More...
int	main ()
	Main function: Reads poses data from a text file and performs incremental fixed-lag smoothing. More...
Matrix6	readCovarianceMatrix (istringstream &iss)
	Read a 6x6 covariance matrix from an input string stream. More...
void	saveG2oGraph (const NonlinearFactorGraph &graph, const Values &estimate, const string &filename, int iterCount)
	Save the factor graph and estimates to a .g2o file (for visualization/debugging). More...

Typedef Documentation

Matrix6

typedef Eigen::Matrix<double, 6, 6> Matrix6

Definition at line 77 of file IncrementalFixedLagSmootherExample.cpp.

Enumeration Type Documentation

FactorType

enum FactorType

Enumerator
PRIOR
BETWEEN

Definition at line 72 of file IncrementalFixedLagSmootherExample.cpp.

Function Documentation

createPose()

Pose3 createPose	(	double	x,
		double	y,
		double	z,
		double	roll,
		double	pitch,
		double	yaw
	)

Create a Pose3 object from individual pose parameters.

Parameters

x	Translation in x
y	Translation in y
z	Translation in z
roll	Rotation about x-axis
pitch	Rotation about y-axis
yaw	Rotation about z-axis

Returns

Constructed Pose3 object

Definition at line 108 of file IncrementalFixedLagSmootherExample.cpp.

main()

int main

(

)

Main function: Reads poses data from a text file and performs incremental fixed-lag smoothing.

Data Flow: 1) Parse lines from "IncrementalFixedLagSmootherTestData.txt". 2) For each line:

• If it's a PRIOR factor, add a PriorFactor with a specified pose and covariance.
• If it's a BETWEEN factor, add a BetweenFactor with a relative pose and covariance.
• Insert new variables with initial guesses into the current solution if they don't exist. 3) Update the fixed-lag smoother (with iSAM2 inside) to incrementally optimize and marginalize out old poses beyond the specified lag window. 4) Repeat until all lines are processed. 5) Save the resulting factor graph and estimate of the last sliding window to a .g2o file.

Format: PRIOR factor factor_type timestamp key pose(x y z roll pitch yaw) cov(6x6)

Format: BETWEEN factor factor_type timestamp key1 key2 pose(x y z roll pitch yaw) cov(6x6)

Definition at line 147 of file IncrementalFixedLagSmootherExample.cpp.

readCovarianceMatrix()

Matrix6 readCovarianceMatrix

(

istringstream &

iss

)

Read a 6x6 covariance matrix from an input string stream.

Parameters

iss	Input string stream containing covariance entries.

Returns

6x6 covariance matrix.

Definition at line 86 of file IncrementalFixedLagSmootherExample.cpp.

saveG2oGraph()

void saveG2oGraph	(	const NonlinearFactorGraph &	graph,
		const Values &	estimate,
		const string &	filename,
		int	iterCount
	)

Save the factor graph and estimates to a .g2o file (for visualization/debugging).

Parameters

graph	The factor graph
estimate	Current estimates of all variables
filename	Base filename for saving
iterCount	Iteration count to differentiate successive outputs

Definition at line 121 of file IncrementalFixedLagSmootherExample.cpp.