g2o Namespace Reference

Namespaces
	csparse_extension
	deprecated
	internal
	opengl
	tutorial
	types_icp
	types_six_dof_expmap

Classes
class	AbstractHyperGraphElementCreator
	Abstract interface for allocating HyperGraphElement. More...
class	AbstractOptimizationAlgorithmCreator
	base for allocating an optimization algorithm More...
class	AbstractRobustKernelCreator
	Abstract interface for allocating a robust kernel. More...
struct	BackBoneTreeAction
class	BaseBinaryEdge
class	BaseEdge
class	BaseEdge<-1, E >
class	BaseMultiEdge
	base class to represent an edge connecting an arbitrary number of nodes More...
class	BaseMultiEdge<-1, E >
class	BaseProperty
class	BaseRobot
class	BaseSensor
class	BaseUnaryEdge
class	BaseVertex
	Templatized BaseVertex. More...
class	BaseWorldObject
class	BinarySensor
class	BlockSolver
	Implementation of a solver operating on the blocks of the Hessian. More...
class	BlockSolverBase
	base for the block solvers with some basic function interfaces More...
struct	BlockSolverTraits
	traits to summarize the properties of the fixed size optimization problem More...
struct	BlockSolverTraits< Eigen::Dynamic, Eigen::Dynamic >
	traits to summarize the properties of the dynamic size optimization problem More...
class	Cache
class	CacheCamera
class	CacheContainer
class	CacheSE2Offset
	caching the offset related to a vertex More...
class	CacheSE3Offset
	caching the offset related to a vertex More...
class	CameraParameters
struct	CholmodExt
	Our extension of the CHOLMOD matrix struct. More...
class	CholmodSolverCreator
class	ClosedFormCalibration
	Simultaneous calibration of the laser offest and the parameters of a differential drive. More...
struct	CmpPairFirst
struct	ColSort
class	CommandArgs
	Command line parsing of argc and argv. More...
struct	CSparseExt
	Our C++ version of the csparse struct. More...
class	CSparseSolverCreator
class	DataQueue
	a simple queue to store data and retrieve based on a timestamp More...
class	DenseSolverCreator
class	DlWrapper
	Loading libraries during run-time. More...
class	DrawAction
	draw actions More...
class	Edge_V_V_GICP
class	Edge_XYZ_VSC
	Point vertex, XYZ, is in types_sba. More...
struct	EdgeCreator
class	EdgeGICP
struct	EdgeLabeler
class	EdgeLine2D
class	EdgeLine2DPointXY
class	EdgeLine3D
class	EdgeNormal
class	EdgePlane
class	EdgePointXY
class	EdgePointXYZ
class	EdgeProjectP2MC
class	EdgeProjectP2MC_Intrinsics
class	EdgeProjectP2SC
class	EdgeProjectPSI2UV
class	EdgeProjectXYZ2UV
class	EdgeProjectXYZ2UVU
class	EdgeSBACam
	3D edge between two SBAcam More...
class	EdgeSBAScale
	edge between two SBAcam that specifies the distance between them More...
class	EdgeSE2
	2D edge between two Vertex2 More...
class	EdgeSE2Line2D
class	EdgeSE2LotsOfXY
class	EdgeSE2OdomDifferentialCalib
class	EdgeSE2Offset
	Offset edge. More...
class	EdgeSE2PointXY
class	EdgeSE2PointXYBearing
class	EdgeSE2PointXYBearingWriteGnuplotAction
class	EdgeSE2PointXYCalib
	Landmark measurement that also calibrates an offset for the landmark measurement. More...
class	EdgeSE2PointXYOffset
class	EdgeSE2PointXYWriteGnuplotAction
class	EdgeSE2Prior
	Prior for a two D pose. More...
class	EdgeSE2PureCalib
	calibrate odometry and laser based on a set of measurements More...
class	EdgeSE2Segment2D
class	EdgeSE2Segment2DLine
class	EdgeSE2Segment2DPointLine
class	EdgeSE2SensorCalib
	scanmatch measurement that also calibrates an offset for the laser More...
class	EdgeSE2TwoPointsXY
class	EdgeSE2WriteGnuplotAction
class	EdgeSE2XYPrior
	Prior for a two D pose with constraints only in xy direction (like gps) More...
class	EdgeSE3
	Edge between two 3D pose vertices. More...
class	EdgeSE3Calib
	Landmark measurement that also calibrates an offset for the landmark measurement. More...
class	EdgeSE3Euler
	3D edge between two VertexSE3, uses the euler angle parameterization for the read/write functions only. More...
class	EdgeSE3Expmap
	6D edge between two Vertex6 More...
class	EdgeSE3Line3D
class	EdgeSE3LotsOfXYZ
class	EdgeSE3Offset
	Offset edge. More...
class	EdgeSE3PlaneSensorCalib
	plane measurement that also calibrates an offset for the sensor More...
class	EdgeSE3PointXYZ
	g2o edge from a track to a point node More...
class	EdgeSE3PointXYZDepth
class	EdgeSE3PointXYZDisparity
	edge from a track to a depth camera node using a disparity measurement More...
class	EdgeSE3Prior
	prior for an SE3 element More...
class	EdgeSE3WriteGnuplotAction
	Output the pose-pose constraint to Gnuplot data file. More...
class	EdgeSim3
	7D edge between two Vertex7 More...
class	EdgeSim3ProjectXYZ
struct	EdgeTypesCostFunction
class	EigenSolverCreator
class	EstimatePropagator
	propagation of an initial guess More...
class	EstimatePropagatorCost
	cost for traversing along active edges in the optimizer More...
class	EstimatePropagatorCostOdometry
	cost for traversing only odometry edges. More...
class	Factory
	create vertices and edges based on TAGs in, for example, a file More...
struct	ForceLinker
struct	G2OBatchStatistics
	statistics about the optimization More...
class	G2oQGLViewer
	OpenGL based viewer for the graph. More...
class	G2oSlamInterface
class	GaussianSampler
class	Gm2dlIO
	read / write gm2dl file into / out of a SparseOptimizer More...
class	GuiHyperGraphAction
	action which calls an GUI update after each iteration More...
struct	HyperDijkstra
class	HyperGraph
class	HyperGraphAction
	Abstract action that operates on an entire graph. More...
class	HyperGraphActionLibrary
	library of actions, indexed by the action name; More...
class	HyperGraphElementAction
	Abstract action that operates on a graph entity. More...
class	HyperGraphElementActionCollection
	collection of actions More...
class	HyperGraphElementCreator
	templatized creator class which creates graph elements More...
class	JacobianWorkspace
	provide memory workspace for computing the Jacobians More...
struct	LaserParameters
	parameters for a 2D range finder More...
struct	Line2D
class	Line3D
class	LinearSolver
	basic solver for Ax = b More...
class	LinearSolverCCS
	Solver with faster iterating structure for the linear matrix. More...
class	LinearSolverCholmod
	basic solver for Ax = b which has to reimplemented for different linear algebra libraries More...
class	LinearSolverCholmodOnline
	linear solver which allows to update the cholesky factor More...
class	LinearSolverCholmodOnlineInterface
	generic interface for the online solver More...
class	LinearSolverCSparse
	linear solver which uses CSparse More...
class	LinearSolverDense
	linear solver using dense cholesky decomposition More...
class	LinearSolverEigen
	linear solver which uses the sparse Cholesky solver from Eigen More...
class	LinearSolverPCG
	linear solver using PCG, pre-conditioner is block Jacobi More...
class	MarginalCovarianceCholesky
	computing the marginal covariance given a cholesky factor (lower triangle of the factor) More...
struct	MatrixElem
class	MatrixStructure
	representing the structure of a matrix in column compressed structure (only the upper triangular part of the matrix) More...
struct	MotionInformation
class	MotionMeasurement
	A 2D odometry measurement expressed as a transformation. More...
struct	OdomAndLaserMotion
class	OdomConvert
	convert between the different types of odometry measurements More...
class	OnlineEdgeSE2
class	OnlineEdgeSE3
class	OnlineVertexSE2
class	OnlineVertexSE3
class	OpenMPMutex
struct	OptimizableGraph
class	OptimizationAlgorithm
	Generic interface for a non-linear solver operating on a graph. More...
class	OptimizationAlgorithmDogleg
	Implementation of Powell's Dogleg Algorithm. More...
class	OptimizationAlgorithmFactory
	create solvers based on their short name More...
class	OptimizationAlgorithmGaussNewton
	Implementation of the Gauss Newton Algorithm. More...
class	OptimizationAlgorithmLevenberg
	Implementation of the Levenberg Algorithm. More...
struct	OptimizationAlgorithmProperty
	describe the properties of a solver More...
class	OptimizationAlgorithmWithHessian
	Base for solvers operating on the approximated Hessian, e.g., Gauss-Newton, Levenberg. More...
class	Parameter
class	ParameterCamera
	parameters for a camera More...
class	ParameterContainer
	map id to parameters More...
class	ParameterSE2Offset
	offset for an SE2 More...
class	ParameterSE3Offset
	offset for an SE3 More...
class	ParameterStereoCamera
	parameters for a camera More...
class	PCGSolverCreator
class	Plane3D
class	PointSensorParameters
class	Property
class	PropertyMap
	a collection of properties mapping from name to the property itself More...
class	RawLaser
	Raw laser measuerement. More...
class	RegisterActionProxy
class	RegisterOptimizationAlgorithmProxy
class	RegisterRobustKernelProxy
class	RegisterTypeProxy
class	Robot
class	RobotData
	data recorded by the robot More...
class	RobotLaser
	laser measurement obtained by a robot More...
class	RobustKernel
	base for all robust cost functions More...
class	RobustKernelCauchy
	Cauchy cost function. More...
class	RobustKernelCreator
	templatized creator class which creates graph elements More...
class	RobustKernelDCS
	Dynamic covariance scaling - DCS. More...
class	RobustKernelFactory
	create robust kernels based on their human readable name More...
class	RobustKernelFair
	Fair cost function. More...
class	RobustKernelGemanMcClure
	Geman-McClure cost function. More...
class	RobustKernelHuber
	Huber Cost Function. More...
class	RobustKernelPseudoHuber
	Pseudo Huber Cost Function. More...
class	RobustKernelSaturated
	Saturated cost function. More...
class	RobustKernelScaleDelta
	scale a robust kernel to another delta (window size) More...
class	RobustKernelTukey
	Tukey Cost Function. More...
class	RobustKernelWelsch
	Welsch cost function. More...
class	RunG2OViewer
	wrapper for running the g2o viewer More...
class	Sampler
class	SBACam
class	ScopedArray
	a scoped pointer for an array, i.e., array will be deleted on leaving the scope More...
class	ScopedOpenMPMutex
	lock a mutex within a scope More...
class	ScopedPointer
	a scoped pointer for an objectarray, i.e., object will be deleted on leaving the scope More...
class	ScopedTictoc
	Simplify calls to tictoc() for a whole scope. More...
class	ScopeTime
	Class to measure the time spent in a scope. More...
class	SE2
	represent SE2 More...
class	SE3Quat
class	SensorOdometry
class	SensorOdometry2D
class	SensorOdometry3D
class	SensorPointLine3D
class	SensorPointXY
class	SensorPointXYBearing
class	SensorPointXYOffset
class	SensorPointXYZ
class	SensorPointXYZDepth
class	SensorPointXYZDisparity
class	SensorPose2D
class	SensorPose3D
class	SensorPose3DOffset
class	SensorSE3Prior
class	SensorSegment2D
class	SensorSegment2DLine
class	SensorSegment2DPointLine
struct	SigmaPoint
struct	Sim3
class	SLAM2DLinearSolverCreator
class	Slam2DViewer
class	SleepThread
	helper for calling usleep on any system using Qt More...
class	Solver
	Generic interface for a sparse solver operating on a graph which solves one iteration of the linearized objective function. More...
class	SolverSLAM2DLinear
	Implementation of a linear approximation for 2D pose graph SLAM. More...
class	SparseBlockMatrix
	Sparse matrix which uses blocks. More...
class	SparseBlockMatrixCCS
	Sparse matrix which uses blocks. More...
class	SparseBlockMatrixDiagonal
	Sparse matrix which uses blocks on the diagonal. More...
class	SparseBlockMatrixHashMap
	Sparse matrix which uses blocks based on hash structures. More...
class	SparseOptimizer
class	SparseOptimizerIncremental
class	SparseOptimizerOnline
class	SparseOptimizerTerminateAction
	stop iterating based on the gain which is (oldChi - currentChi) / currentChi. More...
struct	Star
class	StructureOnlyCreator
class	StructureOnlySolver
	This is a solver for "structure-only" optimization". More...
class	ThetaTreeAction
	compute the initial guess of theta while travelling along the MST More...
struct	TicTocElement
	Internal structure of the tictoc profiling. More...
struct	TicTocInitializer
	helper for printing the struct at the end of the lifetime of the program More...
class	UnarySensor
struct	UniformCostFunction
class	VelocityMeasurement
	velocity measurement of a differential robot More...
class	VertexCam
	SBACam Vertex, (x,y,z,qw,qx,qy,qz) the parameterization for the increments constructed is a 6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion. qw is assumed to be positive, otherwise there is an ambiguity in qx,qy,qz as a rotation. More...
class	VertexEllipse
	string ellipse to be attached to a vertex More...
class	VertexIntrinsics
	Vertex encoding the intrinsics of the camera fx, fy, cx, xy, baseline;. More...
class	VertexLine2D
class	VertexLine3D
class	VertexOdomDifferentialParams
class	VertexPlane
class	VertexPointXY
class	VertexPointXYWriteGnuplotAction
class	VertexPointXYZ
	Vertex for a tracked point in space. More...
class	VertexPointXYZWriteGnuplotAction
class	VertexSBAPointXYZ
	Point vertex, XYZ. More...
class	VertexSCam
	Stereo camera vertex, derived from SE3 class. Note that we use the actual pose of the vertex as its parameterization, rather than the transform from RW to camera coords. Uses static vars for camera params, so there is a single camera setup. More...
class	VertexSE2
	2D pose Vertex, (x,y,theta) More...
class	VertexSE2WriteGnuplotAction
class	VertexSE3
	3D pose Vertex, represented as an Isometry3D More...
class	VertexSE3Euler
	3D pose Vertex, (x,y,z,roll,pitch,yaw) the internal parameterization is the same as veretx_se3_quat. Only the read/write operations are rewritten to input/output euler angles. More...
class	VertexSE3Expmap
	SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map. More...
class	VertexSE3WriteGnuplotAction
	write the vertex to some Gnuplot data file More...
class	VertexSegment2D
class	VertexSegment2DWriteGnuplotAction
class	VertexSim3Expmap
	Sim3 Vertex, (x,y,z,qw,qx,qy,qz) the parameterization for the increments constructed is a 7d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion. More...
class	VertexTag
	string tag to be attached to a vertex More...
class	World
class	WorldObject
class	WriteGnuplotAction

Typedefs
typedef SigmaPoint< VectorXd >	MySigmaPoint
typedef std::multimap< OptimizableGraph::Vertex *, Star * >	VertexStarMultimap
typedef std::map< OptimizableGraph::Vertex *, Star * >	VertexStarMap
typedef std::set< Star * >	StarSet
typedef std::map< HyperGraph::Edge *, Star * >	EdgeStarMap
typedef WorldObject< VertexSE2 >	WorldObjectSE2
typedef WorldObject< VertexPointXY >	WorldObjectPointXY
typedef WorldObject< VertexSegment2D >	WorldObjectSegment2D
typedef Robot< WorldObjectSE2 >	Robot2D
typedef WorldObject< VertexSE3 >	WorldObjectSE3
typedef WorldObject< VertexPointXYZ >	WorldObjectTrackXYZ
typedef WorldObject< VertexLine3D >	WorldObjectLine3D
typedef Robot< WorldObjectSE3 >	Robot3D
typedef std::vector< G2OBatchStatistics >	BatchStatisticsContainer
typedef BlockSolver< BlockSolverTraits< Eigen::Dynamic, Eigen::Dynamic > >	BlockSolverX
typedef BlockSolver< BlockSolverTraits< 6, 3 > >	BlockSolver_6_3
typedef BlockSolver< BlockSolverTraits< 7, 3 > >	BlockSolver_7_3
typedef BlockSolver< BlockSolverTraits< 3, 2 > >	BlockSolver_3_2
typedef Eigen::Matrix< int, 2, 1, Eigen::ColMajor >	Vector2I
typedef Eigen::Matrix< int, 3, 1, Eigen::ColMajor >	Vector3I
typedef Eigen::Matrix< int, 4, 1, Eigen::ColMajor >	Vector4I
typedef Eigen::Matrix< int, Eigen::Dynamic, 1, Eigen::ColMajor >	VectorXI
typedef Eigen::Matrix< float, 2, 1, Eigen::ColMajor >	Vector2F
typedef Eigen::Matrix< float, 3, 1, Eigen::ColMajor >	Vector3F
typedef Eigen::Matrix< float, 4, 1, Eigen::ColMajor >	Vector4F
typedef Eigen::Matrix< float, Eigen::Dynamic, 1, Eigen::ColMajor >	VectorXF
typedef Eigen::Matrix< double, 2, 1, Eigen::ColMajor >	Vector2D
typedef Eigen::Matrix< double, 3, 1, Eigen::ColMajor >	Vector3D
typedef Eigen::Matrix< double, 4, 1, Eigen::ColMajor >	Vector4D
typedef Eigen::Matrix< double, Eigen::Dynamic, 1, Eigen::ColMajor >	VectorXD
typedef Eigen::Matrix< int, 2, 2, Eigen::ColMajor >	Matrix2I
typedef Eigen::Matrix< int, 3, 3, Eigen::ColMajor >	Matrix3I
typedef Eigen::Matrix< int, 4, 4, Eigen::ColMajor >	Matrix4I
typedef Eigen::Matrix< int, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor >	MatrixXI
typedef Eigen::Matrix< float, 2, 2, Eigen::ColMajor >	Matrix2F
typedef Eigen::Matrix< float, 3, 3, Eigen::ColMajor >	Matrix3F
typedef Eigen::Matrix< float, 4, 4, Eigen::ColMajor >	Matrix4F
typedef Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor >	MatrixXF
typedef Eigen::Matrix< double, 2, 2, Eigen::ColMajor >	Matrix2D
typedef Eigen::Matrix< double, 3, 3, Eigen::ColMajor >	Matrix3D
typedef Eigen::Matrix< double, 4, 4, Eigen::ColMajor >	Matrix4D
typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor >	MatrixXD
typedef Eigen::Transform< double, 2, Eigen::Isometry, Eigen::ColMajor >	Isometry2D
typedef Eigen::Transform< double, 3, Eigen::Isometry, Eigen::ColMajor >	Isometry3D
typedef Eigen::Transform< double, 2, Eigen::Affine, Eigen::ColMajor >	Affine2D
typedef Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor >	Affine3D
typedef std::vector< Parameter * >	ParameterVector
typedef std::shared_ptr< RobustKernel >	RobustKernelPtr
typedef SparseBlockMatrix< MatrixXD >	SparseBlockMatrixXd
typedef std::vector< MotionInformation, Eigen::aligned_allocator< MotionInformation > >	MotionInformationVector
typedef void(*	ForceLinkFunction) (void)
typedef Property< int >	IntProperty
typedef Property< bool >	BoolProperty
typedef Property< float >	FloatProperty
typedef Property< double >	DoubleProperty
typedef Property< std::string >	StringProperty
typedef std::map< std::string, TicTocElement >	TicTocMap
typedef Matrix< double, 6, 1, Eigen::ColMajor >	Vector6d
typedef Eigen::Matrix< double, 6, 6, Eigen::ColMajor >	Matrix6d
typedef Eigen::Matrix< double, 7, 1 >	Vector7d
typedef Eigen::Matrix< double, 7, 7 >	Matrix7d
typedef Eigen::Matrix< double, 7, 6, Eigen::ColMajor >	Matrix7x6d

Enumerations
enum	CommandArgumentType {   CAT_DOUBLE, CAT_FLOAT, CAT_INT, CAT_STRING,   CAT_BOOL, CAT_VECTOR_INT, CAT_VECTOR_DOUBLE }

