#include <sba/spa2d.h>#include <stdio.h>#include <iostream>#include "src/Core/util/DisableMSVCWarnings.h"#include "src/Core/util/Macros.h"#include <cerrno>#include <cstdlib>#include <cmath>#include <complex>#include <cassert>#include <functional>#include <iosfwd>#include <cstring>#include <string>#include <limits>#include <climits>#include <algorithm>#include "src/Core/util/Constants.h"#include "src/Core/util/ForwardDeclarations.h"#include "src/Core/util/Meta.h"#include "src/Core/util/XprHelper.h"#include "src/Core/util/StaticAssert.h"#include "src/Core/util/Memory.h"#include "src/Core/NumTraits.h"#include "src/Core/MathFunctions.h"#include "src/Core/GenericPacketMath.h"#include "src/Core/arch/Default/Settings.h"#include "src/Core/Functors.h"#include "src/Core/DenseCoeffsBase.h"#include "src/Core/DenseBase.h"#include "src/Core/MatrixBase.h"#include "src/Core/EigenBase.h"#include "src/Core/Assign.h"#include "src/Core/util/BlasUtil.h"#include "src/Core/DenseStorage.h"#include "src/Core/NestByValue.h"#include "src/Core/ForceAlignedAccess.h"#include "src/Core/ReturnByValue.h"#include "src/Core/NoAlias.h"#include "src/Core/PlainObjectBase.h"#include "src/Core/Matrix.h"#include "src/Core/Array.h"#include "src/Core/CwiseBinaryOp.h"#include "src/Core/CwiseUnaryOp.h"#include "src/Core/CwiseNullaryOp.h"#include "src/Core/CwiseUnaryView.h"#include "src/Core/SelfCwiseBinaryOp.h"#include "src/Core/Dot.h"#include "src/Core/StableNorm.h"#include "src/Core/MapBase.h"#include "src/Core/Stride.h"#include "src/Core/Map.h"#include "src/Core/Block.h"#include "src/Core/VectorBlock.h"#include "src/Core/Transpose.h"#include "src/Core/DiagonalMatrix.h"#include "src/Core/Diagonal.h"#include "src/Core/DiagonalProduct.h"#include "src/Core/PermutationMatrix.h"#include "src/Core/Transpositions.h"#include "src/Core/Redux.h"#include "src/Core/Visitor.h"#include "src/Core/Fuzzy.h"#include "src/Core/IO.h"#include "src/Core/Swap.h"#include "src/Core/CommaInitializer.h"#include "src/Core/Flagged.h"#include "src/Core/ProductBase.h"#include "src/Core/Product.h"#include "src/Core/TriangularMatrix.h"#include "src/Core/SelfAdjointView.h"#include "src/Core/SolveTriangular.h"#include "src/Core/products/Parallelizer.h"#include "src/Core/products/CoeffBasedProduct.h"#include "src/Core/products/GeneralBlockPanelKernel.h"#include "src/Core/products/GeneralMatrixVector.h"#include "src/Core/products/GeneralMatrixMatrix.h"#include "src/Core/products/GeneralMatrixMatrixTriangular.h"#include "src/Core/products/SelfadjointMatrixVector.h"#include "src/Core/products/SelfadjointMatrixMatrix.h"#include "src/Core/products/SelfadjointProduct.h"#include "src/Core/products/SelfadjointRank2Update.h"#include "src/Core/products/TriangularMatrixVector.h"#include "src/Core/products/TriangularMatrixMatrix.h"#include "src/Core/products/TriangularSolverMatrix.h"#include "src/Core/products/TriangularSolverVector.h"#include "src/Core/BandMatrix.h"#include "src/Core/BooleanRedux.h"#include "src/Core/Select.h"#include "src/Core/VectorwiseOp.h"#include "src/Core/Random.h"#include "src/Core/Replicate.h"#include "src/Core/Reverse.h"#include "src/Core/ArrayBase.h"#include "src/Core/ArrayWrapper.h"#include "src/Core/GlobalFunctions.h"#include "src/Core/util/EnableMSVCWarnings.h"#include "Core"#include "SVD"#include "LU"#include "src/Geometry/OrthoMethods.h"#include "src/Geometry/EulerAngles.h"#include "src/Geometry/Homogeneous.h"#include "src/Geometry/RotationBase.h"#include "src/Geometry/Rotation2D.h"#include "src/Geometry/Quaternion.h"#include "src/Geometry/AngleAxis.h"#include "src/Geometry/Transform.h"#include "src/Geometry/Translation.h"#include "src/Geometry/Scaling.h"#include "src/Geometry/Hyperplane.h"#include "src/Geometry/ParametrizedLine.h"#include "src/Geometry/AlignedBox.h"#include "src/Geometry/Umeyama.h"#include "src/misc/Solve.h"#include "src/misc/Kernel.h"#include "src/misc/Image.h"#include "src/LU/FullPivLU.h"#include "src/LU/PartialPivLU.h"#include "src/LU/Determinant.h"#include "src/LU/Inverse.h"#include <vector>#include "src/StlSupport/StdVector.h"#include <Eigen/Core>#include <Eigen/Geometry>#include <Eigen/LU>#include <Eigen/StdVector>#include <stdlib.h>#include <limits.h>#include <math.h>#include <map>#include <bpcg/bpcg.h>#include <pose_graph/constraint_graph.h>#include <geometry_msgs/Pose.h>#include <boost/shared_ptr.hpp>
Go to the source code of this file.
Classes | |
| struct | pose_graph::Spa2DConversionResult |
Namespaces | |
| namespace | pose_graph |
Typedefs | |
| typedef std::map< unsigned, unsigned > | pose_graph::NodeIndexMap |
| typedef boost::shared_ptr < sba::SysSPA2d > | pose_graph::Spa2DPtr |
Functions | |
| tf::Pose | pose_graph::getNodePose (const sba::Node2d &n) |
| Return the Pose corresponding to a spa node. | |
| NodePoseMap | pose_graph::optimizeGraph2D (const ConstraintGraph &g, const NodePoseMap &init, const NodeSet &nodes) |
| Optimize a component of a graph using SPA 2d. | |
| NodePoseMap | pose_graph::optimizeGraph2D (const ConstraintGraph &g, const NodePoseMap &init) |
| Optimize a graph using SPA 2d init Initial estimates. | |
Code to convert between PoseGraph to sba::SysSPA2D
Definition in file spa_2d_conversion.h.