Functions
void	findArguments (const std::string &option, vector< string > &args, int argc, char **argv)
void	loadStandardTypes (DlWrapper &dlTypesWrapper, int argc, char **argv)
void	loadStandardSolver (DlWrapper &dlSolverWrapper, int argc, char **argv)
bool	edgeAllVertsSameDim (OptimizableGraph::Edge *e, int dim)
bool	saveGnuplot (const std::string &gnudump, const OptimizableGraph &optimizer)
bool	saveGnuplot (const std::string &gnudump, const HyperGraph::VertexSet &vertices, const HyperGraph::EdgeSet &edges)
bool	dumpEdges (std::ostream &os, const OptimizableGraph &optimizer)
double	activeVertexChi (const OptimizableGraph::Vertex *v)
void	constructEdgeStarMap (EdgeStarMap &esmap, StarSet &stars, bool low)
size_t	vertexEdgesInStar (HyperGraph::EdgeSet &eset, HyperGraph::Vertex *v, Star *s, EdgeStarMap &esmap)
void	starsInVertex (StarSet &stars, HyperGraph::Vertex *v, EdgeStarMap &esmap)
void	starsInEdge (StarSet &stars, HyperGraph::Edge *e, EdgeStarMap &esmap, HyperGraph::VertexSet &gauge)
void	assignHierarchicalEdges (StarSet &stars, EdgeStarMap &esmap, EdgeLabeler *labeler, EdgeCreator *creator, SparseOptimizer *optimizer, int minNumEdges, int maxIterations)
void	computeBorder (StarSet &stars, EdgeStarMap &hesmap)
void	computeSimpleStars (StarSet &stars, SparseOptimizer *optimizer, EdgeLabeler *labeler, EdgeCreator *creator, OptimizableGraph::Vertex *gauge_, std::string edgeTag, std::string vertexTag, int level, int step, int backboneIterations, int starIterations, double rejectionThreshold, bool debug)
int	clipSegmentCircle (Eigen::Vector2d &p1, Eigen::Vector2d &p2, double r)
int	clipSegmentLine (Eigen::Vector2d &p1, Eigen::Vector2d &p2, double a, double b, double c)
int	clipSegmentFov (Eigen::Vector2d &p1, Eigen::Vector2d &p2, double min, double max)
Eigen::Vector2d	computeLineParameters (const Eigen::Vector2d &p1, const Eigen::Vector2d &p2)
std::ostream &	operator<< (std::ostream &os, const G2OBatchStatistics &st)
bool	operator< (const HyperDijkstra::AdjacencyMapEntry &a, const HyperDijkstra::AdjacencyMapEntry &b)
void	applyAction (HyperGraph *graph, HyperGraphElementAction *action, HyperGraphElementAction::Parameters *params, const std::string &typeName)
template<class MatrixType >
std::ostream &	operator<< (std::ostream &, const SparseBlockMatrix< MatrixType > &m)
void	addOdometryCalibLinksDifferential (SparseOptimizer &optimizer, const DataQueue &odomData)
void	allocateSolverForSclam (SparseOptimizer &optimizer, bool levenberg)
void	findConnectedEdgesWithCostLimit (HyperGraph::EdgeSet &selected, HyperGraph::EdgeSet &border, HyperGraph::Edge *start, HyperDijkstra::CostFunction *cost, double maxEdgeCost, double comparisonConditioner)
static OptimizationAlgorithm *	createSolver (const std::string &solverName)
	G2O_USE_OPTIMIZATION_LIBRARY (cholmod)
static Solver *	createSolver (const std::string &solverName)
	G2O_ATTRIBUTE_CONSTRUCTOR (init_types_interactive_online)
void	drawDisk (GLfloat radius)
void	drawCircle (GLfloat radius, int segments)
static OptimizationAlgorithm *	createSolver (const std::string &fullSolverName)
	G2O_REGISTER_OPTIMIZATION_LIBRARY (cholmod)
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (gn_var_cholmod, new CholmodSolverCreator(OptimizationAlgorithmProperty("gn_var_cholmod","Gauss-Newton: Cholesky solver using CHOLMOD (variable blocksize)","CHOLMOD", false, Eigen::Dynamic, Eigen::Dynamic)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (gn_fix3_2_cholmod, new CholmodSolverCreator(OptimizationAlgorithmProperty("gn_fix3_2_cholmod","Gauss-Newton: Cholesky solver using CHOLMOD (fixed blocksize)","CHOLMOD", true, 3, 2)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (gn_fix6_3_cholmod, new CholmodSolverCreator(OptimizationAlgorithmProperty("gn_fix6_3_cholmod","Gauss-Newton: Cholesky solver using CHOLMOD (fixed blocksize)","CHOLMOD", true, 6, 3)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (gn_fix7_3_cholmod, new CholmodSolverCreator(OptimizationAlgorithmProperty("gn_fix7_3_cholmod","Gauss-Newton: Cholesky solver using CHOLMOD (fixed blocksize)","CHOLMOD", true, 7, 3)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (lm_var_cholmod, new CholmodSolverCreator(OptimizationAlgorithmProperty("lm_var_cholmod","Levenberg: Cholesky solver using CHOLMOD (variable blocksize)","CHOLMOD", false, Eigen::Dynamic, Eigen::Dynamic)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (lm_fix3_2_cholmod, new CholmodSolverCreator(OptimizationAlgorithmProperty("lm_fix3_2_cholmod","Levenberg: Cholesky solver using CHOLMOD (fixed blocksize)","CHOLMOD", true, 3, 2)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (lm_fix6_3_cholmod, new CholmodSolverCreator(OptimizationAlgorithmProperty("lm_fix6_3_cholmod","Levenberg: Cholesky solver using CHOLMOD (fixed blocksize)","CHOLMOD", true, 6, 3)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (lm_fix7_3_cholmod, new CholmodSolverCreator(OptimizationAlgorithmProperty("lm_fix7_3_cholmod","Levenberg: Cholesky solver using CHOLMOD (fixed blocksize)","CHOLMOD", true, 7, 3)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (dl_var_cholmod, new CholmodSolverCreator(OptimizationAlgorithmProperty("dl_var_cholmod","Dogleg: Cholesky solver using CHOLMOD (variable blocksize)","CHOLMOD", false, Eigen::Dynamic, Eigen::Dynamic)))
static OptimizationAlgorithm *	createSolver (const std::string &fullSolverName)
	G2O_REGISTER_OPTIMIZATION_LIBRARY (csparse)
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (gn_var, new CSparseSolverCreator(OptimizationAlgorithmProperty("gn_var","Gauss-Newton: Cholesky solver using CSparse (variable blocksize)","CSparse", false, Eigen::Dynamic, Eigen::Dynamic)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (gn_fix3_2, new CSparseSolverCreator(OptimizationAlgorithmProperty("gn_fix3_2","Gauss-Newton: Cholesky solver using CSparse (fixed blocksize)","CSparse", true, 3, 2)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (gn_fix6_3, new CSparseSolverCreator(OptimizationAlgorithmProperty("gn_fix6_3","Gauss-Newton: Cholesky solver using CSparse (fixed blocksize)","CSparse", true, 6, 3)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (gn_fix7_3, new CSparseSolverCreator(OptimizationAlgorithmProperty("gn_fix7_3","Gauss-Newton: Cholesky solver using CSparse (fixed blocksize)","CSparse", true, 7, 3)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (lm_var, new CSparseSolverCreator(OptimizationAlgorithmProperty("lm_var","Levenberg: Cholesky solver using CSparse (variable blocksize)","CSparse", false, Eigen::Dynamic, Eigen::Dynamic)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (lm_fix3_2, new CSparseSolverCreator(OptimizationAlgorithmProperty("lm_fix3_2","Levenberg: Cholesky solver using CSparse (fixed blocksize)","CSparse", true, 3, 2)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (lm_fix6_3, new CSparseSolverCreator(OptimizationAlgorithmProperty("lm_fix6_3","Levenberg: Cholesky solver using CSparse (fixed blocksize)","CSparse", true, 6, 3)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (lm_fix7_3, new CSparseSolverCreator(OptimizationAlgorithmProperty("lm_fix7_3","Levenberg: Cholesky solver using CSparse (fixed blocksize)","CSparse", true, 7, 3)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (dl_var, new CSparseSolverCreator(OptimizationAlgorithmProperty("dl_var","Dogleg: Cholesky solver using CSparse (variable blocksize)","CSparse", false, Eigen::Dynamic, Eigen::Dynamic)))
static OptimizationAlgorithm *	createSolver (const std::string &fullSolverName)
	G2O_REGISTER_OPTIMIZATION_LIBRARY (dense)
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (gn_dense, new DenseSolverCreator(OptimizationAlgorithmProperty("gn_dense","Gauss-Newton: Dense solver (variable blocksize)","Dense", false, Eigen::Dynamic, Eigen::Dynamic)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (gn_dense3_2, new DenseSolverCreator(OptimizationAlgorithmProperty("gn_dense3_2","Gauss-Newton: Dense solver (fixed blocksize)","Dense", true, 3, 2)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (gn_dense6_3, new DenseSolverCreator(OptimizationAlgorithmProperty("gn_dense6_3","Gauss-Newton: Dense solver (fixed blocksize)","Dense", true, 6, 3)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (gn_dense7_3, new DenseSolverCreator(OptimizationAlgorithmProperty("gn_dense7_3","Gauss-Newton: Dense solver (fixed blocksize)","Dense", true, 7, 3)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (lm_dense, new DenseSolverCreator(OptimizationAlgorithmProperty("lm_dense","Levenberg: Dense solver (variable blocksize)","Dense", false,-1,-1)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (lm_dense3_2, new DenseSolverCreator(OptimizationAlgorithmProperty("lm_dense3_2","Levenberg: Dense solver (fixed blocksize)","Dense", true, 3, 2)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (lm_dense6_3, new DenseSolverCreator(OptimizationAlgorithmProperty("lm_dense6_3","Levenberg: Dense solver (fixed blocksize)","Dense", true, 6, 3)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (lm_dense7_3, new DenseSolverCreator(OptimizationAlgorithmProperty("lm_dense7_3","Levenberg: Dense solver (fixed blocksize)","Dense", true, 7, 3)))
static OptimizationAlgorithm *	createSolver (const std::string &fullSolverName)
	G2O_REGISTER_OPTIMIZATION_LIBRARY (eigen)
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (gn_var_eigen, new EigenSolverCreator(OptimizationAlgorithmProperty("gn_var_eigen","Gauss-Newton: Cholesky solver using Eigen's Sparse Cholesky methods (variable blocksize)","Eigen", false, Eigen::Dynamic, Eigen::Dynamic)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (lm_var_eigen, new EigenSolverCreator(OptimizationAlgorithmProperty("lm_var_eigen","Levenberg: Cholesky solver using Eigen's Sparse Cholesky methods (variable blocksize)","Eigen", false, Eigen::Dynamic, Eigen::Dynamic)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (dl_var_eigen, new EigenSolverCreator(OptimizationAlgorithmProperty("dl_var_eigen","Dogleg: Cholesky solver using Eigen's Sparse Cholesky methods (variable blocksize)","Eigen", false, Eigen::Dynamic, Eigen::Dynamic)))
static OptimizationAlgorithm *	createSolver (const std::string &fullSolverName)
	G2O_REGISTER_OPTIMIZATION_LIBRARY (pcg)
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (gn_pcg, new PCGSolverCreator(OptimizationAlgorithmProperty("gn_pcg","Gauss-Newton: PCG solver using block-Jacobi pre-conditioner (variable blocksize)","PCG", false, Eigen::Dynamic, Eigen::Dynamic)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (gn_pcg3_2, new PCGSolverCreator(OptimizationAlgorithmProperty("gn_pcg3_2","Gauss-Newton: PCG solver using block-Jacobi pre-conditioner (fixed blocksize)","PCG", true, 3, 2)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (gn_pcg6_3, new PCGSolverCreator(OptimizationAlgorithmProperty("gn_pcg6_3","Gauss-Newton: PCG solver using block-Jacobi pre-conditioner (fixed blocksize)","PCG", true, 6, 3)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (gn_pcg7_3, new PCGSolverCreator(OptimizationAlgorithmProperty("gn_pcg7_3","Gauss-Newton: PCG solver using block-Jacobi pre-conditioner (fixed blocksize)","PCG", true, 7, 3)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (lm_pcg, new PCGSolverCreator(OptimizationAlgorithmProperty("lm_pcg","Levenberg: PCG solver using block-Jacobi pre-conditioner (variable blocksize)","PCG", false, Eigen::Dynamic, Eigen::Dynamic)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (lm_pcg3_2, new PCGSolverCreator(OptimizationAlgorithmProperty("lm_pcg3_2","Levenberg: PCG solver using block-Jacobi pre-conditioner (fixed blocksize)","PCG", true, 3, 2)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (lm_pcg6_3, new PCGSolverCreator(OptimizationAlgorithmProperty("lm_pcg6_3","Levenberg: PCG solver using block-Jacobi pre-conditioner (fixed blocksize)","PCG", true, 6, 3)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (lm_pcg7_3, new PCGSolverCreator(OptimizationAlgorithmProperty("lm_pcg7_3","Levenberg: PCG solver using block-Jacobi pre-conditioner (fixed blocksize)","PCG", true, 7, 3)))
static OptimizationAlgorithm *	createSolver (const std::string &fullSolverName)
	G2O_REGISTER_OPTIMIZATION_LIBRARY (slam2d_linear)
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (2dlinear, new SLAM2DLinearSolverCreator(OptimizationAlgorithmProperty("2dlinear","Solve Orientation + Gauss-Newton: Works only on 2D pose graphs!!","CSparse", false, 3, 3)))
static OptimizationAlgorithm *	createSolver (const std::string &fullSolverName)
	G2O_REGISTER_OPTIMIZATION_LIBRARY (structure_only)
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (structure_only_2, new StructureOnlyCreator(OptimizationAlgorithmProperty("structure_only_2","Optimize the landmark poses (2D)","Eigen", true, 3, 2)))
	G2O_REGISTER_OPTIMIZATION_ALGORITHM (structure_only_3, new StructureOnlyCreator(OptimizationAlgorithmProperty("structure_only_3","Optimize the landmark poses (3D)","Eigen", true, 6, 3)))
std::istream &	operator>> (std::istream &is, std::vector< int > &v)
std::ostream &	operator<< (std::ostream &os, const std::vector< int > &v)
std::istream &	operator>> (std::istream &is, std::vector< double > &v)
std::ostream &	operator<< (std::ostream &os, const std::vector< double > &v)
template<typename T >
bool	convertString (const std::string &s, T &x)
std::string	getFileExtension (const std::string &filename)
std::string	getPureFilename (const std::string &filename)
std::string	getBasename (const std::string &filename)
std::string	getDirname (const std::string &filename)
std::string	changeFileExtension (const std::string &filename, const std::string &newExt, bool stripDot)
bool	fileExists (const char *filename)
std::vector< std::string >	getFilesByPattern (const char *pattern)
template<typename T >
T	square (T x)
template<typename T >
T	hypot (T x, T y)
template<typename T >
T	hypot_sqr (T x, T y)
double	deg2rad (double degree)
double	rad2deg (double rad)
double	normalize_theta (double theta)
double	inverse_theta (double th)
double	average_angle (double theta1, double theta2)
template<typename T >
int	sign (T x)
template<typename T >
T	clamp (T l, T x, T u)
template<typename T >
T	wrap (T l, T x, T u)
bool	arrayHasNaN (const double *array, int size, int *nanIndex=0)
static std::normal_distribution< double >	_univariateSampler (0., 1.)
double	sampleUniform (double min, double max, std::mt19937 *generator)
double	sampleGaussian (std::mt19937 *generator)
bool	writeVector (const string &filename, const double *v, int n)
bool	writeCCSMatrix (const string &filename, int rows, int cols, const int *Ap, const int *Ai, const double *Ax, bool upperTriangleSymmetric)
G2O_STUFF_API bool	writeVector (const std::string &filename, const double *v, int n)
G2O_STUFF_API bool	writeCCSMatrix (const std::string &filename, int rows, int cols, const int *p, const int *i, const double *v, bool upperTriangleSymmetric=true)
std::string	trim (const std::string &s)
std::string	trimLeft (const std::string &s)
std::string	trimRight (const std::string &s)
std::string	strToLower (const std::string &s)
std::string	strToUpper (const std::string &s)
std::string	formatString (const char *fmt,...)
int	strPrintf (std::string &str, const char *fmt,...)
std::string	strExpandFilename (const std::string &filename)
std::vector< std::string >	strSplit (const std::string &str, const std::string &delimiters)
bool	strStartsWith (const std::string &s, const std::string &start)
bool	strEndsWith (const std::string &s, const std::string &end)
int	readLine (std::istream &is, std::stringstream &currentLine)
template<typename OutputIterator >
OutputIterator	readInts (const char *str, OutputIterator out)
template<typename OutputIterator >
OutputIterator	readFloats (const char *str, OutputIterator out)
template<typename T >
bool	convertString (const std::string &s, T &x, bool failIfLeftoverChars=true)
template<typename T >
T	stringToType (const std::string &s, bool failIfLeftoverChars=true)
double	tictoc (const char *algorithmPart)
	Profile the timing of certain parts of your algorithm. More...
double	get_monotonic_time ()
double	get_time ()
template<class SampleType , class CovarianceType >
bool	sampleUnscented (std::vector< SigmaPoint< SampleType >, Eigen::aligned_allocator< SigmaPoint< SampleType > > > &sigmaPoints, const SampleType &mean, const CovarianceType &covariance)
template<class SampleType , class CovarianceType >
void	reconstructGaussian (SampleType &mean, CovarianceType &covariance, const std::vector< SigmaPoint< SampleType >, Eigen::aligned_allocator< SigmaPoint< SampleType > > > &sigmaPoints)
	G2O_REGISTER_TYPE_GROUP (data)
	G2O_REGISTER_TYPE (VERTEX_TAG, VertexTag)
	G2O_REGISTER_TYPE (ROBOTLASER1, RobotLaser)
	G2O_REGISTER_TYPE (VERTEX_ELLIPSE, VertexEllipse)
	G2O_REGISTER_TYPE_GROUP (icp)
	G2O_REGISTER_TYPE (EDGE_V_V_GICP, Edge_V_V_GICP)
	G2O_ATTRIBUTE_CONSTRUCTOR (init_icp_types)
std::ostream &	operator<< (std::ostream &out_str, const SBACam &cam)
	G2O_REGISTER_TYPE_GROUP (sba)
	G2O_REGISTER_TYPE (VERTEX_CAM, VertexCam)
	G2O_REGISTER_TYPE (VERTEX_XYZ, VertexSBAPointXYZ)
	G2O_REGISTER_TYPE (VERTEX_INTRINSICS, VertexIntrinsics)
	G2O_REGISTER_TYPE (EDGE_PROJECT_P2MC, EdgeProjectP2MC)
	G2O_REGISTER_TYPE (EDGE_PROJECT_P2MC_INTRINSICS, EdgeProjectP2MC_Intrinsics)
	G2O_REGISTER_TYPE (EDGE_PROJECT_P2SC, EdgeProjectP2SC)
	G2O_REGISTER_TYPE (EDGE_CAM, EdgeSBACam)
	G2O_REGISTER_TYPE (EDGE_SCALE, EdgeSBAScale)
	G2O_REGISTER_TYPE_GROUP (expmap)
	G2O_REGISTER_TYPE (VERTEX_SE3:EXPMAP, VertexSE3Expmap)
	G2O_REGISTER_TYPE (EDGE_SE3:EXPMAP, EdgeSE3Expmap)
	G2O_REGISTER_TYPE (EDGE_PROJECT_XYZ2UV:EXPMAP, EdgeProjectXYZ2UV)
	G2O_REGISTER_TYPE (EDGE_PROJECT_XYZ2UVU:EXPMAP, EdgeProjectXYZ2UVU)
	G2O_REGISTER_TYPE (PARAMS_CAMERAPARAMETERS, CameraParameters)
Vector2D	project2d (const Vector3D &v)
Vector3D	unproject2d (const Vector2D &v)
Vector3D	invert_depth (const Vector3D &x)
Matrix< double, 2, 3, Eigen::ColMajor >	d_proj_d_y (const double &f, const Vector3D &xyz)
Matrix< double, 3, 6, Eigen::ColMajor >	d_expy_d_y (const Vector3D &y)
Matrix3D	d_Tinvpsi_d_psi (const SE3Quat &T, const Vector3D &psi)
	G2O_USE_TYPE_GROUP (slam2d)
	G2O_REGISTER_TYPE_GROUP (sclam)
	G2O_REGISTER_TYPE (VERTEX_ODOM_DIFFERENTIAL, VertexOdomDifferentialParams)
	G2O_REGISTER_TYPE (EDGE_SE2_CALIB, EdgeSE2SensorCalib)
	G2O_REGISTER_TYPE (EDGE_SE2_ODOM_DIFFERENTIAL_CALIB, EdgeSE2OdomDifferentialCalib)
std::ostream &	operator<< (std::ostream &out_str, const Sim3 &sim3)
	G2O_USE_TYPE_GROUP (sba)
	G2O_REGISTER_TYPE_GROUP (sim3)
	G2O_REGISTER_TYPE (VERTEX_SIM3:EXPMAP, VertexSim3Expmap)
	G2O_REGISTER_TYPE (EDGE_SIM3:EXPMAP, EdgeSim3)
	G2O_REGISTER_TYPE (EDGE_PROJECT_SIM3_XYZ:EXPMAP, EdgeSim3ProjectXYZ)
	G2O_REGISTER_TYPE_GROUP (slam2d)
	G2O_REGISTER_TYPE (VERTEX_SE2, VertexSE2)
	G2O_REGISTER_TYPE (VERTEX_XY, VertexPointXY)
	G2O_REGISTER_TYPE (PARAMS_SE2OFFSET, ParameterSE2Offset)
	G2O_REGISTER_TYPE (CACHE_SE2_OFFSET, CacheSE2Offset)
	G2O_REGISTER_TYPE (EDGE_PRIOR_SE2, EdgeSE2Prior)
	G2O_REGISTER_TYPE (EDGE_PRIOR_SE2_XY, EdgeSE2XYPrior)
	G2O_REGISTER_TYPE (EDGE_SE2, EdgeSE2)
	G2O_REGISTER_TYPE (EDGE_SE2_XY, EdgeSE2PointXY)
	G2O_REGISTER_TYPE (EDGE_BEARING_SE2_XY, EdgeSE2PointXYBearing)
	G2O_REGISTER_TYPE (EDGE_SE2_XY_CALIB, EdgeSE2PointXYCalib)
	G2O_REGISTER_TYPE (EDGE_SE2_OFFSET, EdgeSE2Offset)
	G2O_REGISTER_TYPE (EDGE_SE2_POINTXY_OFFSET, EdgeSE2PointXYOffset)
	G2O_REGISTER_TYPE (EDGE_POINTXY, EdgePointXY)
	G2O_REGISTER_TYPE (EDGE_SE2_TWOPOINTSXY, EdgeSE2TwoPointsXY)
	G2O_REGISTER_TYPE (EDGE_SE2_LOTSOFXY, EdgeSE2LotsOfXY)
	G2O_REGISTER_ACTION (VertexSE2WriteGnuplotAction)
	G2O_REGISTER_ACTION (VertexPointXYWriteGnuplotAction)
	G2O_REGISTER_ACTION (EdgeSE2WriteGnuplotAction)
	G2O_REGISTER_ACTION (EdgeSE2PointXYWriteGnuplotAction)
	G2O_REGISTER_ACTION (EdgeSE2PointXYBearingWriteGnuplotAction)
Line2D	operator* (const SE2 &t, const Line2D &l)
	G2O_REGISTER_TYPE_GROUP (slam2d_segment)
	G2O_REGISTER_TYPE (VERTEX_SEGMENT2D, VertexSegment2D)
	G2O_REGISTER_TYPE (VERTEX_LINE2D, VertexLine2D)
	G2O_REGISTER_TYPE (EDGE_SE2_SEGMENT2D, EdgeSE2Segment2D)
	G2O_REGISTER_TYPE (EDGE_SE2_SEGMENT2D_LINE, EdgeSE2Segment2DLine)
	G2O_REGISTER_TYPE (EDGE_SE2_SEGMENT2D_POINTLINE, EdgeSE2Segment2DPointLine)
	G2O_REGISTER_TYPE (EDGE_SE2_LINE2D, EdgeSE2Line2D)
	G2O_REGISTER_TYPE (EDGE_LINE2D, EdgeLine2D)
	G2O_REGISTER_TYPE (EDGE_LINE2D_POINTXY, EdgeLine2DPointXY)
G2O_TYPES_SLAM3D_API Matrix3D	skew (const Vector3D &v)
G2O_TYPES_SLAM3D_API Vector3D	deltaR (const Matrix3D &R)
G2O_TYPES_SLAM3D_API Vector2D	project (const Vector3D &)
G2O_TYPES_SLAM3D_API Vector3D	project (const Vector4D &)
G2O_TYPES_SLAM3D_API Vector3D	unproject (const Vector2D &)
G2O_TYPES_SLAM3D_API Vector4D	unproject (const Vector3D &)
std::ostream &	operator<< (std::ostream &out_str, const SE3Quat &se3)
	G2O_REGISTER_TYPE_GROUP (slam3d)
	G2O_REGISTER_TYPE (VERTEX_SE3:QUAT, VertexSE3)
	G2O_REGISTER_TYPE (EDGE_SE3:QUAT, EdgeSE3)
	G2O_REGISTER_TYPE (VERTEX_TRACKXYZ, VertexPointXYZ)
	G2O_REGISTER_TYPE (PARAMS_SE3OFFSET, ParameterSE3Offset)
	G2O_REGISTER_TYPE (EDGE_SE3_TRACKXYZ, EdgeSE3PointXYZ)
	G2O_REGISTER_TYPE (EDGE_SE3_PRIOR, EdgeSE3Prior)
	G2O_REGISTER_TYPE (CACHE_SE3_OFFSET, CacheSE3Offset)
	G2O_REGISTER_TYPE (EDGE_SE3_OFFSET, EdgeSE3Offset)
	G2O_REGISTER_TYPE (PARAMS_CAMERACALIB, ParameterCamera)
	G2O_REGISTER_TYPE (PARAMS_STEREOCAMERACALIB, ParameterStereoCamera)
	G2O_REGISTER_TYPE (CACHE_CAMERA, CacheCamera)
	G2O_REGISTER_TYPE (EDGE_PROJECT_DISPARITY, EdgeSE3PointXYZDisparity)
	G2O_REGISTER_TYPE (EDGE_PROJECT_DEPTH, EdgeSE3PointXYZDepth)
	G2O_REGISTER_TYPE (EDGE_POINTXYZ, EdgePointXYZ)
	G2O_REGISTER_ACTION (VertexSE3WriteGnuplotAction)
	G2O_REGISTER_ACTION (VertexPointXYZWriteGnuplotAction)
	G2O_REGISTER_ACTION (EdgeSE3WriteGnuplotAction)
static void	jac_quat3_euler3 (Eigen::Matrix< double, 6, 6, Eigen::ColMajor > &J, const Isometry3D &t)
static void	_skew (Matrix3D &S, const Vector3D &t)
static Matrix3D	_skew (const Vector3D &t)
Line3D	operator* (const Isometry3D &t, const Line3D &line)
Plane3D	operator* (const Eigen::Isometry3d &t, const Plane3D &plane)
	G2O_REGISTER_TYPE_GROUP (slam3d_addons)
	G2O_REGISTER_TYPE (VERTEX3, VertexSE3Euler)
	G2O_REGISTER_TYPE (EDGE3, EdgeSE3Euler)
	G2O_REGISTER_TYPE (VERTEX_PLANE, VertexPlane)
	G2O_REGISTER_TYPE (EDGE_SE3_PLANE_CALIB, EdgeSE3PlaneSensorCalib)
	G2O_REGISTER_TYPE (VERTEX_LINE3D, VertexLine3D)
	G2O_REGISTER_TYPE (EDGE_SE3_LINE3D, EdgeSE3Line3D)
	G2O_REGISTER_TYPE (EDGE_LINE3D, EdgeLine3D)
	G2O_REGISTER_TYPE (EDGE_PLANE, EdgePlane)
	G2O_REGISTER_TYPE (EDGE_SE3_CALIB, EdgeSE3Calib)
	G2O_ATTRIBUTE_CONSTRUCTOR (init_slam3d_addons_types)

Variables
class G2O_CORE_API	OptimizationAlgorithm
class G2O_CORE_API	SparseOptimizer
static const double	INFORMATION_SCALING_ODOMETRY = 100
static std::uniform_real_distribution< double >	_uniformReal
static std::mt19937	_gen_real

Detailed Description

Basic types definitions for SBA translation to HChol

Camera nodes use camera pose in real world v3 position normalized quaternion rotation

Point nodes: v3 position

Typedef Documentation

typedef Eigen::Transform<double,2,Eigen::Affine,Eigen::ColMajor> g2o::Affine2D

Definition at line 68 of file eigen_types.h.

typedef Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> g2o::Affine3D

Definition at line 69 of file eigen_types.h.

typedef std::vector<G2OBatchStatistics> g2o::BatchStatisticsContainer

Definition at line 81 of file batch_stats.h.

typedef BlockSolver< BlockSolverTraits<3, 2> > g2o::BlockSolver_3_2

Definition at line 185 of file block_solver.h.

typedef BlockSolver< BlockSolverTraits<6, 3> > g2o::BlockSolver_6_3

Definition at line 181 of file block_solver.h.

typedef BlockSolver< BlockSolverTraits<7, 3> > g2o::BlockSolver_7_3

Definition at line 183 of file block_solver.h.

typedef BlockSolver< BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> > g2o::BlockSolverX

Definition at line 179 of file block_solver.h.

typedef Property<bool> g2o::BoolProperty

Definition at line 153 of file property.h.

typedef Property<double> g2o::DoubleProperty

Definition at line 155 of file property.h.

typedef std::map<HyperGraph::Edge*, Star*> g2o::EdgeStarMap

Definition at line 72 of file star.h.

typedef Property<float> g2o::FloatProperty

Definition at line 154 of file property.h.

typedef void(* g2o::ForceLinkFunction) (void)

The following two functions are used to force linkage with static libraries.

Definition at line 193 of file misc.h.

typedef Property<int> g2o::IntProperty

Definition at line 152 of file property.h.

typedef Eigen::Transform<double,2,Eigen::Isometry,Eigen::ColMajor> g2o::Isometry2D

Definition at line 65 of file eigen_types.h.

typedef Eigen::Transform<double,3,Eigen::Isometry,Eigen::ColMajor> g2o::Isometry3D

Definition at line 66 of file eigen_types.h.

typedef Eigen::Matrix<double,2,2,Eigen::ColMajor> g2o::Matrix2D

Definition at line 60 of file eigen_types.h.

typedef Eigen::Matrix<float,2,2,Eigen::ColMajor> g2o::Matrix2F

Definition at line 55 of file eigen_types.h.

typedef Eigen::Matrix<int,2,2,Eigen::ColMajor> g2o::Matrix2I

Definition at line 50 of file eigen_types.h.

typedef Eigen::Matrix<double,3,3,Eigen::ColMajor> g2o::Matrix3D

Definition at line 61 of file eigen_types.h.

typedef Eigen::Matrix<float,3,3,Eigen::ColMajor> g2o::Matrix3F

Definition at line 56 of file eigen_types.h.

typedef Eigen::Matrix<int,3,3,Eigen::ColMajor> g2o::Matrix3I

Definition at line 51 of file eigen_types.h.

typedef Eigen::Matrix<double,4,4,Eigen::ColMajor> g2o::Matrix4D

Definition at line 62 of file eigen_types.h.

typedef Eigen::Matrix<float,4,4,Eigen::ColMajor> g2o::Matrix4F

Definition at line 57 of file eigen_types.h.

typedef Eigen::Matrix<int,4,4,Eigen::ColMajor> g2o::Matrix4I

Definition at line 52 of file eigen_types.h.

typedef Eigen::Matrix< double, 6, 6, Eigen::ColMajor > g2o::Matrix6d

Definition at line 41 of file types_six_dof_expmap.h.

typedef Eigen::Matrix<double, 7, 7> g2o::Matrix7d

Definition at line 36 of file sim3.h.

typedef Eigen::Matrix<double, 7, 6, Eigen::ColMajor> g2o::Matrix7x6d

Definition at line 37 of file line3d.h.

typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> g2o::MatrixXD

Definition at line 63 of file eigen_types.h.

typedef Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> g2o::MatrixXF

Definition at line 58 of file eigen_types.h.

typedef Eigen::Matrix<int,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> g2o::MatrixXI

Definition at line 53 of file eigen_types.h.

typedef std::vector<MotionInformation, Eigen::aligned_allocator<MotionInformation> > g2o::MotionInformationVector

Definition at line 46 of file motion_information.h.

typedef SigmaPoint<VectorXd> g2o::MySigmaPoint

Definition at line 10 of file edge_labeler.cpp.

typedef std::vector<Parameter*> g2o::ParameterVector

Definition at line 52 of file parameter.h.

typedef Robot<WorldObjectSE2> g2o::Robot2D

Definition at line 42 of file simulator2d_base.h.

typedef Robot<WorldObjectSE3> g2o::Robot3D

Definition at line 42 of file simulator3d_base.h.

typedef std::shared_ptr<RobustKernel> g2o::RobustKernelPtr

Definition at line 74 of file robust_kernel.h.

typedef SparseBlockMatrix<MatrixXD> g2o::SparseBlockMatrixXd

Definition at line 224 of file sparse_block_matrix.h.

typedef std::set<Star*> g2o::StarSet

Definition at line 71 of file star.h.

typedef Property<std::string> g2o::StringProperty

Definition at line 156 of file property.h.

typedef std::map<std::string, TicTocElement> g2o::TicTocMap

Definition at line 64 of file tictoc.cpp.

typedef Eigen::Matrix<double,2,1,Eigen::ColMajor> g2o::Vector2D

Definition at line 45 of file eigen_types.h.

typedef Eigen::Matrix<float,2,1,Eigen::ColMajor> g2o::Vector2F

Definition at line 40 of file eigen_types.h.

typedef Eigen::Matrix<int,2,1,Eigen::ColMajor> g2o::Vector2I

Definition at line 35 of file eigen_types.h.

typedef Eigen::Matrix<double,3,1,Eigen::ColMajor> g2o::Vector3D

Definition at line 46 of file eigen_types.h.

typedef Eigen::Matrix<float,3,1,Eigen::ColMajor> g2o::Vector3F

Definition at line 41 of file eigen_types.h.

typedef Eigen::Matrix<int,3,1,Eigen::ColMajor> g2o::Vector3I

Definition at line 36 of file eigen_types.h.

typedef Eigen::Matrix<double,4,1,Eigen::ColMajor> g2o::Vector4D

Definition at line 47 of file eigen_types.h.

typedef Eigen::Matrix<float,4,1,Eigen::ColMajor> g2o::Vector4F

Definition at line 42 of file eigen_types.h.

typedef Eigen::Matrix<int,4,1,Eigen::ColMajor> g2o::Vector4I

Definition at line 37 of file eigen_types.h.

typedef Eigen::Matrix< double, 6, 1, Eigen::ColMajor > g2o::Vector6d

Definition at line 75 of file types_icp.cpp.

typedef Eigen::Matrix< double, 7, 1, Eigen::ColMajor > g2o::Vector7d

Definition at line 35 of file sim3.h.

typedef Eigen::Matrix<double,Eigen::Dynamic,1,Eigen::ColMajor> g2o::VectorXD

Definition at line 48 of file eigen_types.h.

typedef Eigen::Matrix<float,Eigen::Dynamic,1,Eigen::ColMajor> g2o::VectorXF

Definition at line 43 of file eigen_types.h.

typedef Eigen::Matrix<int,Eigen::Dynamic,1,Eigen::ColMajor> g2o::VectorXI

Definition at line 38 of file eigen_types.h.

typedef std::map<OptimizableGraph::Vertex*, Star*> g2o::VertexStarMap

Definition at line 70 of file star.h.

typedef std::multimap<OptimizableGraph::Vertex*, Star*> g2o::VertexStarMultimap

Definition at line 69 of file star.h.

typedef WorldObject<VertexLine3D> g2o::WorldObjectLine3D

Definition at line 40 of file simulator3d_base.h.

typedef WorldObject<VertexPointXY> g2o::WorldObjectPointXY

Definition at line 38 of file simulator2d_base.h.

typedef WorldObject<VertexSE2> g2o::WorldObjectSE2

Definition at line 36 of file simulator2d_base.h.

typedef WorldObject<VertexSE3> g2o::WorldObjectSE3

Definition at line 36 of file simulator3d_base.h.

typedef WorldObject<VertexSegment2D> g2o::WorldObjectSegment2D

Definition at line 40 of file simulator2d_base.h.

typedef WorldObject<VertexPointXYZ> g2o::WorldObjectTrackXYZ

Definition at line 38 of file simulator3d_base.h.

Enumeration Type Documentation

enum g2o::CommandArgumentType

Enumerator
CAT_DOUBLE
CAT_FLOAT
CAT_INT
CAT_STRING
CAT_BOOL
CAT_VECTOR_INT
CAT_VECTOR_DOUBLE

Definition at line 98 of file command_args.cpp.

{
  CAT_DOUBLE, CAT_FLOAT, CAT_INT, CAT_STRING, CAT_BOOL, CAT_VECTOR_INT, CAT_VECTOR_DOUBLE
};

Function Documentation

static void g2o::_skew ( Matrix3D &  S, const Vector3D &  t  )

inlinestatic

Definition at line 34 of file line3d.cpp.

Referenced by g2o::Line3D::jacobian(), operator*(), and g2o::Line3D::toCartesian().

                                                          {
    S <<
      0,  -t.z(),   t.y(),
      t.z(),     0,     -t.x(),
      -t.y()     ,t.x(),   0;
  }

static Matrix3D g2o::_skew ( const Vector3D &  t )

inlinestatic

Definition at line 41 of file line3d.cpp.

                                                 {
    Matrix3D S;
    S <<
      0,  -t.z(),   t.y(),
      t.z(),     0,     -t.x(),
      -t.y(),     t.x(),   0;
    return S;
  }

static std::normal_distribution<double> g2o::_univariateSampler ( 0.  , 1.    )

static

Referenced by sampleGaussian().

double g2o::activeVertexChi

(

const OptimizableGraph::Vertex *

v

)

Definition at line 16 of file simple_star_ops.cpp.

References g2o::SparseOptimizer::activeEdges(), g2o::OptimizableGraph::Edge::chi2(), g2o::HyperGraph::Vertex::edges(), g2o::SparseOptimizer::findActiveEdge(), and g2o::OptimizableGraph::Vertex::graph().

                                                         {
    const SparseOptimizer* s = dynamic_cast<const SparseOptimizer*>(v->graph());
    const OptimizableGraph::EdgeContainer& av = s->activeEdges();
    double chi = 0;
    int ne =0;
    for (HyperGraph::EdgeSet::iterator it = v->edges().begin(); it!=v->edges().end(); it++){
      OptimizableGraph::Edge* e = dynamic_cast <OptimizableGraph::Edge*> (*it);
      if (!e)
        continue;
      if (s->findActiveEdge(e)!=av.end()) {
        chi +=e->chi2();
        ne++;
      }
    }
    if (! ne)
      return -1;
    return chi/ne;
  }

G2O_CALIBRATION_ODOM_LASER_API void g2o::addOdometryCalibLinksDifferential	(	SparseOptimizer &	optimizer,
		const DataQueue &	odomData
	)

Definition at line 52 of file sclam_helpers.cpp.

References g2o::OptimizableGraph::addEdge(), g2o::OptimizableGraph::addVertex(), g2o::HyperGraph::edges(), g2o::DataQueue::findClosestData(), g2o::HyperGraph::Vertex::id(), g2o::BaseEdge< D, E >::information(), INFORMATION_SCALING_ODOMETRY, g2o::SE2::inverse(), g2o::RobotLaser::odomPose(), g2o::SE2::rotation(), g2o::HyperGraph::Vertex::setId(), g2o::BaseEdge< D, E >::setMeasurement(), g2o::RobotData::timestamp(), g2o::SE2::translation(), g2o::HyperGraph::DataContainer::userData(), and g2o::HyperGraph::Edge::vertices().

Referenced by main().

  {
    SparseOptimizer::Vertex* odomParamsVertex = 0;
    odomParamsVertex = new VertexOdomDifferentialParams;
    odomParamsVertex->setToOrigin();
    odomParamsVertex->setId(Gm2dlIO::ID_ODOMCALIB);
    optimizer.addVertex(odomParamsVertex);

    SparseOptimizer::EdgeSet odomCalibEdges;
    for (SparseOptimizer::EdgeSet::const_iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
      EdgeSE2SensorCalib* scanmatchEdge = dynamic_cast<EdgeSE2SensorCalib*>(*it);
      if (! scanmatchEdge)
        continue;

      VertexSE2* r1 = dynamic_cast<VertexSE2*>(scanmatchEdge->vertices()[0]);
      VertexSE2* r2 = dynamic_cast<VertexSE2*>(scanmatchEdge->vertices()[1]);
      if (r2->id() - r1->id() != 1) { // ignore non-incremental edges
        continue;
      }

      RobotLaser* rl1 = dynamic_cast<RobotLaser*>(r1->userData());
      RobotLaser* rl2 = dynamic_cast<RobotLaser*>(r2->userData());
      RobotLaser* odom1 = dynamic_cast<RobotLaser*>(odomData.findClosestData(rl1->timestamp()));
      RobotLaser* odom2 = dynamic_cast<RobotLaser*>(odomData.findClosestData(rl2->timestamp()));

      if (fabs(rl1->timestamp() - rl2->timestamp()) < 1e-7) {
        cerr << "strange egde " << r1->id() << " <-> " << r2->id() << endl;
        cerr << FIXED(PVAR(rl1->timestamp()) << "\t " << PVAR(rl2->timestamp())) << endl;
        cerr << FIXED(PVAR(odom1->timestamp()) << "\t " << PVAR(odom2->timestamp())) << endl;
      }

      //cerr << PVAR(odom1->odomPose().toVector().transpose()) << endl;

      SE2 odomMotion = odom1->odomPose().inverse() * odom2->odomPose();
      //cerr << PVAR(odomMotion.toVector().transpose()) << endl;
      //cerr << PVAR(scanmatchEdge->measurement().toVector().transpose()) << endl;

      EdgeSE2OdomDifferentialCalib* e = new EdgeSE2OdomDifferentialCalib;
      e->vertices()[0] = r1;
      e->vertices()[1] = r2;
      e->vertices()[2] = odomParamsVertex;

      MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), odom2->timestamp() - odom1->timestamp());
      e->setMeasurement(OdomConvert::convertToVelocity(mm));
      //cerr << PVAR(e->measurement()) << endl;

      e->information() = Matrix3d::Identity() * INFORMATION_SCALING_ODOMETRY;
      odomCalibEdges.insert(e);

    }

    for (SparseOptimizer::EdgeSet::iterator it = odomCalibEdges.begin(); it != odomCalibEdges.end(); ++it)
      optimizer.addEdge(dynamic_cast<OptimizableGraph::Edge*>(*it));

  }

G2O_CALIBRATION_ODOM_LASER_API void g2o::allocateSolverForSclam	(	SparseOptimizer &	optimizer,
		bool	levenberg
	)

Definition at line 108 of file sclam_helpers.cpp.

References g2o::SparseOptimizer::setAlgorithm().

Referenced by main().

  {
    typedef BlockSolver< BlockSolverTraits<-1, -1> >  SclamBlockSolver;
    typedef LinearSolverCSparse<SclamBlockSolver::PoseMatrixType> SclamLinearSolver;

    SclamLinearSolver* linearSolver = new SclamLinearSolver();
    linearSolver->setBlockOrdering(false);
    SclamBlockSolver* blockSolver = new SclamBlockSolver(linearSolver);
    OptimizationAlgorithm* solver = 0;
    if (levenberg) {
      solver = new OptimizationAlgorithmLevenberg(blockSolver);
    } else {
      solver = new OptimizationAlgorithmGaussNewton(blockSolver);
    }
    optimizer.setAlgorithm(solver);
  }

void G2O_CORE_API g2o::applyAction	(	HyperGraph *	graph,
		HyperGraphElementAction *	action,
		HyperGraphElementAction::Parameters *	parameters = 0,
		const std::string &	typeName = ""
	)

apply an action to all the elements of the graph.

Definition at line 277 of file hyper_graph_action.cpp.

References g2o::HyperGraph::edges(), g2o::HyperGraphElementAction::name(), g2o::HyperGraphElementAction::typeName(), and g2o::HyperGraph::vertices().

Referenced by g2o::G2oQGLViewer::draw().

  {
    for (HyperGraph::VertexIDMap::iterator it=graph->vertices().begin(); 
        it!=graph->vertices().end(); ++it){
      if ( typeName.empty() || typeid(*it->second).name()==typeName){
        (*action)(it->second, params);
      }
    }
    for (HyperGraph::EdgeSet::iterator it=graph->edges().begin(); 
        it!=graph->edges().end(); ++it){
      if ( typeName.empty() || typeid(**it).name()==typeName)
        (*action)(*it, params);
    }
  }

bool g2o::arrayHasNaN ( const double *  array, int  size, int *  nanIndex = 0  )

inline

tests whether there is a NaN in the array

Definition at line 177 of file misc.h.

References g2o_isnan.

Referenced by g2o::BlockSolver< Traits >::buildSystem(), g2o::SparseOptimizer::computeActiveErrors(), g2o::SparseOptimizer::initializeOptimization(), and g2o::SparseOptimizer::optimize().

{
  for (int i = 0; i < size; ++i)
    if (g2o_isnan(array[i])) {
      if (nanIndex)
        *nanIndex = i;
      return true;
    }
  return false;
}

void g2o::assignHierarchicalEdges	(	StarSet &	stars,
		EdgeStarMap &	esmap,
		EdgeLabeler *	labeler,
		EdgeCreator *	creator,
		SparseOptimizer *	optimizer,
		int	minNumEdges,
		int	maxIterations
	)

Definition at line 83 of file simple_star_ops.cpp.

References g2o::Star::_gauge, g2o::Star::_lowLevelEdges, g2o::Star::_lowLevelVertices, g2o::Star::_starEdges, g2o::OptimizableGraph::addEdge(), g2o::EdgeCreator::createEdge(), g2o::Factory::instance(), g2o::Star::labelStarEdges(), g2o::Star::lowLevelEdges(), g2o::Star::lowLevelVertices(), g2o::OptimizableGraph::saveSubset(), g2o::OptimizableGraph::Edge::setLevel(), vertexEdgesInStar(), and vertices.

                                                                                                                                                                            {
  // now construct the hierarchical edges for all the stars
  int starNum=0;
  for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++){
    cerr << "STAR# " << starNum << endl;
    Star* s=*it;
    std::vector<OptimizableGraph::Vertex*> vertices(2);
    vertices[0]= (OptimizableGraph::Vertex*) *s->_gauge.begin();
    cerr << "eIs"  << endl;
    HyperGraph::VertexSet vNew =s->lowLevelVertices();
    for (HyperGraph::VertexSet::iterator vit=s->_lowLevelVertices.begin(); vit!=s->_lowLevelVertices.end(); vit++){
      OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)*vit;
      vertices[1]=v;
      if (v==vertices[0])
        continue;
      HyperGraph::EdgeSet eInSt;
      int numEdges = vertexEdgesInStar(eInSt, v, s, esmap);
      if (Factory::instance()->tag(v)==Factory::instance()->tag(vertices[0]) || numEdges>minNumEdges) {
        OptimizableGraph::Edge* e=creator->createEdge(vertices);
        //cerr << "creating edge" << e << endl;
        if (e) {
          e->setLevel(1);
          optimizer->addEdge(e);
          s->_starEdges.insert(e);
        } else {
          cerr << "THERE" << endl;
          cerr << "FATAL, cannot create edge" << endl;
        }
      } else {
        vNew.erase(v);
        // cerr << numEdges << " ";
        // cerr << "r " <<  v-> id() << endl;
        // remove from the star all edges that are not sufficiently connected
        for (HyperGraph::EdgeSet::iterator it=eInSt.begin(); it!=eInSt.end(); it++){
          HyperGraph::Edge* e=*it;
          s->lowLevelEdges().erase(e);
        }
      }
    }
    s->lowLevelVertices()=vNew;
    //cerr << endl;
    cerr <<  "gauge: " << (*s->_gauge.begin())->id()
      << " edges:" << s->_lowLevelEdges.size()
      << " hedges" << s->_starEdges.size() << endl;

    const bool debug = false;
    if (debug){
      char starLowName[100];
      sprintf(starLowName, "star-%04d-low.g2o", starNum);
      ofstream starLowStream(starLowName);
      optimizer->saveSubset(starLowStream, s->_lowLevelEdges);
    }
    bool labelOk=s->labelStarEdges(maxIterations, labeler);
    if (labelOk) {
      if (debug) {
        char starHighName[100];
        sprintf(starHighName, "star-%04d-high.g2o", starNum);
        ofstream starHighStream(starHighName);
        optimizer->saveSubset(starHighStream, s->_starEdges);
      }
    } else {
      cerr << "FAILURE" << endl;
    }
    starNum++;
  }
}

double g2o::average_angle ( double  theta1, double  theta2  )

inline

average two angles

Definition at line 120 of file misc.h.

{
  double x, y;

  x = cos(theta1) + cos(theta2);
  y = sin(theta1) + sin(theta2);
  if(x == 0 && y == 0)
    return 0;
  else
    return std::atan2(y, x);
}

G2O_STUFF_API std::string g2o::changeFileExtension	(	const std::string &	filename,
		const std::string &	newExt,
		bool	stripDot = false
	)

change the fileextension of a given filename. Only if filename contains an extension, otherwise filename is returned.

Definition at line 104 of file filesys_tools.cpp.

{
  std::string::size_type lastDot = filename.find_last_of('.');
  if (lastDot != std::string::npos) {
    if (stripDot)
      return filename.substr(0, lastDot) + newExt;
    else
      return filename.substr(0, lastDot + 1) + newExt;
  } else
    return filename;
}

template<typename T >

T g2o::clamp ( T  l, T  x, T  u  )

inline

clamp x to the interval [l, u]

Definition at line 151 of file misc.h.

{
  if (x < l)
    return l;
  if (x > u)
    return u;
  return x;
}

G2O_SIMULATOR_API int g2o::clipSegmentCircle	(	Eigen::Vector2d &	p1,
		Eigen::Vector2d &	p2,
		double	r
	)

Definition at line 11 of file simutils.cpp.

Referenced by g2o::SensorSegment2D::isVisible(), g2o::SensorSegment2DPointLine::isVisible(), and g2o::SensorSegment2DLine::isVisible().

                                                                        {
    double r2=r*r;
    Eigen::Vector2d pBase=p1;
    Eigen::Vector2d dp=p2-p1;
    double length=dp.norm();
    dp.normalize();
    double p=2*dp.dot(p1);
    double q=p1.squaredNorm()-r2;
    double disc = p*p - 4*q;
    
    if (disc<=0) { // no intersection or single point intersection
      return -1; 
    }
    disc = sqrt(disc);

    double t1=.5*(-p-disc);
    double t2=.5*(-p+disc);

    if ( t1 > length || t2 <0 ) 
      return -1; // no intersection
    bool clip1=false;
    bool clip2=false;
    if ( t1 > 0 ) {
      p1 = pBase + dp*t1;
      clip1 = true;
    }
    if ( t2 < length ) {
      p2 = pBase + dp*t1;
      clip2 = true;
    }
    if (clip1)
      if (clip2) return 3;
      else return 0;
    else
      if (clip2) return 1;
    return 2;
  }

G2O_SIMULATOR_API int g2o::clipSegmentFov	(	Eigen::Vector2d &	p1,
		Eigen::Vector2d &	p2,
		double	min,
		double	max
	)

Definition at line 82 of file simutils.cpp.

References clipSegmentLine().

Referenced by g2o::SensorSegment2D::isVisible(), g2o::SensorSegment2DPointLine::isVisible(), and g2o::SensorSegment2DLine::isVisible().

                                                                                  {
    bool clip1 = false, clip2 = false;
    // normal to the first line
    double amin =  sin(min), bmin = -cos(min);
    int minClip=clipSegmentLine(p1,p2,amin,bmin,0);
    switch(minClip){
    case -1: 
      return -1; 
    case  0: 
      clip1 = true; 
      break;
    case  1: 
      clip2 = true; 
      break;
    default:;
    }
    // normal to the second line
    double amax = -sin(max), bmax =  cos(max);
    int maxClip=clipSegmentLine(p1,p2,amax,bmax,0);
    switch(maxClip){
    case -1: 
      return -1; 
    case  0: 
      clip1 = true; 
      break;
    case  1: 
      clip2 = true; 
      break;
    default:;
    }
    if (clip1)
      if (clip2) return 3;
      else return 0;
    else
      if (clip2) return 1;
    return 2;
  }

G2O_SIMULATOR_API int g2o::clipSegmentLine	(	Eigen::Vector2d &	p1,
		Eigen::Vector2d &	p2,
		double	a,
		double	b,
		double	c
	)

Definition at line 54 of file simutils.cpp.

Referenced by clipSegmentFov().

                                                                                          {
    bool p1inside = true;
    bool p2inside = true;
    if (a*p1.x()+b*p1.y()+c < 0){
      p1inside=false;
    }
    if (a*p2.x()+b*p2.y()+c < 0){
      p2inside=false;
    }
    if (p1inside && p2inside)
      return 2;
    if (!p1inside && !p2inside)
      return -1;

    Eigen::Vector2d dp=p2-p1;
    double den=a*dp.x()+b*dp.y();
    if (den==0)
      return -1;
    double num = c + a*p1.x()+b*p1.y();
    double  t = - num/den;
    if (p1inside){
      p2=p1+dp*t;
      return 1;
    }
    p1=p1+dp*t;
    return 0;
  }

G2O_HIERARCHICAL_API void g2o::computeBorder	(	StarSet &	stars,
		EdgeStarMap &	hesmap
	)

Definition at line 150 of file simple_star_ops.cpp.

References g2o::Star::_starEdges, g2o::Star::gauge(), g2o::Star::starFrontierEdges(), and starsInEdge().

Referenced by main().

                                                       {
  cerr << "computing edges on the border" << endl;
  for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++){
    Star* s=*it;
    for (HyperGraph::EdgeSet::iterator iit=s->_starEdges.begin(); iit!=s->_starEdges.end(); iit++){
      OptimizableGraph::Edge* e= (OptimizableGraph::Edge*) *iit;
      StarSet sset;
      starsInEdge(sset, e, hesmap, s->gauge());
      //cerr << "e: " << e << " l:" << e->level() << " sset.size()=" << sset.size() << endl;
      if (sset.size()>1){
        s->starFrontierEdges().insert(e);
      }
    }
  }
}

G2O_SIMULATOR_API Eigen::Vector2d g2o::computeLineParameters	(	const Eigen::Vector2d &	p1,
		const Eigen::Vector2d &	p2
	)

Definition at line 120 of file simutils.cpp.

Referenced by LineInfo::LineInfo(), and main().

                                                                                       {
    Eigen::Vector2d lp;
    Eigen::Vector2d dp=p2-p1;
    lp[0]=atan2(-dp.x(), dp.y());
    Eigen::Vector2d n(cos(lp[0]), sin(lp[0]));
    lp[1]=n.dot(p1+p2)*.5;
    return lp;
  } 

G2O_HIERARCHICAL_API void g2o::computeSimpleStars	(	StarSet &	stars,
		SparseOptimizer *	optimizer,
		EdgeLabeler *	labeler,
		EdgeCreator *	creator,
		OptimizableGraph::Vertex *	gauge_,
		std::string	edgeTag,
		std::string	vertexTag,
		int	level,
		int	step,
		int	backboneIterations,
		int	starIterations,
		double	rejectionThreshold,
		bool	debug
	)

Definition at line 167 of file simple_star_ops.cpp.

References g2o::Star::_gauge, g2o::Star::_lowLevelEdges, g2o::Star::_lowLevelVertices, g2o::Star::_starEdges, g2o::SparseOptimizer::activeChi2(), g2o::OptimizableGraph::addEdge(), g2o::HyperDijkstra::adjacencyMap(), g2o::OptimizationAlgorithmWithHessian::buildLinearStructure(), g2o::SparseOptimizer::computeActiveErrors(), g2o::SparseOptimizer::computeInitialGuess(), g2o::HyperDijkstra::computeTree(), g2o::EdgeCreator::createEdge(), g2o::OptimizableGraph::Vertex::dimension(), g2o::HyperGraph::Vertex::edges(), g2o::Star::gauge(), g2o::OptimizableGraph::Vertex::hessian(), g2o::OptimizationAlgorithm::init(), g2o::SparseOptimizer::initializeOptimization(), g2o::Star::labelStarEdges(), g2o::Star::lowLevelEdges(), g2o::SparseOptimizer::optimize(), g2o::Star::optimizer(), g2o::SparseOptimizer::pop(), g2o::SparseOptimizer::push(), g2o::OptimizableGraph::saveSubset(), g2o::OptimizableGraph::setFixed(), g2o::OptimizableGraph::Edge::setLevel(), g2o::HyperDijkstra::shortestPaths(), g2o::OptimizableGraph::Vertex::solveDirect(), g2o::SparseOptimizer::solver(), g2o::OptimizationAlgorithmWithHessian::updateLinearSystem(), vertices, g2o::HyperGraph::Edge::vertices(), and g2o::HyperDijkstra::visitAdjacencyMap().

Referenced by main().

                                     {

    cerr << "preforming the tree actions" << endl;
    HyperDijkstra d(optimizer);
    // compute a spanning tree based on the types of edges and vertices in the pool
    EdgeTypesCostFunction f(edgeTag, vertexTag, level);
    d.shortestPaths(gauge_,
        &f,
        std::numeric_limits< double >::max(),
        1e-6,
        false,
        std::numeric_limits< double >::max()/2);

    HyperDijkstra::computeTree(d.adjacencyMap());
    // constructs the stars on the backbone

    BackBoneTreeAction bact(optimizer, vertexTag, level, step);
    bact.init();

    cerr << "free edges size " << bact.freeEdges().size() << endl;

    // perform breadth-first visit of the visit tree and create the stars on the backbone
    d.visitAdjacencyMap(d.adjacencyMap(),&bact,true);
    stars.clear();

    for (VertexStarMultimap::iterator it=bact.vertexStarMultiMap().begin();
        it!=bact.vertexStarMultiMap().end(); it++){
      stars.insert(it->second);
    }
    cerr << "stars.size: " << stars.size() << endl;
    cerr << "size: " << bact.vertexStarMultiMap().size() << endl;


    //  for each star

    //    for all vertices in the backbone, select all edges leading/leaving from that vertex
    //    that are contained in freeEdges.

    //      mark the corresponding "open" vertices and add them to a multimap (vertex->star)

    //    select a gauge in the backbone

    //    push all vertices on the backbone

    //    compute an initial guess on the backbone

    //    one round of optimization backbone

    //    lock all vertices in the backbone

    //    push all "open" vertices

    //    for each open vertex,
    //      compute an initial guess given the backbone
    //      do some rounds of solveDirect
    //      if (fail)
    //        - remove the vertex and the edges in that vertex from the star
    //   - make the structures consistent

    //    pop all "open" vertices
    //    pop all "vertices" in the backbone
    //    unfix the vertices in the backbone

    int starNum=0;
    for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++){
      Star* s =*it;
      HyperGraph::VertexSet backboneVertices = s->_lowLevelVertices;
      HyperGraph::EdgeSet backboneEdges = s->_lowLevelEdges;
      if (backboneEdges.empty())
        continue;


      // cerr << "optimizing backbone" << endl;
      // one of these  should be the gauge, to be simple we select the fisrt one in the backbone
      OptimizableGraph::VertexSet gauge;
      gauge.insert(*backboneVertices.begin());
      s->gauge()=gauge;
      s->optimizer()->push(backboneVertices);
      s->optimizer()->setFixed(gauge,true);
      s->optimizer()->initializeOptimization(backboneEdges);
      s->optimizer()->computeInitialGuess();
      s->optimizer()->optimize(backboneIterations);
      s->optimizer()->setFixed(backboneVertices, true);

      // cerr << "assignind edges.vertices not in bbone" << endl;
      HyperGraph::EdgeSet otherEdges;
      HyperGraph::VertexSet otherVertices;
      std::multimap<HyperGraph::Vertex*, HyperGraph::Edge*> vemap;
      for (HyperGraph::VertexSet::iterator bit=backboneVertices.begin(); bit!=backboneVertices.end(); bit++){
        HyperGraph::Vertex* v=*bit;
        for (HyperGraph::EdgeSet::iterator eit=v->edges().begin(); eit!=v->edges().end(); eit++){
          OptimizableGraph::Edge* e = (OptimizableGraph::Edge*) *eit;
          HyperGraph::EdgeSet::iterator feit=bact.freeEdges().find(e);
          if (feit!=bact.freeEdges().end()){ // edge is admissible
            otherEdges.insert(e);
            bact.freeEdges().erase(feit);
            for (size_t i=0; i<e->vertices().size(); i++){
              OptimizableGraph::Vertex* ve= (OptimizableGraph::Vertex*)e->vertices()[i];
              if (backboneVertices.find(ve)==backboneVertices.end()){
                otherVertices.insert(ve);
                vemap.insert(make_pair(ve,e));
              }
            }
          }
        }
      }

      // RAINER TODO maybe need a better solution than dynamic casting here??
      OptimizationAlgorithmWithHessian* solverWithHessian = dynamic_cast<OptimizationAlgorithmWithHessian*>(s->optimizer()->solver());
      if (solverWithHessian) {
        s->optimizer()->push(otherVertices);
        // cerr << "optimizing vertices out of bbone" << endl;
        // cerr << "push" << endl;
        // cerr << "init" << endl;
        s->optimizer()->initializeOptimization(otherEdges);
        // cerr << "guess" << endl;
        s->optimizer()->computeInitialGuess();
        // cerr << "solver init" << endl;
        s->optimizer()->solver()->init();
        // cerr << "structure" << endl;
        if (!solverWithHessian->buildLinearStructure())
          cerr << "FATAL: failure while building linear structure" << endl;
        // cerr << "errors" << endl;
        s->optimizer()->computeActiveErrors();
        // cerr << "system" << endl;
        solverWithHessian->updateLinearSystem();
        // cerr << "directSolove" << endl;
      } else {
        cerr << "FATAL: hierarchical thing cannot be used with a solver that does not support the system structure construction" << endl;
      }


      // // then optimize the vertices one at a time to check if a solution is good
      for (HyperGraph::VertexSet::iterator vit=otherVertices.begin(); vit!=otherVertices.end(); vit++){
        OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)(*vit);
        v->solveDirect();
        // cerr << " " << d;
        // if  a solution is found, add a vertex and all the edges in
        //othervertices that are pointing to that edge to the star
        s->_lowLevelVertices.insert(v);
        for (HyperGraph::EdgeSet::iterator eit=v->edges().begin(); eit!=v->edges().end(); eit++){
          OptimizableGraph::Edge* e = (OptimizableGraph::Edge*) *eit;
          if (otherEdges.find(e)!=otherEdges.end())
            s->_lowLevelEdges.insert(e);
        }
      }
      //cerr <<  endl;

      // relax the backbone and optimize it all
      // cerr << "relax bbone" << endl;
      s->optimizer()->setFixed(backboneVertices, false);
      //cerr << "fox gauge bbone" << endl;
      s->optimizer()->setFixed(s->gauge(),true);

      //cerr << "opt init" << endl;
      s->optimizer()->initializeOptimization(s->_lowLevelEdges);
      optimizer->computeActiveErrors();
      double initialChi = optimizer->activeChi2();
      int starOptResult = s->optimizer()->optimize(starIterations);
      //cerr << starOptResult << "(" << starIterations << ")  " << endl;
      double finalchi=-1.;

      cerr <<  "computing star: " << starNum << endl;

      int vKept=0, vDropped=0;
      if (!starIterations || starOptResult > 0  ){
        optimizer->computeActiveErrors();
        finalchi = optimizer->activeChi2();

#if 1

        s->optimizer()->computeActiveErrors();
        // cerr << "system" << endl;
        solverWithHessian->updateLinearSystem();
        HyperGraph::EdgeSet prunedStarEdges = backboneEdges;
        HyperGraph::VertexSet prunedStarVertices = backboneVertices;
        for (HyperGraph::VertexSet::iterator vit=otherVertices.begin(); vit!=otherVertices.end(); vit++){

          //discard the vertices whose error is too big


          OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)(*vit);
          MatrixXd h(v->dimension(), v->dimension());
          for (int i=0; i<v->dimension(); i++){
            for (int j=0; j<v->dimension(); j++)
              h(i,j)=v->hessian(i,j);
          }
          EigenSolver<Eigen::MatrixXd> esolver;
          esolver.compute(h);
          VectorXcd ev= esolver.eigenvalues();
          double emin = std::numeric_limits<double>::max();
          double emax = -std::numeric_limits<double>::max();
          for (int i=0; i<ev.size(); i++){
            emin = ev(i).real()>emin ? emin : ev(i).real();
            emax = ev(i).real()<emax ? emax : ev(i).real();
          }

          double d=emin/emax;


          // cerr << " " << d;
          if (d>rejectionThreshold){
          // if  a solution is found, add a vertex and all the edges in
          //othervertices that are pointing to that edge to the star
            prunedStarVertices.insert(v);
            for (HyperGraph::EdgeSet::iterator eit=v->edges().begin(); eit!=v->edges().end(); eit++){
              OptimizableGraph::Edge* e = (OptimizableGraph::Edge*) *eit;
              if (otherEdges.find(e)!=otherEdges.end())
                prunedStarEdges.insert(e);
            }
            //cerr << "K( " << v->id() << "," << d << ")" ;
            vKept ++;
          } else {
            vDropped++;
            //cerr << "R( " << v->id() << "," << d << ")" ;
          }
        }
        s->_lowLevelEdges=prunedStarEdges;
        s->_lowLevelVertices=prunedStarVertices;

#endif
        //cerr << "addHedges" << endl;
        //now add to the star the hierarchical edges
        std::vector<OptimizableGraph::Vertex*> vertices(2);
        vertices[0]= (OptimizableGraph::Vertex*) *s->_gauge.begin();

        for (HyperGraph::VertexSet::iterator vit=s->_lowLevelVertices.begin(); vit!=s->_lowLevelVertices.end(); vit++){
          OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)*vit;
          vertices[1]=v;
          if (v==vertices[0])
            continue;
          OptimizableGraph::Edge* e=creator->createEdge(vertices);
          //rr << "creating edge" << e <<  Factory::instance()->tag(vertices[0]) << "->" <<  Factory::instance()->tag(v) <endl;
          if (e) {
            e->setLevel(level+1);
            optimizer->addEdge(e);
            s->_starEdges.insert(e);
          } else {
            cerr << "HERE" << endl;
            cerr << "FATAL, cannot create edge" << endl;
          }
        }
      }

      cerr << " gauge: " << (*s->_gauge.begin())->id()
           << " kept: " << vKept
           << " dropped: " << vDropped
           << " edges:" << s->_lowLevelEdges.size()
           << " hedges" << s->_starEdges.size()
           << " initial chi " << initialChi
           << " final chi " << finalchi << endl;

      if (debug) {
        char starLowName[100];
        sprintf(starLowName, "star-%04d-low.g2o", starNum);
        ofstream starLowStream(starLowName);
        optimizer->saveSubset(starLowStream, s->_lowLevelEdges);
      }
      bool labelOk=false;
      if (!starIterations || starOptResult > 0)
        labelOk = s->labelStarEdges(0, labeler);
      if (labelOk) {
        if (debug) {
          char starHighName[100];
          sprintf(starHighName, "star-%04d-high.g2o", starNum);
          ofstream starHighStream(starHighName);
          optimizer->saveSubset(starHighStream, s->_starEdges);
        }
      } else {

        cerr << "FAILURE: " << starOptResult << endl;
      }
      starNum++;

      //label each hierarchical edge
      s->optimizer()->pop(otherVertices);
      s->optimizer()->pop(backboneVertices);
      s->optimizer()->setFixed(s->gauge(),false);
    }


    StarSet stars2;
    // now erase the stars that have 0 edges. They r useless
    for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++){
      Star* s=*it;
      if (s->lowLevelEdges().size()==0) {
        delete s;
      } else
        stars2.insert(s);
    }
    stars=stars2;
  }

G2O_HIERARCHICAL_API void g2o::constructEdgeStarMap	(	EdgeStarMap &	esmap,
		StarSet &	stars,
		bool	low
	)

Definition at line 35 of file simple_star_ops.cpp.

References g2o::Star::lowLevelEdges(), and g2o::Star::starEdges().

Referenced by main().

                                                                       {
  esmap.clear();
  for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++){
    Star* s = *it;
    if (low) {
      for (HyperGraph::EdgeSet::iterator it = s->lowLevelEdges().begin();
          it!=s->lowLevelEdges().end(); it++){
        HyperGraph::Edge* e=*it;
        esmap.insert(make_pair(e,s));
      }
    } else {
      for (HyperGraph::EdgeSet::iterator it = s->starEdges().begin();
          it!=s->starEdges().end(); it++){
        HyperGraph::Edge* e=*it;
        esmap.insert(make_pair(e,s));
      }
    }
  }
}

template<typename T >

bool g2o::convertString	(	const std::string &	s,
		T &	x
	)

convert a string into an other type.

Definition at line 82 of file command_args.cpp.

Referenced by g2o::Property< bool >::fromString(), g2o::CommandArgs::str2arg(), and stringToType().

{
  std::istringstream i(s);
  if (! (i >> x))
    return false;
  return true;
}

static OptimizationAlgorithm* g2o::createSolver ( const std::string &  fullSolverName )

static

helper function for allocating

Definition at line 38 of file structure_only.cpp.

  {
    if (fullSolverName == "structure_only_2") {
      OptimizationAlgorithm* optimizationAlgo = new StructureOnlySolver<2>;
      return optimizationAlgo;
    }
    else if (fullSolverName == "structure_only_3") {
      OptimizationAlgorithm* optimizationAlgo = new StructureOnlySolver<3>;
      return optimizationAlgo;
    }
    else
      return 0;
  }

static OptimizationAlgorithm* g2o::createSolver ( const std::string &  fullSolverName )

static

helper function for allocating

Definition at line 47 of file solver_eigen.cpp.

References ALLOC_EIGEN_SPARSE_CHOLESKY.

Referenced by g2o::EigenSolverCreator::construct().

  {
    g2o::Solver* s = 0;

    string methodName = fullSolverName.substr(0, 2);
    string solverName = fullSolverName.substr(3);

    if (solverName == "var_eigen") {
      ALLOC_EIGEN_SPARSE_CHOLESKY(s, -1, -1, true);
    }
#if 0
    else if (solverName == "fix3_2_eigen") {
      ALLOC_EIGEN_SPARSE_CHOLESKY(s, 3, 2, true);
    }
    else if (solverName == "fix6_3_eigen") {
      ALLOC_EIGEN_SPARSE_CHOLESKY(s, 6, 3, true);
    }
    else if (solverName == "fix7_3_eigen") {
      ALLOC_EIGEN_SPARSE_CHOLESKY(s, 7, 3, true);
    }
    else if (solverName == "fix3_2_scalar_eigen") {
      ALLOC_EIGEN_SPARSE_CHOLESKY(s, 3, 2, false);
    }
    else if (solverName == "fix6_3_scalar_eigen") {
      ALLOC_EIGEN_SPARSE_CHOLESKY(s, 6, 3, false);
    }
    else if (solverName == "fix7_3_scalar_eigen") {
      ALLOC_EIGEN_SPARSE_CHOLESKY(s, 7, 3, false);
    }
#endif

    OptimizationAlgorithm* snl = 0;
    if (methodName == "gn") {
      snl = new OptimizationAlgorithmGaussNewton(s);
    }
    else if (methodName == "lm") {
      snl = new OptimizationAlgorithmLevenberg(s);
    }
    else if (methodName == "dl") {
      BlockSolverBase* blockSolver = dynamic_cast<BlockSolverBase*>(s);
      snl = new OptimizationAlgorithmDogleg(blockSolver);
    }

    return snl;
  }

static OptimizationAlgorithm* g2o::createSolver ( const std::string &  fullSolverName )

static

helper function for allocating

Definition at line 49 of file solver_csparse.cpp.

References ALLOC_CSPARSE.

Referenced by g2o::CSparseSolverCreator::construct().

  {
    g2o::Solver* s = 0;

    string methodName = fullSolverName.substr(0, 2);
    string solverName = fullSolverName.substr(3);

    if (solverName == "var") {
      ALLOC_CSPARSE(s, -1, -1, false);
    }
    else if (solverName == "fix3_2") {
      ALLOC_CSPARSE(s, 3, 2, true);
    }
    else if (solverName == "fix6_3") {
      ALLOC_CSPARSE(s, 6, 3, true);
    }
    else if (solverName == "fix7_3") {
      ALLOC_CSPARSE(s, 7, 3, true);
    }
#  ifdef ADD_SCALAR_ORDERING
    else if (solverName == "fix3_2_scalar") {
      ALLOC_CSPARSE(s, 3, 2, false);
    }
    else if (solverName == "fix6_3_scalar") {
      ALLOC_CSPARSE(s, 6, 3, false);
    }
    else if (solverName == "fix7_3_scalar") {
      ALLOC_CSPARSE(s, 7, 3, false);
    }
#endif

    OptimizationAlgorithm* snl = 0;
    if (methodName == "gn") {
      snl = new OptimizationAlgorithmGaussNewton(s);
    }
    else if (methodName == "lm") {
      snl = new OptimizationAlgorithmLevenberg(s);
    }
    else if (methodName == "dl") {
      BlockSolverBase* blockSolver = dynamic_cast<BlockSolverBase*>(s);
      snl = new OptimizationAlgorithmDogleg(blockSolver);
    }

    return snl;
  }

static OptimizationAlgorithm* g2o::createSolver ( const std::string &  fullSolverName )

static

Definition at line 50 of file solver_pcg.cpp.

References ALLOC_PCG.

Referenced by g2o::PCGSolverCreator::construct().

  {
    g2o::Solver* s = 0;

    string methodName = fullSolverName.substr(0, 2);
    string solverName = fullSolverName.substr(3);

    if (solverName == "pcg") {
      ALLOC_PCG(s, -1, -1);
    }
    else if (solverName == "pcg3_2") {
      ALLOC_PCG(s, 3, 2);
    }
    else if (solverName == "pcg6_3") {
      ALLOC_PCG(s, 6, 3);
    }
    else if (solverName == "pcg7_3") {
      ALLOC_PCG(s, 7, 3);
    }

    OptimizationAlgorithm* snl = 0;
    if (methodName == "gn") {
      snl = new OptimizationAlgorithmGaussNewton(s);
    }
    else if (methodName == "lm") {
      snl = new OptimizationAlgorithmLevenberg(s);
    }

    return snl;
  }

static OptimizationAlgorithm* g2o::createSolver ( const std::string &  fullSolverName )

static

Definition at line 50 of file solver_dense.cpp.

References ALLOC_DENSE.

Referenced by g2o::DenseSolverCreator::construct().

  {
    g2o::Solver* s = 0;

    string methodName = fullSolverName.substr(0, 2);
    string solverName = fullSolverName.substr(3);

    if (solverName == "dense") {
      ALLOC_DENSE(s, -1, -1);
    }
    else if (solverName == "dense3_2") {
      ALLOC_DENSE(s, 3, 2);
    }
    else if (solverName == "dense6_3") {
      ALLOC_DENSE(s, 6, 3);
    }
    else if (solverName == "dense7_3") {
      ALLOC_DENSE(s, 7, 3);
    }

    OptimizationAlgorithm* snl = 0;
    if (methodName == "gn") {
      snl = new OptimizationAlgorithmGaussNewton(s);
    }
    else if (methodName == "lm") {
      snl = new OptimizationAlgorithmLevenberg(s);
    }

    return snl;
  }

static OptimizationAlgorithm* g2o::createSolver ( const std::string &  fullSolverName )

static

helper function for allocating

Definition at line 54 of file slam2d_linear.cpp.

References ALLOC_CSPARSE.

  {
    if (fullSolverName != "2dlinear")
      return 0;

    g2o::Solver* s = 0;
    ALLOC_CSPARSE(s, 3, 2, true);
    OptimizationAlgorithm* snl = 0;
    snl = new SolverSLAM2DLinear(s);

    return snl;
  }

static OptimizationAlgorithm* g2o::createSolver ( const std::string &  fullSolverName )

static

Definition at line 55 of file solver_cholmod.cpp.

References ALLOC_CHOLMOD.

Referenced by g2o::CholmodSolverCreator::construct().

  {
    g2o::Solver* s = 0;

    string methodName = fullSolverName.substr(0, 2);
    string solverName = fullSolverName.substr(3);

    if (solverName == "var_cholmod") {
      ALLOC_CHOLMOD(s, -1, -1, false);
    }
    else if (solverName == "fix3_2_cholmod") {
      ALLOC_CHOLMOD(s, 3, 2, true);
    }
    else if (solverName == "fix6_3_cholmod") {
      ALLOC_CHOLMOD(s, 6, 3, true);
    }
    else if (solverName == "fix7_3_cholmod") {
      ALLOC_CHOLMOD(s, 7, 3, true);
    }
#ifdef ADD_SCALAR_ORDERING
    else if (solverName == "fix3_2_cholmod_scalar") {
      ALLOC_CHOLMOD(s, 3, 2, false);
    }
    else if (solverName == "fix6_3_cholmod_scalar") {
      ALLOC_CHOLMOD(s, 6, 3, false);
    }
    else if (solverName == "fix7_3_cholmod_scalar") {
      ALLOC_CHOLMOD(s, 7, 3, false);
    }
#endif

    OptimizationAlgorithm* snl = 0;
    if (methodName == "gn") {
      snl = new OptimizationAlgorithmGaussNewton(s);
    }
    else if (methodName == "lm") {
      snl = new OptimizationAlgorithmLevenberg(s);
    }
    else if (methodName == "dl") {
      BlockSolverBase* blockSolver = dynamic_cast<BlockSolverBase*>(s);
      snl = new OptimizationAlgorithmDogleg(blockSolver);
    }
    else {
      delete s;
    }

    return snl;
  }

static Solver* g2o::createSolver ( const std::string &  solverName )

static

Definition at line 194 of file graph_optimizer_sparse_online.cpp.

References ALLOC_PCG.

{
  g2o::Solver* s = 0;
  if (solverName == "pcg3_2") {
    ALLOC_PCG(s, 3, 2);
  }
  else if (solverName == "pcg6_3") {
    ALLOC_PCG(s, 6, 3);
  }
  return s;
}

static OptimizationAlgorithm* g2o::createSolver ( const std::string &  solverName )

static

Definition at line 459 of file graph_optimizer_sparse_incremental.cpp.

References ALLOC_CHOLMOD.

Referenced by g2o::StructureOnlyCreator::construct(), g2o::SLAM2DLinearSolverCreator::construct(), g2o::SparseOptimizerIncremental::initSolver(), and g2o::SparseOptimizerOnline::initSolver().

  {
    g2o::Solver* s = 0;

    if (solverName == "fix3_2_cholmod") {
      ALLOC_CHOLMOD(s, 3, 2);
    }
    else if (solverName == "fix6_3_cholmod") {
      ALLOC_CHOLMOD(s, 6, 3);
    }

    OptimizationAlgorithmGaussNewton* gaussNewton = new OptimizationAlgorithmGaussNewton(s);
    return gaussNewton;
  }

Matrix<double,3,6,Eigen::ColMajor> g2o::d_expy_d_y ( const Vector3D &  y )

inline

Definition at line 194 of file types_six_dof_expmap.cpp.

References skew().

Referenced by g2o::EdgeProjectPSI2UV::linearizeOplus().

                                                                      {
  Matrix<double,3,6,Eigen::ColMajor> J;
  J.topLeftCorner<3,3>() = -skew(y);
  J.bottomRightCorner<3,3>().setIdentity();

  return J;
}

Matrix<double,2,3,Eigen::ColMajor> g2o::d_proj_d_y ( const double &  f, const Vector3D &  xyz  )

inline

Definition at line 186 of file types_six_dof_expmap.cpp.

Referenced by g2o::EdgeProjectPSI2UV::linearizeOplus().

                                                                                          {
  double z_sq = xyz[2]*xyz[2];
  Matrix<double,2,3,Eigen::ColMajor> J;
  J << f/xyz[2], 0,           -(f*xyz[0])/z_sq,
      0,           f/xyz[2], -(f*xyz[1])/z_sq;
  return J;
}

Matrix3D g2o::d_Tinvpsi_d_psi ( const SE3Quat &  T, const Vector3D &  psi  )

inline

Definition at line 202 of file types_six_dof_expmap.cpp.

References invert_depth(), and g2o::SE3Quat::rotation().

Referenced by g2o::EdgeProjectPSI2UV::linearizeOplus().

                                                                        {
  Matrix3D R = T.rotation().toRotationMatrix();
  Vector3D x = invert_depth(psi);
  Vector3D r1 = R.col(0);
  Vector3D r2 = R.col(1);
  Matrix3D J;
  J.col(0) = r1;
  J.col(1) = r2;
  J.col(2) = -R*x;
  J*=1./psi.z();
  return J;
}

double g2o::deg2rad ( double  degree )

inline

convert from degree to radian

Definition at line 78 of file misc.h.

{
  return degree * 0.01745329251994329576;
}

Vector3D g2o::deltaR ( const Matrix3D &  R )

inline

Definition at line 41 of file se3_ops.h.

Referenced by g2o::Sim3::log(), g2o::SE3Quat::log(), and skew().

void g2o::drawCircle	(	GLfloat	radius,
		int	segments = 32
	)

draw a circle using GL_LINES

Definition at line 72 of file draw_helpers.cpp.

References M_PI.

  {
    double angleStep = (2 * M_PI / (segments));
    glBegin(GL_LINE_STRIP);
    for (int i = 0; i <= segments; i++) {
      double angle = i * angleStep;
      float x = radius * cos(angle);
      float y = radius * sin(angle);
      glVertex3f(x, y, 0.f);
    }
    glEnd();
  }

void g2o::drawDisk

(

GLfloat

radius

)

draw a disk

Definition at line 67 of file draw_helpers.cpp.

  {
    gluDisk(GLUWrapper::getQuadradic(), 0, radius, 32, 1);
  }

G2O_CLI_API bool g2o::dumpEdges	(	std::ostream &	os,
		const OptimizableGraph &	optimizer
	)

dump the edges to a stream, e.g., cout and redirect to gnuplot

Definition at line 180 of file output_helper.cpp.

References __PRETTY_FUNCTION__, g2o::HyperGraph::edges(), g2o::WriteGnuplotAction::Parameters::os, and saveGnuplot().

Referenced by main().

{
  // seek for an action whose name is writeGnuplot in the library
  HyperGraphElementAction* saveGnuplot = HyperGraphActionLibrary::instance()->actionByName("writeGnuplot");
  if (! saveGnuplot ){
    cerr << __PRETTY_FUNCTION__ << ": no action \"writeGnuplot\" registered" << endl;
    return false;
  }
  WriteGnuplotAction::Parameters params;
  params.os = &os;

  // writing all edges
  os << "set terminal x11 noraise" << endl;
  os << "set size ratio -1" << endl;
  os << "plot \"-\" w l" << endl;
  for (HyperGraph::EdgeSet::const_iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
    OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
    (*saveGnuplot)(e, &params);
  }
  os << "e" << endl;

  return true;
}

bool g2o::edgeAllVertsSameDim	(	OptimizableGraph::Edge *	e,
		int	dim
	)

Definition at line 43 of file output_helper.cpp.

References g2o::OptimizableGraph::Vertex::dimension(), and g2o::HyperGraph::Edge::vertices().

Referenced by saveGnuplot().

{
  for (size_t i = 0; i < e->vertices().size(); ++i) {
    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(e->vertices()[i]);
    if (v->dimension() != dim)
      return false;
  }
  return true;
}

G2O_STUFF_API bool g2o::fileExists

(

const char *

filename

)

check if file exists (note a directory is also a file)

Definition at line 116 of file filesys_tools.cpp.

{
  struct stat statInfo;
  return (stat(filename, &statInfo) == 0);
}

void g2o::findArguments	(	const std::string &	option,
		vector< string > &	args,
		int	argc,
		char **	argv
	)

Definition at line 86 of file g2o_common.cpp.

Referenced by loadStandardSolver(), and loadStandardTypes().

{
  args.clear();
  for (int i = 0; i < argc; ++i) {
    if (argv[i] == option && i + 1 < argc) {
      args.push_back(argv[i+1]);
    }
  }
}

void g2o::findConnectedEdgesWithCostLimit	(	HyperGraph::EdgeSet &	selected,
		HyperGraph::EdgeSet &	border,
		HyperGraph::Edge *	start,
		HyperDijkstra::CostFunction *	cost,
		double	maxEdgeCost,
		double	comparisonConditioner
	)

Definition at line 17 of file tools.cpp.

References g2o::BaseVertex< D, T >::push(), and g2o::HyperGraph::Edge::vertices().

Referenced by main().

                                            {

    (void) comparisonConditioner; // no warning (unused)
    typedef std::queue<HyperGraph::Edge*> EdgeDeque;
    EdgeDeque frontier;
    frontier.push(start);

    selected.clear();
    border.clear();

    while (! frontier.empty()){
      HyperGraph::Edge* e=frontier.front();
      frontier.pop();

      const VertexSE2* from = dynamic_cast<const VertexSE2*>(e->vertices()[0]);
      const VertexSE2* to   = dynamic_cast<const VertexSE2*>(e->vertices()[1]);

      if (!(from && to))
        continue;

      double edgecost=(*cost)(e, e->vertices()[0], e->vertices()[1]);
      if (edgecost != std::numeric_limits< double >::max()) {

  if (edgecost > maxEdgeCost) {// + comparisonConditioner) {
    border.insert(e);
  }
  else if (edgecost <= maxEdgeCost) {
    selected.insert(e);
    
    for (HyperGraph::EdgeSet::iterator it=e->vertices()[0]->edges().begin(); 
         it!=e->vertices()[0]->edges().end(); ++it) {
      if (selected.find(*it)==selected.end())
        frontier.push(dynamic_cast<HyperGraph::Edge*>(*it));
    }
    for (HyperGraph::EdgeSet::iterator it=e->vertices()[1]->edges().begin(); 
         it!=e->vertices()[1]->edges().end(); ++it) {
      if (selected.find(*it)==selected.end())
        frontier.push(dynamic_cast<HyperGraph::Edge*>(*it));
    }
  }
  else
    cerr << "? nan ?" << endl;
      }
      else
  cerr << "? max ?" << endl;
    }
  }

g2o::G2O_ATTRIBUTE_CONSTRUCTOR

(

init_types_interactive_online

)

Definition at line 37 of file types_online.cpp.

References g2o::Factory::instance(), and g2o::Factory::registerType().

  {
    Factory* factory = Factory::instance();
    //cerr << "Calling " << __FILE__ << " " << __PRETTY_FUNCTION__ << endl;

    factory->registerType("ONLINE_EDGE_SE2", new HyperGraphElementCreator<OnlineEdgeSE2>);
    factory->registerType("ONLINE_VERTEX_SE2", new HyperGraphElementCreator<OnlineVertexSE2>);

    factory->registerType("ONLINE_VERTEX_SE3:QUAT", new HyperGraphElementCreator<OnlineVertexSE3>);
    factory->registerType("ONLINE_EDGE_SE3:QUAT", new HyperGraphElementCreator<OnlineEdgeSE3>);
  }

g2o::G2O_ATTRIBUTE_CONSTRUCTOR

(

init_slam3d_addons_types

)

Definition at line 55 of file types_slam3d_addons.cpp.

References g2o::types_icp::initialized, g2o::HyperGraphActionLibrary::instance(), g2o::HyperGraphActionLibrary::registerAction(), and g2o::HyperGraphElementAction::setTypeName().

  {
    static bool initialized = false;
    if (initialized)
      return;
    initialized = true;

#ifdef G2O_HAVE_OPENGL
    HyperGraphActionLibrary* actionLib = HyperGraphActionLibrary::instance();
    HyperGraphElementAction* vertexse3eulerdraw=new g2o::VertexSE3DrawAction;
    vertexse3eulerdraw->setTypeName(typeid(VertexSE3Euler).name());
    actionLib->registerAction(vertexse3eulerdraw);

    HyperGraphElementAction* edgese3eulerdraw=new g2o::EdgeSE3DrawAction;
    edgese3eulerdraw->setTypeName(typeid(EdgeSE3Euler).name());
    actionLib->registerAction(edgese3eulerdraw);
#endif
  }

g2o::G2O_ATTRIBUTE_CONSTRUCTOR

(

init_icp_types

)

Definition at line 87 of file types_icp.cpp.

References g2o::types_icp::init().

  {
    types_icp::init();
  }

g2o::G2O_REGISTER_ACTION

(

VertexSE3WriteGnuplotAction

)

g2o::G2O_REGISTER_ACTION

(

VertexPointXYZWriteGnuplotAction

)

g2o::G2O_REGISTER_ACTION

(

EdgeSE3WriteGnuplotAction

)

g2o::G2O_REGISTER_ACTION

(

VertexSE2WriteGnuplotAction

)

g2o::G2O_REGISTER_ACTION

(

VertexPointXYWriteGnuplotAction

)

g2o::G2O_REGISTER_ACTION

(

EdgeSE2WriteGnuplotAction

)

g2o::G2O_REGISTER_ACTION

(

EdgeSE2PointXYWriteGnuplotAction

)

g2o::G2O_REGISTER_ACTION

(

EdgeSE2PointXYBearingWriteGnuplotAction

)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	structure_only_2	,
		new	StructureOnlyCreatorOptimizationAlgorithmProperty("structure_only_2","Optimize the landmark poses (2D)","Eigen", true, 3, 2)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	structure_only_3	,
		new	StructureOnlyCreatorOptimizationAlgorithmProperty("structure_only_3","Optimize the landmark poses (3D)","Eigen", true, 6, 3)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	2dlinear	,
		new	SLAM2DLinearSolverCreatorOptimizationAlgorithmProperty("2dlinear","Solve Orientation + Gauss-Newton: Works only on 2D pose graphs!!","CSparse", false, 3, 3)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	gn_pcg	,
		new	PCGSolverCreatorOptimizationAlgorithmProperty("gn_pcg","Gauss-Newton: PCG solver using block-Jacobi pre-conditioner (variable blocksize)","PCG", false, Eigen::Dynamic, Eigen::Dynamic)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	gn_dense	,
		new	DenseSolverCreatorOptimizationAlgorithmProperty("gn_dense","Gauss-Newton: Dense solver (variable blocksize)","Dense", false, Eigen::Dynamic, Eigen::Dynamic)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	gn_pcg3_2	,
		new	PCGSolverCreatorOptimizationAlgorithmProperty("gn_pcg3_2","Gauss-Newton: PCG solver using block-Jacobi pre-conditioner (fixed blocksize)","PCG", true, 3, 2)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	gn_dense3_2	,
		new	DenseSolverCreatorOptimizationAlgorithmProperty("gn_dense3_2","Gauss-Newton: Dense solver (fixed blocksize)","Dense", true, 3, 2)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	gn_pcg6_3	,
		new	PCGSolverCreatorOptimizationAlgorithmProperty("gn_pcg6_3","Gauss-Newton: PCG solver using block-Jacobi pre-conditioner (fixed blocksize)","PCG", true, 6, 3)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	gn_dense6_3	,
		new	DenseSolverCreatorOptimizationAlgorithmProperty("gn_dense6_3","Gauss-Newton: Dense solver (fixed blocksize)","Dense", true, 6, 3)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	gn_pcg7_3	,
		new	PCGSolverCreatorOptimizationAlgorithmProperty("gn_pcg7_3","Gauss-Newton: PCG solver using block-Jacobi pre-conditioner (fixed blocksize)","PCG", true, 7, 3)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	gn_dense7_3	,
		new	DenseSolverCreatorOptimizationAlgorithmProperty("gn_dense7_3","Gauss-Newton: Dense solver (fixed blocksize)","Dense", true, 7, 3)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	lm_pcg	,
		new	PCGSolverCreatorOptimizationAlgorithmProperty("lm_pcg","Levenberg: PCG solver using block-Jacobi pre-conditioner (variable blocksize)","PCG", false, Eigen::Dynamic, Eigen::Dynamic)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	lm_dense	,
		new	DenseSolverCreatorOptimizationAlgorithmProperty("lm_dense","Levenberg: Dense solver (variable blocksize)","Dense", false,-1,-1)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	lm_pcg3_2	,
		new	PCGSolverCreatorOptimizationAlgorithmProperty("lm_pcg3_2","Levenberg: PCG solver using block-Jacobi pre-conditioner (fixed blocksize)","PCG", true, 3, 2)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	lm_dense3_2	,
		new	DenseSolverCreatorOptimizationAlgorithmProperty("lm_dense3_2","Levenberg: Dense solver (fixed blocksize)","Dense", true, 3, 2)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	lm_pcg6_3	,
		new	PCGSolverCreatorOptimizationAlgorithmProperty("lm_pcg6_3","Levenberg: PCG solver using block-Jacobi pre-conditioner (fixed blocksize)","PCG", true, 6, 3)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	lm_dense6_3	,
		new	DenseSolverCreatorOptimizationAlgorithmProperty("lm_dense6_3","Levenberg: Dense solver (fixed blocksize)","Dense", true, 6, 3)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	lm_dense7_3	,
		new	DenseSolverCreatorOptimizationAlgorithmProperty("lm_dense7_3","Levenberg: Dense solver (fixed blocksize)","Dense", true, 7, 3)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	lm_pcg7_3	,
		new	PCGSolverCreatorOptimizationAlgorithmProperty("lm_pcg7_3","Levenberg: PCG solver using block-Jacobi pre-conditioner (fixed blocksize)","PCG", true, 7, 3)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	gn_var_eigen	,
		new	EigenSolverCreatorOptimizationAlgorithmProperty("gn_var_eigen","Gauss-Newton: Cholesky solver using Eigen's Sparse Cholesky methods (variable blocksize)","Eigen", false, Eigen::Dynamic, Eigen::Dynamic)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	lm_var_eigen	,
		new	EigenSolverCreatorOptimizationAlgorithmProperty("lm_var_eigen","Levenberg: Cholesky solver using Eigen's Sparse Cholesky methods (variable blocksize)","Eigen", false, Eigen::Dynamic, Eigen::Dynamic)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	gn_var	,
		new	CSparseSolverCreatorOptimizationAlgorithmProperty("gn_var","Gauss-Newton: Cholesky solver using CSparse (variable blocksize)","CSparse", false, Eigen::Dynamic, Eigen::Dynamic)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	gn_fix3_2	,
		new	CSparseSolverCreatorOptimizationAlgorithmProperty("gn_fix3_2","Gauss-Newton: Cholesky solver using CSparse (fixed blocksize)","CSparse", true, 3, 2)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	dl_var_eigen	,
		new	EigenSolverCreatorOptimizationAlgorithmProperty("dl_var_eigen","Dogleg: Cholesky solver using Eigen's Sparse Cholesky methods (variable blocksize)","Eigen", false, Eigen::Dynamic, Eigen::Dynamic)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	gn_fix6_3	,
		new	CSparseSolverCreatorOptimizationAlgorithmProperty("gn_fix6_3","Gauss-Newton: Cholesky solver using CSparse (fixed blocksize)","CSparse", true, 6, 3)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	gn_fix7_3	,
		new	CSparseSolverCreatorOptimizationAlgorithmProperty("gn_fix7_3","Gauss-Newton: Cholesky solver using CSparse (fixed blocksize)","CSparse", true, 7, 3)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	lm_var	,
		new	CSparseSolverCreatorOptimizationAlgorithmProperty("lm_var","Levenberg: Cholesky solver using CSparse (variable blocksize)","CSparse", false, Eigen::Dynamic, Eigen::Dynamic)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	lm_fix3_2	,
		new	CSparseSolverCreatorOptimizationAlgorithmProperty("lm_fix3_2","Levenberg: Cholesky solver using CSparse (fixed blocksize)","CSparse", true, 3, 2)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	lm_fix6_3	,
		new	CSparseSolverCreatorOptimizationAlgorithmProperty("lm_fix6_3","Levenberg: Cholesky solver using CSparse (fixed blocksize)","CSparse", true, 6, 3)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	lm_fix7_3	,
		new	CSparseSolverCreatorOptimizationAlgorithmProperty("lm_fix7_3","Levenberg: Cholesky solver using CSparse (fixed blocksize)","CSparse", true, 7, 3)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	gn_var_cholmod	,
		new	CholmodSolverCreatorOptimizationAlgorithmProperty("gn_var_cholmod","Gauss-Newton: Cholesky solver using CHOLMOD (variable blocksize)","CHOLMOD", false, Eigen::Dynamic, Eigen::Dynamic)
	)

Referenced by g2o::StructureOnlyCreator::construct(), and g2o::SLAM2DLinearSolverCreator::construct().

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	gn_fix3_2_cholmod	,
		new	CholmodSolverCreatorOptimizationAlgorithmProperty("gn_fix3_2_cholmod","Gauss-Newton: Cholesky solver using CHOLMOD (fixed blocksize)","CHOLMOD", true, 3, 2)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	gn_fix6_3_cholmod	,
		new	CholmodSolverCreatorOptimizationAlgorithmProperty("gn_fix6_3_cholmod","Gauss-Newton: Cholesky solver using CHOLMOD (fixed blocksize)","CHOLMOD", true, 6, 3)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	gn_fix7_3_cholmod	,
		new	CholmodSolverCreatorOptimizationAlgorithmProperty("gn_fix7_3_cholmod","Gauss-Newton: Cholesky solver using CHOLMOD (fixed blocksize)","CHOLMOD", true, 7, 3)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	lm_var_cholmod	,
		new	CholmodSolverCreatorOptimizationAlgorithmProperty("lm_var_cholmod","Levenberg: Cholesky solver using CHOLMOD (variable blocksize)","CHOLMOD", false, Eigen::Dynamic, Eigen::Dynamic)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	lm_fix3_2_cholmod	,
		new	CholmodSolverCreatorOptimizationAlgorithmProperty("lm_fix3_2_cholmod","Levenberg: Cholesky solver using CHOLMOD (fixed blocksize)","CHOLMOD", true, 3, 2)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	lm_fix6_3_cholmod	,
		new	CholmodSolverCreatorOptimizationAlgorithmProperty("lm_fix6_3_cholmod","Levenberg: Cholesky solver using CHOLMOD (fixed blocksize)","CHOLMOD", true, 6, 3)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	lm_fix7_3_cholmod	,
		new	CholmodSolverCreatorOptimizationAlgorithmProperty("lm_fix7_3_cholmod","Levenberg: Cholesky solver using CHOLMOD (fixed blocksize)","CHOLMOD", true, 7, 3)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	dl_var	,
		new	CSparseSolverCreatorOptimizationAlgorithmProperty("dl_var","Dogleg: Cholesky solver using CSparse (variable blocksize)","CSparse", false, Eigen::Dynamic, Eigen::Dynamic)
	)

g2o::G2O_REGISTER_OPTIMIZATION_ALGORITHM	(	dl_var_cholmod	,
		new	CholmodSolverCreatorOptimizationAlgorithmProperty("dl_var_cholmod","Dogleg: Cholesky solver using CHOLMOD (variable blocksize)","CHOLMOD", false, Eigen::Dynamic, Eigen::Dynamic)
	)

g2o::G2O_REGISTER_OPTIMIZATION_LIBRARY

(

structure_only

)

g2o::G2O_REGISTER_OPTIMIZATION_LIBRARY

(

slam2d_linear

)

g2o::G2O_REGISTER_OPTIMIZATION_LIBRARY

(

pcg

)

g2o::G2O_REGISTER_OPTIMIZATION_LIBRARY

(

dense

)

g2o::G2O_REGISTER_OPTIMIZATION_LIBRARY

(

eigen

)

g2o::G2O_REGISTER_OPTIMIZATION_LIBRARY

(

csparse

)

g2o::G2O_REGISTER_OPTIMIZATION_LIBRARY

(

cholmod

)

Referenced by g2o::StructureOnlyCreator::construct(), and g2o::SLAM2DLinearSolverCreator::construct().

g2o::G2O_REGISTER_TYPE	(	VERTEX_SE3:QUAT	,
		VertexSE3
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE3:QUAT	,
		EdgeSE3
	)

g2o::G2O_REGISTER_TYPE	(	VERTEX_TRACKXYZ	,
		VertexPointXYZ
	)

g2o::G2O_REGISTER_TYPE	(	PARAMS_SE3OFFSET	,
		ParameterSE3Offset
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE3_TRACKXYZ	,
		EdgeSE3PointXYZ
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE3_PRIOR	,
		EdgeSE3Prior
	)

g2o::G2O_REGISTER_TYPE	(	CACHE_SE3_OFFSET	,
		CacheSE3Offset
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE3_OFFSET	,
		EdgeSE3Offset
	)

g2o::G2O_REGISTER_TYPE	(	PARAMS_CAMERACALIB	,
		ParameterCamera
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_V_V_GICP	,
		Edge_V_V_GICP
	)

g2o::G2O_REGISTER_TYPE	(	VERTEX_SE3:EXPMAP	,
		VertexSE3Expmap
	)

g2o::G2O_REGISTER_TYPE	(	PARAMS_STEREOCAMERACALIB	,
		ParameterStereoCamera
	)

g2o::G2O_REGISTER_TYPE	(	VERTEX_SIM3:EXPMAP	,
		VertexSim3Expmap
	)

g2o::G2O_REGISTER_TYPE	(	VERTEX3	,
		VertexSE3Euler
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE3:EXPMAP	,
		EdgeSE3Expmap
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SIM3:EXPMAP	,
		EdgeSim3
	)

g2o::G2O_REGISTER_TYPE	(	VERTEX_SE2	,
		VertexSE2
	)

g2o::G2O_REGISTER_TYPE	(	VERTEX_SEGMENT2D	,
		VertexSegment2D
	)

g2o::G2O_REGISTER_TYPE	(	CACHE_CAMERA	,
		CacheCamera
	)

g2o::G2O_REGISTER_TYPE	(	EDGE3	,
		EdgeSE3Euler
	)

g2o::G2O_REGISTER_TYPE	(	VERTEX_CAM	,
		VertexCam
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_PROJECT_XYZ2UV:EXPMAP	,
		EdgeProjectXYZ2UV
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_PROJECT_DISPARITY	,
		EdgeSE3PointXYZDisparity
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_PROJECT_SIM3_XYZ:EXPMAP	,
		EdgeSim3ProjectXYZ
	)

g2o::G2O_REGISTER_TYPE	(	VERTEX_XY	,
		VertexPointXY
	)

g2o::G2O_REGISTER_TYPE	(	VERTEX_LINE2D	,
		VertexLine2D
	)

g2o::G2O_REGISTER_TYPE	(	VERTEX_PLANE	,
		VertexPlane
	)

g2o::G2O_REGISTER_TYPE	(	VERTEX_XYZ	,
		VertexSBAPointXYZ
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_PROJECT_XYZ2UVU:EXPMAP	,
		EdgeProjectXYZ2UVU
	)

g2o::G2O_REGISTER_TYPE	(	PARAMS_SE2OFFSET	,
		ParameterSE2Offset
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_PROJECT_DEPTH	,
		EdgeSE3PointXYZDepth
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE3_PLANE_CALIB	,
		EdgeSE3PlaneSensorCalib
	)

g2o::G2O_REGISTER_TYPE	(	VERTEX_TAG	,
		VertexTag
	)

g2o::G2O_REGISTER_TYPE	(	VERTEX_INTRINSICS	,
		VertexIntrinsics
	)

g2o::G2O_REGISTER_TYPE	(	PARAMS_CAMERAPARAMETERS	,
		CameraParameters
	)

g2o::G2O_REGISTER_TYPE	(	CACHE_SE2_OFFSET	,
		CacheSE2Offset
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE2_SEGMENT2D	,
		EdgeSE2Segment2D
	)

g2o::G2O_REGISTER_TYPE	(	ROBOTLASER1	,
		RobotLaser
	)

g2o::G2O_REGISTER_TYPE	(	VERTEX_ODOM_DIFFERENTIAL	,
		VertexOdomDifferentialParams
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_PRIOR_SE2	,
		EdgeSE2Prior
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_POINTXYZ	,
		EdgePointXYZ
	)

g2o::G2O_REGISTER_TYPE	(	VERTEX_LINE3D	,
		VertexLine3D
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE2_SEGMENT2D_LINE	,
		EdgeSE2Segment2DLine
	)

g2o::G2O_REGISTER_TYPE	(	VERTEX_ELLIPSE	,
		VertexEllipse
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_PROJECT_P2MC	,
		EdgeProjectP2MC
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_PRIOR_SE2_XY	,
		EdgeSE2XYPrior
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE3_LINE3D	,
		EdgeSE3Line3D
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE2_SEGMENT2D_POINTLINE	,
		EdgeSE2Segment2DPointLine
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE2_CALIB	,
		EdgeSE2SensorCalib
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_PROJECT_P2MC_INTRINSICS	,
		EdgeProjectP2MC_Intrinsics
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE2_ODOM_DIFFERENTIAL_CALIB	,
		EdgeSE2OdomDifferentialCalib
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE2	,
		EdgeSE2
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_LINE3D	,
		EdgeLine3D
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE2_LINE2D	,
		EdgeSE2Line2D
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_PROJECT_P2SC	,
		EdgeProjectP2SC
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_LINE2D	,
		EdgeLine2D
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE2_XY	,
		EdgeSE2PointXY
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_PLANE	,
		EdgePlane
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_CAM	,
		EdgeSBACam
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_BEARING_SE2_XY	,
		EdgeSE2PointXYBearing
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_LINE2D_POINTXY	,
		EdgeLine2DPointXY
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE3_CALIB	,
		EdgeSE3Calib
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SCALE	,
		EdgeSBAScale
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE2_XY_CALIB	,
		EdgeSE2PointXYCalib
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE2_OFFSET	,
		EdgeSE2Offset
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE2_POINTXY_OFFSET	,
		EdgeSE2PointXYOffset
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_POINTXY	,
		EdgePointXY
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE2_TWOPOINTSXY	,
		EdgeSE2TwoPointsXY
	)

g2o::G2O_REGISTER_TYPE	(	EDGE_SE2_LOTSOFXY	,
		EdgeSE2LotsOfXY
	)

g2o::G2O_REGISTER_TYPE_GROUP

(

slam3d

)

g2o::G2O_REGISTER_TYPE_GROUP

(

sim3

)

g2o::G2O_REGISTER_TYPE_GROUP

(

slam3d_addons

)

g2o::G2O_REGISTER_TYPE_GROUP

(

expmap

)

g2o::G2O_REGISTER_TYPE_GROUP

(

slam2d_segment

)

g2o::G2O_REGISTER_TYPE_GROUP

(

slam2d

)

g2o::G2O_REGISTER_TYPE_GROUP

(

icp

)

g2o::G2O_REGISTER_TYPE_GROUP

(

sba

)

g2o::G2O_REGISTER_TYPE_GROUP

(

data

)

g2o::G2O_REGISTER_TYPE_GROUP

(

sclam

)

g2o::G2O_USE_OPTIMIZATION_LIBRARY

(

cholmod

)

g2o::G2O_USE_TYPE_GROUP

(

sba

)

g2o::G2O_USE_TYPE_GROUP

(

slam2d

)

G2O_STUFF_API double g2o::get_monotonic_time

(

)

return a monotonic increasing time which basically does not need to have a reference point. Consider this for measuring how long some code fragments required to execute.

On Linux we call clock_gettime() on other systems we currently call get_time().

Definition at line 113 of file timeutil.cpp.

References get_time().

Referenced by g2o::BlockSolver< Traits >::computeMarginals(), g2o::LinearSolverEigen< MatrixType >::computeSymbolicDecomposition(), g2o::LinearSolverCholmodOnline< MatrixType >::computeSymbolicDecomposition(), g2o::LinearSolverCSparse< MatrixType >::computeSymbolicDecomposition(), g2o::LinearSolverCholmod< MatrixType >::computeSymbolicDecomposition(), get_time(), main(), g2o::SparseOptimizer::optimize(), g2o::OptimizationAlgorithmGaussNewton::solve(), g2o::OptimizationAlgorithmLevenberg::solve(), g2o::OptimizationAlgorithmDogleg::solve(), g2o::LinearSolverCholmodOnline< MatrixType >::solve(), g2o::LinearSolverEigen< MatrixType >::solve(), g2o::LinearSolverCSparse< MatrixType >::solve(), g2o::LinearSolverCholmod< MatrixType >::solve(), g2o::BlockSolver< Traits >::solve(), tictoc(), and g2o::ScopeTime::~ScopeTime().

{
#if (defined(_POSIX_TIMERS) && (_POSIX_TIMERS+0 >= 0) && defined(_POSIX_MONOTONIC_CLOCK))
  struct timespec ts;
  clock_gettime(CLOCK_MONOTONIC, &ts);
  return ts.tv_sec + ts.tv_nsec*1e-9;
#else
  return get_time();
#endif
}

double g2o::get_time ( )

inline

return the current time in seconds since 1. Jan 1970

Definition at line 92 of file timeutil.h.

References G2O_STUFF_API, and get_monotonic_time().

Referenced by get_monotonic_time().

{
  struct timeval ts;
  gettimeofday(&ts,0);
  return ts.tv_sec + ts.tv_usec*1e-6;
}

G2O_STUFF_API std::string g2o::getBasename

(

const std::string &

filename

)

return the basename of the given filename /etc/fstab -> fstab

Definition at line 78 of file filesys_tools.cpp.

{
#ifdef WINDOWS
  std::string::size_type lastSlash = filename.find_last_of('\\');
#else
  std::string::size_type lastSlash = filename.find_last_of('/');
#endif
  if (lastSlash != std::string::npos)
    return filename.substr(lastSlash + 1);
  else
    return filename;
}

G2O_STUFF_API std::string g2o::getDirname

(

const std::string &

filename

)

return the directory of a given filename /etc/fstab -> /etc

Definition at line 91 of file filesys_tools.cpp.

Referenced by loadStandardSolver(), and loadStandardTypes().

{
#ifdef WINDOWS
  std::string::size_type lastSlash = filename.find_last_of('\\');
#else
  std::string::size_type lastSlash = filename.find_last_of('/');
#endif
  if (lastSlash != std::string::npos)
    return filename.substr(0, lastSlash);
  else
    return "";
}

G2O_STUFF_API std::string g2o::getFileExtension

(

const std::string &

filename

)

get filename extension (the part after the last .), e.g. the extension of file.txt is txt

Definition at line 60 of file filesys_tools.cpp.

Referenced by gnudump_edges(), gnudump_features(), and saveGnuplot().

{
  std::string::size_type lastDot = filename.find_last_of('.');
  if (lastDot != std::string::npos)
    return filename.substr(lastDot + 1);
  else
    return "";
}

G2O_STUFF_API std::vector< std::string > g2o::getFilesByPattern

(

const char *

pattern

)

return all files that match a given pattern, e.g., ~/blaa*.txt, /tmp/a*

Definition at line 122 of file filesys_tools.cpp.

Referenced by g2o::DlWrapper::openLibraries().

{
  std::vector<std::string> result;

#ifdef WINDOWS

  HANDLE hFind;
  WIN32_FIND_DATA FData;
  if ((hFind = FindFirstFile(pattern, &FData)) != INVALID_HANDLE_VALUE) {
    do {
      result.push_back(FData.cFileName);
    } while (FindNextFile(hFind, &FData));
    FindClose(hFind);
  }
  
#elif (defined (UNIX) || defined (CYGWIN)) && !defined(ANDROID)

  wordexp_t p;
  wordexp(pattern, &p, 0);

  // For some reason, wordexp sometimes fails on an APPLE machine to
  // return anything; therefore, run it several times until we do find
  // something - or give up
#ifdef __APPLE__
  for (int k = 0; (k < 100) && (p.we_wordc == 0); k++) {
    //chrono::milliseconds duration(20);
    //this_thread::sleep_for(duration);
    wordexp(pattern, &p, WRDE_APPEND);
  }
#endif

  result.reserve(p.we_wordc);
  for (size_t i = 0; i < p.we_wordc; ++i)
    result.push_back(p.we_wordv[i]);
  
  wordfree(&p);

#endif

  return result;
}

G2O_STUFF_API std::string g2o::getPureFilename

(

const std::string &

filename

)

get the filename without the extension. file.txt -> file

Definition at line 69 of file filesys_tools.cpp.

Referenced by gnudump_edges(), gnudump_features(), and saveGnuplot().

{
  std::string::size_type lastDot = filename.find_last_of('.');
  if (lastDot != std::string::npos)
    return filename.substr(0, lastDot);
  else
    return filename;
}

template<typename T >

T g2o::hypot ( T  x, T  y  )

inline

return the hypot of x and y

Definition at line 61 of file misc.h.

Referenced by g2o::OdomConvert::convertToVelocity().

{
  return (T) (sqrt(x*x + y*y));
}

template<typename T >

T g2o::hypot_sqr ( T  x, T  y  )

inline

return the squared hypot of x and y

Definition at line 70 of file misc.h.

Referenced by g2o::tutorial::Simulator::simulate().

{
  return x*x + y*y;
}

double g2o::inverse_theta ( double  th )

inline

inverse of an angle, i.e., +180 degree

Definition at line 112 of file misc.h.

References M_PI, and normalize_theta().

{
  return normalize_theta(th + M_PI);
}

Vector3D g2o::invert_depth ( const Vector3D &  x )

inline

Definition at line 66 of file types_six_dof_expmap.cpp.

References unproject2d().

Referenced by g2o::EdgeProjectPSI2UV::computeError(), d_Tinvpsi_d_psi(), and g2o::EdgeProjectPSI2UV::linearizeOplus().

                                                {
  return unproject2d(x.head<2>())/x[2];
}

static void g2o::jac_quat3_euler3 ( Eigen::Matrix< double, 6, 6, Eigen::ColMajor > &  J, const Isometry3D &  t  )

static

conversion code from Euler angles

Definition at line 37 of file edge_se3_euler.cpp.

References g2o::internal::fromVectorQT(), g2o::internal::toVectorET(), and g2o::internal::toVectorQT().

Referenced by g2o::G2oSlamInterface::addEdge(), g2o::EdgeSE3Euler::read(), and g2o::EdgeSE3Euler::write().

{
  Vector7d t0 = g2o::internal::toVectorQT(t);

  double delta=1e-6;
  double idelta= 1. / (2. * delta);

  Vector7d ta = t0;
  Vector7d tb = t0;
  for (int i=0; i<6; i++){
    ta=tb=t0;
    ta[i]-=delta;
    tb[i]+=delta;
    Vector6d ea = g2o::internal::toVectorET(g2o::internal::fromVectorQT(ta));
    Vector6d eb = g2o::internal::toVectorET(g2o::internal::fromVectorQT(tb));
    J.col(3)=(eb-ea)*idelta;
  }
}

G2O_CLI_API void g2o::loadStandardSolver	(	DlWrapper &	dlSolverWrapper,
		int	argc = 0,
		char **	argv = 0
	)

will also look for -solverlib in (argc, argv) and load that solver

Definition at line 133 of file g2o_common.cpp.

References findArguments(), getDirname(), getMyInstance(), g2o::DlWrapper::openLibraries(), g2o::DlWrapper::openLibrary(), PATH_SEPARATOR, SOLVERS_PATTERN, and strSplit().

Referenced by main().

{
  char * envSolversPath = getenv("G2O_SOLVERS_DIR");
  string solversPath = G2O_DEFAULT_SOLVERS_DIR_;

  if (envSolversPath != NULL) {
      solversPath = envSolversPath;
  } else {
#if (defined UNIX)
    if (dladdr(&info, &info) != 0) {
      solversPath = getDirname(info.dli_fname);
    }
#elif (defined WINDOWS)
    char libFilename[MAX_PATH + 1];
    HMODULE instance = getMyInstance();
    if (instance && GetModuleFileName(instance, libFilename, MAX_PATH) > 0) {
      solversPath = getDirname(libFilename);
    }
#endif
  }

  vector<string> paths = strSplit(solversPath, PATH_SEPARATOR);
  for (vector<string>::const_iterator it = paths.begin(); it != paths.end(); ++it) {
    if (it->size() > 0)
      dlSolverWrapper.openLibraries(*it, SOLVERS_PATTERN);
  }

  vector<string> libs;
  if (argc > 0 && argv != 0)
    findArguments("-solverlib", libs, argc, argv);
  for (vector<string>::const_iterator it = libs.begin(); it != libs.end(); ++it) {
    cerr << "Loading solver " << *it << endl;
    dlSolverWrapper.openLibrary(*it);
  }
}

G2O_CLI_API void g2o::loadStandardTypes	(	DlWrapper &	dlWrapper,
		int	argc = 0,
		char **	argv = 0
	)

will also look for -typeslib in (argc, argv) and load that types

Definition at line 96 of file g2o_common.cpp.

References findArguments(), getDirname(), getMyInstance(), g2o::DlWrapper::openLibraries(), g2o::DlWrapper::openLibrary(), PATH_SEPARATOR, strSplit(), and TYPES_PATTERN.

Referenced by main().

{
  char * envTypesPath = getenv("G2O_TYPES_DIR");
  string typesPath;

  if (envTypesPath != NULL) {
    typesPath = envTypesPath;
  } else {
    typesPath = G2O_DEFAULT_TYPES_DIR_;
#if (defined UNIX)
    if (dladdr(&info, &info) != 0) {
      typesPath = getDirname(info.dli_fname);
    }
#elif (defined WINDOWS)
    char libFilename[MAX_PATH + 1];
    HMODULE instance = getMyInstance();
    if (instance && GetModuleFileName(instance, libFilename, MAX_PATH) > 0) {
      typesPath = getDirname(libFilename);
    }
#endif
  }

  vector<string> paths = strSplit(typesPath, PATH_SEPARATOR);
  for (vector<string>::const_iterator it = paths.begin(); it != paths.end(); ++it) {
    if (it->size() > 0)
      dlTypesWrapper.openLibraries(*it, TYPES_PATTERN);
  }

  vector<string> libs;
  if (argc > 0 && argv != 0)
    findArguments("-typeslib", libs, argc, argv);
  for (vector<string>::const_iterator it = libs.begin(); it != libs.end(); ++it) {
    cerr << "Loading types " << *it << endl;
    dlTypesWrapper.openLibrary(*it);
  }
}

double g2o::normalize_theta ( double  theta )

inline

normalize the angle

Definition at line 94 of file misc.h.

References M_PI.

Referenced by g2o::EdgeSE2PointXYBearing::computeError(), g2o::EdgeSE2Line2D::computeError(), g2o::EdgeSE2Segment2DLine::computeError(), g2o::EdgeSE2Segment2DPointLine::computeError(), g2o::EdgeSE2Line2D::initialEstimate(), g2o::tutorial::SE2::inverse(), g2o::SE2::inverse(), inverse_theta(), operator*(), g2o::tutorial::SE2::operator*(), g2o::tutorial::SE2::operator*=(), g2o::SE2::operator*=(), g2o::VertexSE2::oplusImpl(), g2o::VertexLine2D::oplusImpl(), g2o::OnlineVertexSE2::oplusUpdatedEstimate(), g2o::EdgeSE2Line2D::setMeasurementFromState(), and g2o::SolverSLAM2DLinear::solveOrientation().

{
  if (theta >= -M_PI && theta < M_PI)
    return theta;
  
  double multiplier = floor(theta / (2*M_PI));
  theta = theta - multiplier*2*M_PI;
  if (theta >= M_PI)
    theta -= 2*M_PI;
  if (theta < -M_PI)
    theta += 2*M_PI;

  return theta;
}

Line2D g2o::operator* ( const SE2 &  t, const Line2D &  l  )

inline

Definition at line 48 of file line_2d.h.

References normalize_theta(), g2o::SE2::rotation(), and g2o::SE2::translation().

                                                           {
    Line2D est = l;
    est[0] += t.rotation().angle();
    est[0] = normalize_theta(est[0]);
    Vector2D n(cos(est[0]), sin(est[0]));
    est[1] += n.dot(t.translation());
    return est;
  }

G2O_TYPES_SLAM3D_ADDONS_API Line3D g2o::operator*	(	const Isometry3D &	t,
		const Line3D &	line
	)

Definition at line 94 of file line3d.cpp.

References _skew().

                                                           {
    Matrix6d A=Matrix6d::Zero();
    A.block<3,3>(0,0)=t.linear();
    A.block<3,3>(0,3)= _skew(t.translation())*t.linear();
    A.block<3,3>(3,3)=t.linear();
    Vector6d v = (Vector6d)line;
    // cout << "v" << endl << v << endl;
    // cout << "A" << endl << A << endl;
    // cout << "Av" << endl << A*v << endl;
    return Line3D(A*v);
  }

Plane3D g2o::operator* ( const Eigen::Isometry3d &  t, const Plane3D &  plane  )

inline

Definition at line 120 of file plane3d.h.

References g2o::Plane3D::_coeffs.

                                                                          {
    Eigen::Vector4d v=plane._coeffs;
    Eigen::Vector4d v2;
    Eigen::Matrix3d R=t.rotation();
    v2.head<3>() = R*v.head<3>();
    v2(3)=v(3) - t.translation().dot(v2.head<3>());
    return Plane3D(v2);
  };

bool g2o::operator<	(	const HyperDijkstra::AdjacencyMapEntry &	a,
		const HyperDijkstra::AdjacencyMapEntry &	b
	)

Definition at line 80 of file hyper_dijkstra.cpp.

References g2o::HyperDijkstra::AdjacencyMapEntry::distance().

  {
    return a.distance()>b.distance();
  }

std::ostream & g2o::operator<<	(	std::ostream &	os,
		const std::vector< int > &	v
	)

Definition at line 68 of file command_args.cpp.

                                                               {
  if (v.size()){
    os << v[0];
  }
  for (size_t i=1; i<v.size(); i++){
    os << "," << v[i];
  }
  return os;
}

std::ostream & g2o::operator<<	(	std::ostream &	os,
		const std::vector< double > &	v
	)

Definition at line 539 of file command_args.cpp.

{
  if (v.size())
    os << v[0];
  for (size_t i=1; i<v.size(); i++)
    os << ";" << v[i];
  return os;
}

G2O_CORE_API std::ostream & g2o::operator<<	(	std::ostream &	os,
		const G2OBatchStatistics &	st
	)

timings

Definition at line 48 of file batch_stats.cpp.

References PTHING.

  {
    os << PTHING(iteration);

    os << PTHING( numVertices ); // how many vertices are involved
    os << PTHING( numEdges ); // hoe many edges
    os << PTHING(  chi2 );  // total chi2
    
    // nonlinear part
    os << PTHING(  timeResiduals );  
    os << PTHING(  timeLinearize );   // jacobians
    os << PTHING(  timeQuadraticForm ); // construct the quadratic form in the graph
    
    // block_solver (constructs Ax=b, plus maybe schur);
    os << PTHING(  timeSchurComplement ); // compute schur complement (0 if not done);
    
    // linear solver (computes Ax=b); );
    os << PTHING(  timeSymbolicDecomposition ); // symbolic decomposition (0 if not done);
    os << PTHING(  timeNumericDecomposition ); // numeric decomposition  (0 if not done);
    os << PTHING(  timeLinearSolution );             // total time for solving Ax=b
    os << PTHING(  iterationsLinearSolver );  // iterations of PCG
    os << PTHING(  timeUpdate ); // oplus
    os << PTHING(  timeIteration ); // total time );

    os << PTHING( levenbergIterations );
    os << PTHING( timeLinearSolver);

    os << PTHING(hessianDimension);
    os << PTHING(hessianPoseDimension);
    os << PTHING(hessianLandmarkDimension);
    os << PTHING(choleskyNNZ);
    os << PTHING(timeMarginals);

    return os;
  };

std::ostream& g2o::operator<< ( std::ostream &  out_str, const SBACam &  cam  )

inline

Definition at line 184 of file sbacam.h.

References g2o::SBACam::Kcam, g2o::SE3Quat::rotation(), g2o::SE3Quat::translation(), g2o::SBACam::w2i, and g2o::SBACam::w2n.

  {
    out_str << cam.translation().transpose() << std::endl;
    out_str << cam.rotation().coeffs().transpose() << std::endl << std::endl;
    out_str << cam.Kcam << std::endl << std::endl;
    out_str << cam.w2n << std::endl << std::endl;
    out_str << cam.w2i << std::endl << std::endl;

    return out_str;
  };

template<class MatrixType >

std::ostream & g2o::operator<<	(	std::ostream &	os,
		const SparseBlockMatrix< MatrixType > &	m
	)

Definition at line 378 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::blockCols().

                                                                                  {
    os << "RBI: " << m.rowBlockIndices().size();
    for (size_t i=0; i<m.rowBlockIndices().size(); ++i)
      os << " " << m.rowBlockIndices()[i];
    os << std::endl;
    os << "CBI: " << m.colBlockIndices().size();
    for (size_t i=0; i<m.colBlockIndices().size(); ++i)
      os << " " << m.colBlockIndices()[i];
    os << std::endl;

    for (size_t i=0; i<m.blockCols().size(); ++i){
      for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=m.blockCols()[i].begin(); it!=m.blockCols()[i].end(); ++it){
        const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
        os << "BLOCK: " << it->first << " " << i << std::endl;
        os << *b << std::endl;
      }
    }
    return os;
  }

std::ostream& g2o::operator<< ( std::ostream &  out_str, const Sim3 &  sim3  )

inline

Definition at line 285 of file sim3.h.

References g2o::Sim3::rotation(), g2o::Sim3::scale(), and g2o::Sim3::translation().

  {
    out_str << sim3.rotation().coeffs() << std::endl;
    out_str << sim3.translation() << std::endl;
    out_str << sim3.scale() << std::endl;

    return out_str;
  }

std::ostream& g2o::operator<< ( std::ostream &  out_str, const SE3Quat &  se3  )

inline

Definition at line 295 of file se3quat.h.

References g2o::SE3Quat::to_homogeneous_matrix().

  {
    out_str << se3.to_homogeneous_matrix()  << std::endl;
    return out_str;
  }

std::istream & g2o::operator>>	(	std::istream &	is,
		std::vector< int > &	v
	)

Definition at line 46 of file command_args.cpp.

                                                         {
  string s;
  if (! (is >> s) )
    return is;

  const char* c = s.c_str();
  char* caux = const_cast<char*>(c);

  v.clear();
  bool hasNextValue=true;
  while(hasNextValue){
    int i = static_cast<int>(strtol(c,&caux,10));
    if (c!=caux){
      c=caux;
      c++;
      v.push_back(i);
    } else
      hasNextValue = false;
  }
  return is;
}

std::istream & g2o::operator>>	(	std::istream &	is,
		std::vector< double > &	v
	)

Definition at line 517 of file command_args.cpp.

                                                            {
  string s;
  if (! (is >> s) )
    return is;

  const char* c = s.c_str();
  char* caux = const_cast<char*>(c);

  v.clear();
  bool hasNextValue=true;
  while(hasNextValue){
    double i=strtod(c,&caux);
    if (c!=caux){
      c=caux;
      c++;
      v.push_back(i);
    } else
      hasNextValue = false;
  }
  return is;
}

Vector2D g2o::project ( const Vector3D &  v )

inline

Definition at line 50 of file se3_ops.h.

Referenced by g2o::EdgeSim3ProjectXYZ::computeError(), and skew().

Vector3D g2o::project ( const Vector4D &  v )

inline

Definition at line 58 of file se3_ops.h.

Vector2D g2o::project2d

(

const Vector3D &

v

)

Definition at line 51 of file types_six_dof_expmap.cpp.

Referenced by g2o::CameraParameters::cam_map().

                                       {
  Vector2D res;
  res(0) = v(0)/v(2);
  res(1) = v(1)/v(2);
  return res;
}

double g2o::rad2deg ( double  rad )

inline

convert from radian to degree

Definition at line 86 of file misc.h.

{
  return rad * 57.29577951308232087721;
}

template<class SampleType , class CovarianceType >

void g2o::reconstructGaussian	(	SampleType &	mean,
		CovarianceType &	covariance,
		const std::vector< SigmaPoint< SampleType >, Eigen::aligned_allocator< SigmaPoint< SampleType > > > &	sigmaPoints
	)

Definition at line 78 of file unscented.h.

Referenced by g2o::EdgeLabeler::labelEdge(), testMarginals(), and unscentedTest().

                                                                                                                                   {

    mean.fill(0);
    covariance.fill(0);
    for (size_t i=0; i<sigmaPoints.size(); i++){
      mean += sigmaPoints[i]._wi * sigmaPoints[i]._sample;
    }
    for (size_t i=0; i<sigmaPoints.size(); i++){
      SampleType delta = sigmaPoints[i]._sample - mean;
      covariance += sigmaPoints[i]._wp * ( delta* delta.transpose() ) ;
    }

  }

double G2O_STUFF_API g2o::sampleGaussian

(

std::mt19937 *

generator

)

Definition at line 41 of file sampler.cpp.

References _univariateSampler().

Referenced by g2o::GaussianSampler< typename EdgeType::ErrorVector, InformationType >::generateSample(), and main().

                                              {
    if (generator)
      return _univariateSampler(*generator);
    return _univariateSampler(_gen_real);
  }

double G2O_STUFF_API g2o::sampleUniform	(	double	min,
		double	max,
		std::mt19937 *	generator
	)

Definition at line 35 of file sampler.cpp.

References _uniformReal.

Referenced by main().

                                                                     {
    if (generator)
      return _uniformReal(*generator)*(max-min)+min;
    return _uniformReal(_gen_real)*(max-min)+min;
  }

template<class SampleType , class CovarianceType >

bool g2o::sampleUnscented	(	std::vector< SigmaPoint< SampleType >, Eigen::aligned_allocator< SigmaPoint< SampleType > > > &	sigmaPoints,
		const SampleType &	mean,
		const CovarianceType &	covariance
	)

Definition at line 49 of file unscented.h.

Referenced by g2o::EdgeLabeler::labelEdge(), testMarginals(), and unscentedTest().

                                                                                                                                                                                    {

    const int dim = mean.size();
    const int numPoints = 2 * dim + 1;
    assert (covariance.rows() == covariance.cols() && covariance.cols() == mean.size() && "Dimension Mismatch");
    const double alpha = 1e-3;
    const double beta  = 2.;
    const double lambda = alpha * alpha * dim;
    const double wi = 1./(2. * (dim + lambda) );
    
    sigmaPoints.resize(numPoints);
    sigmaPoints[0] = SigmaPoint<SampleType>(mean, 
              lambda/(dim + lambda), 
              lambda/(dim + lambda) + (1.-alpha*alpha+beta) ); 
    Eigen::LLT<CovarianceType> cholDecomp;
    cholDecomp.compute(covariance*(dim+lambda));
    if (cholDecomp.info()==Eigen::NumericalIssue)
      return false;
    const CovarianceType& L=cholDecomp.matrixL();
    int k=1;
    for (int i=0; i<dim; i++) {
      SampleType s(L.col(i));
      sigmaPoints[k++]=SigmaPoint<SampleType>(mean + s, wi, wi);
      sigmaPoints[k++]=SigmaPoint<SampleType>(mean - s, wi, wi);
    }
    return true;
  }

G2O_CLI_API bool g2o::saveGnuplot	(	const std::string &	gnudump,
		const OptimizableGraph &	optimizer
	)

save the state of the optimizer into files for visualizing using Gnuplot

Definition at line 53 of file output_helper.cpp.

References g2o::HyperGraph::edges(), saveGnuplot(), and g2o::HyperGraph::vertices().

Referenced by main().

{
  HyperGraph::VertexSet vset;
  for (HyperGraph::VertexIDMap::const_iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){
    vset.insert(it->second);
  }
  return saveGnuplot(gnudump, vset, optimizer.edges());
}

G2O_CLI_API bool g2o::saveGnuplot	(	const std::string &	gnudump,
		const HyperGraph::VertexSet &	vertices,
		const HyperGraph::EdgeSet &	edges
	)

Definition at line 62 of file output_helper.cpp.

References __PRETTY_FUNCTION__, g2o::OptimizableGraph::Vertex::dimension(), edgeAllVertsSameDim(), getFileExtension(), getPureFilename(), g2o::WriteGnuplotAction::Parameters::os, and g2o::HyperGraph::Edge::vertices().

Referenced by dumpEdges(), and saveGnuplot().

{
  // seek for an action whose name is writeGnuplot in the library
  HyperGraphElementAction* saveGnuplot = HyperGraphActionLibrary::instance()->actionByName("writeGnuplot");
  if (! saveGnuplot ){
    cerr << __PRETTY_FUNCTION__ << ": no action \"writeGnuplot\" registered" << endl;
    return false;
  }
  WriteGnuplotAction::Parameters params;

  int maxDim = -1;
  int minDim = numeric_limits<int>::max();
  for (HyperGraph::VertexSet::const_iterator it = vertices.begin(); it != vertices.end(); ++it){
    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
    int vdim = v->dimension();
    maxDim = (std::max)(vdim, maxDim);
    minDim = (std::min)(vdim, minDim);
  }

  string extension = getFileExtension(gnudump);
  if (extension.size() == 0)
    extension = "dat";
  string baseFilename = getPureFilename(gnudump);

  // check for odometry edges
  bool hasOdomEdge = false;
  bool hasLandmarkEdge = false;
  for (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) {
    OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
    if (e->vertices().size() == 2) {
      if (edgeAllVertsSameDim(e, maxDim))
        hasOdomEdge = true;
      else
        hasLandmarkEdge = true;
    }
    if (hasOdomEdge && hasLandmarkEdge)
      break;
  }

  bool fileStatus = true;
  if (hasOdomEdge) {
    string odomFilename = baseFilename + "_odom_edges." + extension;
    cerr << "# saving " << odomFilename << " ... ";
    ofstream fout(odomFilename.c_str());
    if (! fout) {
      cerr << "Unable to open file" << endl;
      return false;
    }
    params.os = &fout;

    // writing odometry edges
    for (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) {
      OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
      if (e->vertices().size() != 2 || ! edgeAllVertsSameDim(e, maxDim))
        continue;
      (*saveGnuplot)(e, &params);
    }
    cerr << "done." << endl;
  }

  if (hasLandmarkEdge) {
    string filename = baseFilename + "_landmarks_edges." + extension;
    cerr << "# saving " << filename << " ... ";
    ofstream fout(filename.c_str());
    if (! fout) {
      cerr << "Unable to open file" << endl;
      return false;
    }
    params.os = &fout;

    // writing landmark edges
    for (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) {
      OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
      if (e->vertices().size() != 2 || edgeAllVertsSameDim(e, maxDim))
        continue;
      (*saveGnuplot)(e, &params);
    }
    cerr << "done." << endl;
  }

  if (1) {
    string filename = baseFilename + "_edges." + extension;
    cerr << "# saving " << filename << " ... ";
    ofstream fout(filename.c_str());
    if (! fout) {
      cerr << "Unable to open file" << endl;
      return false;
    }
    params.os = &fout;

    // writing all edges
    for (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) {
      OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
      (*saveGnuplot)(e, &params);
    }
    cerr << "done." << endl;
  }

  if (1) {
    string filename = baseFilename + "_vertices." + extension;
    cerr << "# saving " << filename << " ... ";
    ofstream fout(filename.c_str());
    if (! fout) {
      cerr << "Unable to open file" << endl;
      return false;
    }
    params.os = &fout;

    for (HyperGraph::VertexSet::const_iterator it = vertices.begin(); it != vertices.end(); ++it){
      OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
      (*saveGnuplot)(v, &params);
    }
    cerr << "done." << endl;
  }

  return fileStatus;
}

template<typename T >

int g2o::sign ( T  x )

inline

sign function.

Returns

the sign of x. +1 for x > 0, -1 for x < 0, 0 for x == 0

Definition at line 137 of file misc.h.

{
  if (x > 0)
    return 1;
  else if (x < 0)
    return -1;
  else
    return 0;
}

Matrix3D g2o::skew ( const Vector3D &  v )

inline

Definition at line 28 of file se3_ops.h.

References deltaR(), G2O_TYPES_SLAM3D_API, and project().

Referenced by g2o::SE3Quat::adj(), d_expy_d_y(), g2o::SE3Quat::exp(), g2o::Sim3::log(), g2o::SE3Quat::log(), and g2o::Sim3::Sim3().

              {

  inline G2O_TYPES_SLAM3D_API Matrix3D skew(const Vector3D&v);
  inline G2O_TYPES_SLAM3D_API Vector3D deltaR(const Matrix3D& R);
  inline G2O_TYPES_SLAM3D_API Vector2D project(const Vector3D&);

template<typename T >

T g2o::square ( T  x )

inline

return the square value

Definition at line 52 of file misc.h.

{
  return x*x;
}

void g2o::starsInEdge	(	StarSet &	stars,
		HyperGraph::Edge *	e,
		EdgeStarMap &	esmap,
		HyperGraph::VertexSet &	gauge
	)

Definition at line 75 of file simple_star_ops.cpp.

References starsInVertex(), and g2o::HyperGraph::Edge::vertices().

Referenced by computeBorder().

                                                                                                   {
  for (size_t i=0; i<e->vertices().size(); i++){
    OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)e->vertices()[i];
    if (gauge.find(v)==gauge.end())
      starsInVertex(stars, v, esmap);
  }
}

void g2o::starsInVertex	(	StarSet &	stars,
		HyperGraph::Vertex *	v,
		EdgeStarMap &	esmap
	)

Definition at line 66 of file simple_star_ops.cpp.

References g2o::HyperGraph::Vertex::edges().

Referenced by starsInEdge().

                                                                           {
  for (HyperGraph::EdgeSet::iterator it=v->edges().begin(); it!=v->edges().end(); it++){
    HyperGraph::Edge* e=*it;
    EdgeStarMap::iterator eit=esmap.find(e);
    if (eit!=esmap.end())
      stars.insert(eit->second);
  }
}

G2O_STUFF_API double g2o::tictoc

(

const char *

algorithmPart

)

Profile the timing of certain parts of your algorithm.

Profile the timing of certain parts of your algorithm. A typical use-case is as follows:

tictoc("doSomething"); // place the code here. tictoc("doSomething");

This will calculate statistics for the operations within the two calls to tictoc()

If the environment variable G2O_ENABLE_TICTOC is defined, the timing will be performed.

Definition at line 115 of file tictoc.cpp.

References g2o::TicTocElement::algorithmPart, g2o::TicTocInitializer::enabled, get_monotonic_time(), g2o::TicTocElement::ticTime, and g2o::TicTocInitializer::tictocElements.

Referenced by ADD_LIBRARY(), main(), g2o::ScopedTictoc::ScopedTictoc(), and g2o::ScopedTictoc::~ScopedTictoc().

  {
    static TicTocInitializer initializer;
    if (! initializer.enabled)
      return 0.;

    TicTocMap& tictocElements = initializer.tictocElements;
    static double alpha = 0.01;
    double now = get_monotonic_time();

    double dt = 0.;
    TicTocMap::iterator foundIt = tictocElements.find(algorithmPart);
    if (foundIt == tictocElements.end()) {
      // insert element
      TicTocElement e;
      e.ticTime = now;
      e.algorithmPart = algorithmPart;
      tictocElements[e.algorithmPart] = e;
    } else {
      if (foundIt->second.clockIsRunning) {
        dt = now - foundIt->second.ticTime;
        foundIt->second.totalTime += dt;
        foundIt->second.minTime = std::min(foundIt->second.minTime, dt);
        foundIt->second.maxTime = std::max(foundIt->second.maxTime, dt);
        if (foundIt->second.numCalls == 0)
          foundIt->second.exponentialMovingAverage = dt;
        else
          foundIt->second.exponentialMovingAverage = (1. - alpha) * foundIt->second.exponentialMovingAverage + alpha*dt;
        foundIt->second.numCalls++;
      } else {
        foundIt->second.ticTime = now;
      }
      foundIt->second.clockIsRunning = !foundIt->second.clockIsRunning;
    }
    return dt;
  }

Vector3D g2o::unproject ( const Vector2D &  v )

inline

Definition at line 67 of file se3_ops.h.

Vector4D g2o::unproject ( const Vector3D &  v )

inline

Definition at line 76 of file se3_ops.h.

Vector3D g2o::unproject2d

(

const Vector2D &

v

)

Definition at line 58 of file types_six_dof_expmap.cpp.

Referenced by invert_depth().

                                         {
  Vector3D res;
  res(0) = v(0);
  res(1) = v(1);
  res(2) = 1;
  return res;
}

size_t g2o::vertexEdgesInStar	(	HyperGraph::EdgeSet &	eset,
		HyperGraph::Vertex *	v,
		Star *	s,
		EdgeStarMap &	esmap
	)

Definition at line 55 of file simple_star_ops.cpp.

References g2o::HyperGraph::Vertex::edges().

Referenced by assignHierarchicalEdges().

                                                                                                   {
  eset.clear();
  for (HyperGraph::EdgeSet::iterator it=v->edges().begin(); it!=v->edges().end(); it++){
    HyperGraph::Edge* e=*it;
    EdgeStarMap::iterator eit=esmap.find(e);
    if (eit!=esmap.end() && eit->second == s)
      eset.insert(e);
  }
  return eset.size();
}

template<typename T >

T g2o::wrap ( T  l, T  x, T  u  )

inline

wrap x to be in the interval [l, u]

Definition at line 164 of file misc.h.

{
  T intervalWidth = u - l;
  while (x < l)
    x += intervalWidth;
  while (x > u)
    x -= intervalWidth;
  return x;
}

G2O_STUFF_API bool g2o::writeCCSMatrix	(	const std::string &	filename,
		int	rows,
		int	cols,
		const int *	p,
		const int *	i,
		const double *	v,
		bool	upperTriangleSymmetric = true
	)

write a CCS matrix given by pointer to column, row, and values

bool g2o::writeCCSMatrix	(	const string &	filename,
		int	rows,
		int	cols,
		const int *	Ap,
		const int *	Ai,
		const double *	Ax,
		bool	upperTriangleSymmetric
	)

Definition at line 64 of file sparse_helper.cpp.

Referenced by g2o::LinearSolverCholmodOnline< MatrixType >::choleskyUpdate(), g2o::LinearSolverCholmod< MatrixType >::saveMatrix(), and g2o::LinearSolverCholmodOnline< MatrixType >::solve().

  {
    vector<TripletEntry> entries;
    entries.reserve((size_t)Ap[cols]);
    for (int i=0; i < cols; i++) {
      const int& rbeg = Ap[i];
      const int& rend = Ap[i+1];
      for (int j = rbeg; j < rend; j++) {
        entries.push_back(TripletEntry(Ai[j], i, Ax[j]));
        if (upperTriangleSymmetric && Ai[j] != i)
          entries.push_back(TripletEntry(i, Ai[j], Ax[j]));
      }
    }
    sort(entries.begin(), entries.end(), TripletColSort());

    string name = filename;
    std::string::size_type lastDot = name.find_last_of('.');
    if (lastDot != std::string::npos) 
      name = name.substr(0, lastDot);

    std::ofstream fout(filename.c_str());
    fout << "# name: " << name << std::endl;
    fout << "# type: sparse matrix" << std::endl;
    fout << "# nnz: " << entries.size() << std::endl;
    fout << "# rows: " << rows << std::endl;
    fout << "# columns: " << cols << std::endl;
    //fout << fixed;
    fout << setprecision(9) << endl;
    for (vector<TripletEntry>::const_iterator it = entries.begin(); it != entries.end(); ++it) {
      const TripletEntry& entry = *it;
      fout << entry.r+1 << " " << entry.c+1 << " " << entry.x << std::endl;
    }
    return fout.good();
  }

G2O_STUFF_API bool g2o::writeVector	(	const std::string &	filename,
		const double *	v,
		int	n
	)

write an array to a file, debugging

bool g2o::writeVector	(	const string &	filename,
		const double *	v,
		int	n
	)

Definition at line 55 of file sparse_helper.cpp.

  {
    ofstream os(filename.c_str());
    os << fixed;
    for (int i=0; i<n; i++)
      os << *v++ << endl;
    return os.good();
  }

Variable Documentation

std::mt19937 g2o::_gen_real

static

Definition at line 33 of file sampler.cpp.

std::uniform_real_distribution<double> g2o::_uniformReal

static

Definition at line 32 of file sampler.cpp.

Referenced by sampleUniform().

const double g2o::INFORMATION_SCALING_ODOMETRY = 100

static

Definition at line 50 of file sclam_helpers.cpp.

Referenced by addOdometryCalibLinksDifferential().

class G2O_CORE_API g2o::OptimizationAlgorithm

Definition at line 46 of file optimization_algorithm_factory.h.

class G2O_CORE_API g2o::SparseOptimizer

Definition at line 47 of file optimization_algorithm_factory.h.

Referenced by main(), and g2o::RunG2OViewer::run().