Classes | |
| struct | DisconnectedNodesException |
| Exception when expected two nodes to be connected and they aren't. More... | |
| struct | DuplicateEdgeIdException |
| Exception for trying to add an EdgeId that already exists. More... | |
| struct | DuplicateNodeIdException |
| Exception for trying to add a NodeId that already exists. More... | |
| class | EdgeId |
| Used for referring to edges in a graph. Basically an unsigned long int, but typechecked. More... | |
| struct | EdgeInfo |
| struct | FileFormatException |
| Exception for incorrect format of saved graph. More... | |
| struct | FileOpenException |
| Exception for failing to open a file. More... | |
| struct | GraphOptimizationException |
| Exception when SPA optimization fails. More... | |
| class | Id |
| class | NodeId |
| Used for referring to nodes in a graph. Basically an unsigned long int, but typechecked. More... | |
| struct | NodeInfo |
| struct | NonexistentParamException |
| Exception for not finding a ros parameter. More... | |
| struct | PoseConstraint |
| A single pose constraint (including precision matrix) between two nodes. More... | |
| class | PoseGraph |
| class | PoseGraphException |
| A base class for all pose_graph exceptions; provides a handy boost::format parent constructor. More... | |
| class | PoseGraphImpl |
| struct | ShortestPathResult |
| Result of a shortest path query. More... | |
| struct | Spa2DConversionResult |
| struct | SpaConversionResult |
| struct | UnknownEdgeIdException |
| Exception for unknown EdgeId. More... | |
| struct | UnknownNodeIdException |
| Exception for unknown NodeId. More... | |
| struct | UnknownTopicException |
| Exception for a topic that hasn't been registered with a pose graph. More... | |
Typedefs | |
| typedef Eigen3::aligned_allocator < pair< EdgeId, PoseConstraint > > | Allocator |
| typedef set< EdgeId > | EdgeIdSet |
| typedef pair< NodeId, NodeId > | IncidentNodes |
| typedef PoseGraphAdjacencyList::in_edge_iterator | InEdgeIter |
| typedef set< NodeId > | NodeIdSet |
| typedef std::map< NodeId, unsigned > | NodeIndexMap |
| typedef std::pair< NodeId, NodeId > | NodePair |
| typedef std::map< NodeId, geometry_msgs::Pose > | NodePoseMap |
| Maps Node ids to poses. | |
| typedef PoseGraphAdjacencyList::out_edge_iterator | OutEdgeIter |
| typedef map< EdgeId, PoseConstraint, std::less < EdgeId >, Allocator > | PoseConstraintMap |
| typedef adjacency_list < multisetS, listS, undirectedS, NodeInfo, EdgeInfo > | PoseGraphAdjacencyList |
| typedef graph_traits < PoseGraphAdjacencyList > ::edge_descriptor | PoseGraphEdge |
| typedef graph_traits < PoseGraphAdjacencyList > ::vertex_descriptor | PoseGraphVertex |
| typedef Eigen3::Matrix< double, 6, 6 > | PrecisionMatrix |
| typedef boost::shared_ptr < sba::SysSPA2d > | Spa2DPtr |
| typedef boost::shared_ptr < sba::SysSPA > | SpaPtr |
| typedef PoseGraphAdjacencyList::vertex_iterator | VertexIter |
Functions | |
| Pose | addRelativePose (const PoseGraphImpl *g, const Pose &pose, const EdgeId e) |
| void | applyDiff (PoseGraph *g, const PoseGraphDiff &m) |
| Apply a diff to a pose graph. | |
| Pose | applyTransform (const Affine3d &trans, const Pose &pose) |
| Apply an Eigen transformation to a ROS pose. | |
| PoseGraphDiff | composeDiffs (const std::vector< PoseGraphDiff::ConstPtr > &diffs) |
| Compose diffs: new diff formed by applying d1 then d2. | |
| PoseGraphDiff | composeDiffs (const vector< PoseGraphDiff::ConstPtr > &diffs) |
| geometry_msgs::Point | computeBarycenter (const sensor_msgs::LaserScan &scan) |
| gm::Point | computeBarycenter (const sm::LaserScan &scan) |
| template<class T > | |
| std::string | concatenate (const std::string &s, const T &x) |
| PoseConstraint | constraintFromRos (const PoseWithPrecision &m) |
| Helper for converting a ros message to a PoseConstraint. | |
| PoseWithPrecision | constraintToRos (const PoseConstraint &constraint) |
| Helper for converting an individual constraint to a ros message. | |
| template<class Key , class Container > | |
| bool | contains (const Container &container, const Key &key) |
| geometry_msgs::Point | convertToPoint (const geometry_msgs::Point32 &p) |
| Point | convertToPoint (const Point32 &p) |
| Pose | convertToPose (const Pose2D &pose2d) |
| Convert to Pose. | |
| Pose | convertToPose (const Affine3d trans) |
| Convert transform to pose. | |
| const string | DUMMY_TOPIC ("dummy") |
| EdgeId | edgeId (const PoseGraphAdjacencyList &graph, const PoseGraphEdge &e) |
| double | euclideanDistance (const geometry_msgs::Point &p1, const geometry_msgs::Point &p2) |
| double | euclideanDistance (const geometry_msgs::Pose2D &p1, const geometry_msgs::Pose2D &p2, const double rad_to_m=1.0) |
| double | euclideanDistance (const Point &p1, const Point &p2) |
| double | euclideanDistance (const gm::Pose2D &p1, const gm::Pose2D &p2, const double mult) |
| template<class K , class V > | |
| K | getKey (const pair< K, V > entry) |
| nav_msgs::MapMetaData | getMapInfo (const Pose &pose, const double size, const double resolution) |
| Return dimensions of a map centered at pose of the given size (meters) and resolution. | |
| nm::MapMetaData | getMapInfo (const gm::Pose &pose, const double size, const double resolution) |
| Return dimensions of a map centered at pose of the given size (meters) and resolution. | |
| geometry_msgs::Pose | getNodePose (const sba::Node &n) |
| Return the Pose corresponding to a spa node. | |
| geometry_msgs::Pose | getNodePose (const sba::Node2d &n) |
| Return the Pose corresponding to a spa node. | |
| geometry_msgs::Pose | getPose (const PoseConstraint &c) |
| Given a pose constraint, return the expected relative pose. | |
| double | getYaw (const Quaterniond &q) |
| template<class K , class V , class C , class A > | |
| const V & | keyValue (const map< K, V, C, A > &m, const K &key) |
| double | length (const Eigen3::Vector3d &v) |
| Norm of a vector. | |
| double | length (const PoseConstraint &constraint) |
| Norm of mean translation part of a Pose constraint. | |
| double | length (const geometry_msgs::Point &p) |
| double | length (const Vector3d &v) |
| double | length (const gm::Point &p) |
| PoseConstraint | makeConstraint (const Eigen3::Affine3d &trans, const Eigen3::Matrix< double, 6, 6 > &prec) |
| PoseConstraint | makeConstraint (const Affine3d &trans, const Eigen3::Matrix< double, 6, 6 > &prec) |
| Node | makeNode (const Pose &initial_pose) |
| Node2d | makeNode (const double x, const double y, const double theta) |
| Pose2D | makePose2D (double x, double y, double theta=0.0) |
| Construct a pose2d. | |
| PrecisionMatrix | makePrecisionMatrix (const double x_prec, const double y_prec, const double theta_prec) |
| Given precisions of x, y, and theta (yaw), construct a 6x6 diagonal precision matrix Precisions of unspecified components (z, roll, pitch) are set to 1. Theta precision is converted to precision of z component of quaternion using a crude approximation. | |
| PrecisionMatrix | makePrecisionMatrix (const double x_prec, const double y_prec, const double xy_prec, const double theta_prec) |
| Given precisions of x, y, and theta (yaw), as well as xy precision, construct a 6x6 diagonal precision matrix Precisions of unspecified components (z, roll, pitch) are set to 1. Theta precision is converted to precision of z component of quaternion using a crude approximation. | |
| Con2dP2 | makeSpa2DConstraint (const PoseConstraint &constraint) |
| ConP2 | makeSpaConstraint (const PoseConstraint &constraint) |
| ostream & | operator<< (ostream &str, const PoseConstraint &constraint) |
| const std::string | OPTIMIZED_FRAME ("graph_optimization") |
| Implicit frame for graph optimization. | |
| NodePoseMap | optimizeGraph (const PoseGraph &g) |
| Optimize a graph using SPA. | |
| NodePoseMap | optimizeGraph2D (const PoseGraph &g) |
| Optimize a graph using SPA 2d. | |
| Pose | perturbPose (const Pose &pose, double r) |
| Perturb x, y components of pose by uniform noise between -r and r. | |
| Point | pointInReferenceFrame (const Point &p, const Pose &pose) |
| Given point coordinates w.r.t a pose, return its coordinates in reference frame. | |
| void | poseEigenToMsg (const Eigen3::Affine3d &e, geometry_msgs::Pose &m) |
| PoseGraph | poseGraphFromRos (const PoseGraphMessage &g) |
| Converts a PoseGraphMessage into a PoseGraph. | |
| PoseGraphMessage | poseGraphToRos (const PoseGraph &g) |
| Converts a PoseGraph into a PoseGraphMessage. | |
| SpaConversionResult | poseGraphToSpa (const PoseGraph &pose_graph) |
| Convert a pose graph into a SysSPA object and return it. | |
| Spa2DConversionResult | poseGraphToSpa2D (const PoseGraph &pose_graph) |
| Convert a pose graph into a SysSPA object and return it. | |
| Affine3d | poseToWorldTransform (const Pose &pose) |
| Return transformation from pose frame to world frame. | |
| Pose2D | projectToPose2D (const Pose &pose) |
| Project Pose to Pose2D. | |
| Eigen3::Vector4d | quaternionMsgToVector4d (const geometry_msgs::Quaternion &m) |
| Convert ROS quaternion to Eigen Vector4d in x,y,z,w order. | |
| PoseGraph | readFromFile (const string &filename) |
| Reads a PoseGraph from a file. | |
| Pose | relativePose (const Pose &pose1, const Pose &pose2) |
| Affine3d | relativeTransform (const Pose &pose1, const Pose &pose2) |
| Return transformation from frame of pose1 to frame of pose2. | |
| Affine3d | relativeTransform (const Pose2D &pose1, const Pose2D &pose2) |
| Return transformation from frame of pose1 to frame of pose2. | |
| string | toString (const gm::Pose2D &pose) |
| string | toString (const gm::Point &p) |
| string | toString (const Eigen3::Affine3d &trans) |
| string | toString (const PoseConstraint &constraint) |
| string | toString (const gm::Pose &pose) |
| template<class T > | |
| std::string | toString (const std::set< T > &set) |
| template<class T > | |
| std::string | toString (const std::vector< T > &v) |
| std::string | toString (const PrecisionMatrix &m) |
| string | toString2D (const gm::Pose &pose) |
| string | toString2D (const Eigen3::Affine3d &trans) |
| Affine3d | transformBetween (const Pose &pose1, const Pose &pose2) |
| nav_msgs::OccupancyGrid | transformGrid (const Affine3d &trans, const nav_msgs::OccupancyGrid &g) |
| Transform an occupancy grid (just transforms the origin) | |
| OccupancyGrid | transformGrid (const Affine3d &trans, const OccupancyGrid &g) |
| Point32 | transformPoint (const Affine3d &trans, const Point32 &pt) |
| Apply an Eigen transformation to a ROS point. | |
| Point | transformPoint (const Affine3d &trans, const Point &pt) |
| Apply an Eigen transformation to a ROS point. | |
| Eigen3::Vector4d | translationToVector4d (const geometry_msgs::Point &p) |
| Convert a ROS point to an Eigen 4d vector with 1 at the end. | |
| NodeId | vertexId (const PoseGraphAdjacencyList &graph, const PoseGraphVertex &v) |
| void | writeToFile (const PoseGraph &g, const string &filename) |
| Writes a PoseGraph to a file. | |
| typedef Eigen3::aligned_allocator<pair<EdgeId, PoseConstraint> > pose_graph::Allocator |
Definition at line 95 of file pose_graph_impl.h.
| typedef set<EdgeId> pose_graph::EdgeIdSet |
Definition at line 64 of file pose_graph.cpp.
| typedef pair<NodeId, NodeId> pose_graph::IncidentNodes |
Definition at line 69 of file pose_graph.cpp.
| typedef PoseGraphAdjacencyList::in_edge_iterator pose_graph::InEdgeIter |
Definition at line 67 of file pose_graph.cpp.
| typedef set<NodeId> pose_graph::NodeIdSet |
Definition at line 65 of file pose_graph.cpp.
| typedef std::map< NodeId, unsigned > pose_graph::NodeIndexMap |
Definition at line 56 of file spa_2d_conversion.h.
| typedef std::pair<NodeId, NodeId> pose_graph::NodePair |
Definition at line 21 of file spa_2d_conversion.cpp.
| typedef std::map<NodeId, geometry_msgs::Pose> pose_graph::NodePoseMap |
Maps Node ids to poses.
Definition at line 76 of file pose_graph.h.
| typedef PoseGraphAdjacencyList::out_edge_iterator pose_graph::OutEdgeIter |
Definition at line 66 of file pose_graph.cpp.
| typedef map<EdgeId, PoseConstraint, std::less<EdgeId>, Allocator> pose_graph::PoseConstraintMap |
Definition at line 96 of file pose_graph_impl.h.
| typedef adjacency_list<multisetS, listS, undirectedS, NodeInfo, EdgeInfo> pose_graph::PoseGraphAdjacencyList |
Definition at line 100 of file pose_graph_impl.h.
| typedef graph_traits<PoseGraphAdjacencyList>::edge_descriptor pose_graph::PoseGraphEdge |
Definition at line 101 of file pose_graph_impl.h.
| typedef graph_traits<PoseGraphAdjacencyList>::vertex_descriptor pose_graph::PoseGraphVertex |
Definition at line 102 of file pose_graph_impl.h.
| typedef Eigen3::Matrix<double, 6, 6> pose_graph::PrecisionMatrix |
Definition at line 54 of file geometry.h.
| typedef boost::shared_ptr<sba::SysSPA2d> pose_graph::Spa2DPtr |
Definition at line 55 of file spa_2d_conversion.h.
| typedef boost::shared_ptr<sba::SysSPA> pose_graph::SpaPtr |
Definition at line 51 of file spa_conversion.h.
| typedef PoseGraphAdjacencyList::vertex_iterator pose_graph::VertexIter |
Definition at line 68 of file pose_graph.cpp.
| Pose pose_graph::addRelativePose | ( | const PoseGraphImpl * | g, |
| const Pose & | pose, | ||
| const EdgeId | e | ||
| ) |
Definition at line 354 of file pose_graph.cpp.
| void pose_graph::applyDiff | ( | PoseGraph * | g, |
| const PoseGraphDiff & | m | ||
| ) |
Apply a diff to a pose graph.
Definition at line 168 of file pose_graph_message.cpp.
| Pose pose_graph::applyTransform | ( | const Affine3d & | trans, |
| const Pose & | pose | ||
| ) |
Apply an Eigen transformation to a ROS pose.
Definition at line 88 of file transforms.cpp.
| PoseGraphDiff pose_graph::composeDiffs | ( | const std::vector< PoseGraphDiff::ConstPtr > & | diffs | ) |
Compose diffs: new diff formed by applying d1 then d2.
| PoseGraphDiff pose_graph::composeDiffs | ( | const vector< PoseGraphDiff::ConstPtr > & | diffs | ) |
Definition at line 187 of file pose_graph_message.cpp.
| geometry_msgs::Point pose_graph::computeBarycenter | ( | const sensor_msgs::LaserScan & | scan | ) |
| gm::Point pose_graph::computeBarycenter | ( | const sm::LaserScan & | scan | ) |
Sum up the hit points
Definition at line 143 of file geometry.cpp.
| std::string pose_graph::concatenate | ( | const std::string & | s, |
| const T & | x | ||
| ) |
| PoseConstraint pose_graph::constraintFromRos | ( | const PoseWithPrecision & | m | ) |
Helper for converting a ros message to a PoseConstraint.
Definition at line 125 of file pose_graph_message.cpp.
| PoseWithPrecision pose_graph::constraintToRos | ( | const PoseConstraint & | constraint | ) |
Helper for converting an individual constraint to a ros message.
Definition at line 70 of file pose_graph_message.cpp.
| bool pose_graph::contains | ( | const Container & | container, |
| const Key & | key | ||
| ) |
| geometry_msgs::Point pose_graph::convertToPoint | ( | const geometry_msgs::Point32 & | p | ) |
| Point pose_graph::convertToPoint | ( | const Point32 & | p | ) |
Definition at line 133 of file geometry.cpp.
| Pose pose_graph::convertToPose | ( | const Pose2D & | pose2d | ) |
Convert to Pose.
Definition at line 189 of file transforms.cpp.
| Pose pose_graph::convertToPose | ( | const Affine3d | trans | ) |
Convert transform to pose.
Definition at line 199 of file transforms.cpp.
| const string pose_graph::DUMMY_TOPIC | ( | "dummy" | ) |
| EdgeId pose_graph::edgeId | ( | const PoseGraphAdjacencyList & | graph, |
| const PoseGraphEdge & | e | ||
| ) |
Definition at line 111 of file pose_graph.cpp.
| double pose_graph::euclideanDistance | ( | const geometry_msgs::Point & | p1, |
| const geometry_msgs::Point & | p2 | ||
| ) |
| double pose_graph::euclideanDistance | ( | const geometry_msgs::Pose2D & | p1, |
| const geometry_msgs::Pose2D & | p2, | ||
| const double | rad_to_m = 1.0 |
||
| ) |
| double pose_graph::euclideanDistance | ( | const Point & | p1, |
| const Point & | p2 | ||
| ) |
Definition at line 127 of file geometry.cpp.
| double pose_graph::euclideanDistance | ( | const gm::Pose2D & | p1, |
| const gm::Pose2D & | p2, | ||
| const double | mult | ||
| ) |
Definition at line 168 of file geometry.cpp.
| K pose_graph::getKey | ( | const pair< K, V > | entry | ) |
Definition at line 119 of file pose_graph.cpp.
| nav_msgs::MapMetaData pose_graph::getMapInfo | ( | const Pose & | pose, |
| const double | size, | ||
| const double | resolution | ||
| ) |
Return dimensions of a map centered at pose of the given size (meters) and resolution.
| nm::MapMetaData pose_graph::getMapInfo | ( | const gm::Pose & | pose, |
| const double | size, | ||
| const double | resolution | ||
| ) |
Return dimensions of a map centered at pose of the given size (meters) and resolution.
Definition at line 225 of file transforms.cpp.
| Pose pose_graph::getNodePose | ( | const sba::Node & | n | ) |
Return the Pose corresponding to a spa node.
Definition at line 81 of file spa_conversion.cpp.
| Pose pose_graph::getNodePose | ( | const sba::Node2d & | n | ) |
Return the Pose corresponding to a spa node.
Definition at line 105 of file spa_2d_conversion.cpp.
| Pose pose_graph::getPose | ( | const PoseConstraint & | c | ) |
Given a pose constraint, return the expected relative pose.
Definition at line 104 of file geometry.cpp.
| double pose_graph::getYaw | ( | const Quaterniond & | q | ) |
Definition at line 38 of file spa_2d_conversion.cpp.
| const V& pose_graph::keyValue | ( | const map< K, V, C, A > & | m, |
| const K & | key | ||
| ) |
| double pose_graph::length | ( | const Eigen3::Vector3d & | v | ) |
Norm of a vector.
| double pose_graph::length | ( | const PoseConstraint & | constraint | ) |
Norm of mean translation part of a Pose constraint.
Definition at line 93 of file geometry.cpp.
| double pose_graph::length | ( | const geometry_msgs::Point & | p | ) |
| double pose_graph::length | ( | const Vector3d & | v | ) |
Definition at line 87 of file geometry.cpp.
| double pose_graph::length | ( | const gm::Point & | p | ) |
Definition at line 98 of file geometry.cpp.
| PoseConstraint pose_graph::makeConstraint | ( | const Eigen3::Affine3d & | trans, |
| const Eigen3::Matrix< double, 6, 6 > & | prec | ||
| ) |
| PoseConstraint pose_graph::makeConstraint | ( | const Affine3d & | trans, |
| const Eigen3::Matrix< double, 6, 6 > & | prec | ||
| ) |
Definition at line 121 of file geometry.cpp.
| Node pose_graph::makeNode | ( | const Pose & | initial_pose | ) |
Definition at line 20 of file spa_conversion.cpp.
| Node2d pose_graph::makeNode | ( | const double | x, |
| const double | y, | ||
| const double | theta | ||
| ) |
Definition at line 24 of file spa_2d_conversion.cpp.
| Pose2D pose_graph::makePose2D | ( | double | x, |
| double | y, | ||
| double | theta = 0.0 |
||
| ) |
Construct a pose2d.
Definition at line 214 of file transforms.cpp.
| PrecisionMatrix pose_graph::makePrecisionMatrix | ( | const double | x_prec, |
| const double | y_prec, | ||
| const double | theta_prec | ||
| ) |
Given precisions of x, y, and theta (yaw), construct a 6x6 diagonal precision matrix Precisions of unspecified components (z, roll, pitch) are set to 1. Theta precision is converted to precision of z component of quaternion using a crude approximation.
Definition at line 180 of file geometry.cpp.
| PrecisionMatrix pose_graph::makePrecisionMatrix | ( | const double | x_prec, |
| const double | y_prec, | ||
| const double | xy_prec, | ||
| const double | theta_prec | ||
| ) |
Given precisions of x, y, and theta (yaw), as well as xy precision, construct a 6x6 diagonal precision matrix Precisions of unspecified components (z, roll, pitch) are set to 1. Theta precision is converted to precision of z component of quaternion using a crude approximation.
Definition at line 189 of file geometry.cpp.
| Con2dP2 pose_graph::makeSpa2DConstraint | ( | const PoseConstraint & | constraint | ) |
Definition at line 44 of file spa_2d_conversion.cpp.
| ConP2 pose_graph::makeSpaConstraint | ( | const PoseConstraint & | constraint | ) |
Definition at line 34 of file spa_conversion.cpp.
| ostream & pose_graph::operator<< | ( | ostream & | str, |
| const PoseConstraint & | constraint | ||
| ) |
Definition at line 76 of file geometry.cpp.
| const std::string pose_graph::OPTIMIZED_FRAME | ( | "graph_optimization" | ) |
Implicit frame for graph optimization.
| NodePoseMap pose_graph::optimizeGraph | ( | const PoseGraph & | g | ) |
Optimize a graph using SPA.
| map | from node id to optimized pose |
Definition at line 101 of file spa_conversion.cpp.
| NodePoseMap pose_graph::optimizeGraph2D | ( | const PoseGraph & | g | ) |
Optimize a graph using SPA 2d.
| map | from node id to optimized pose |
Definition at line 120 of file spa_2d_conversion.cpp.
| Pose pose_graph::perturbPose | ( | const Pose & | pose, |
| double | r | ||
| ) |
Perturb x, y components of pose by uniform noise between -r and r.
Definition at line 130 of file transforms.cpp.
| Point pose_graph::pointInReferenceFrame | ( | const Point & | p, |
| const Pose & | pose | ||
| ) |
Given point coordinates w.r.t a pose, return its coordinates in reference frame.
Definition at line 145 of file transforms.cpp.
| void pose_graph::poseEigenToMsg | ( | const Eigen3::Affine3d & | e, |
| geometry_msgs::Pose & | m | ||
| ) | [inline] |
| PoseGraph pose_graph::poseGraphFromRos | ( | const PoseGraphMessage & | g | ) |
Converts a PoseGraphMessage into a PoseGraph.
Definition at line 139 of file pose_graph_message.cpp.
| PoseGraphMessage pose_graph::poseGraphToRos | ( | const PoseGraph & | g | ) |
Converts a PoseGraph into a PoseGraphMessage.
Definition at line 92 of file pose_graph_message.cpp.
| SpaConversionResult pose_graph::poseGraphToSpa | ( | const PoseGraph & | pose_graph | ) |
Convert a pose graph into a SysSPA object and return it.
Definition at line 48 of file spa_conversion.cpp.
| Spa2DConversionResult pose_graph::poseGraphToSpa2D | ( | const PoseGraph & | pose_graph | ) |
Convert a pose graph into a SysSPA object and return it.
Definition at line 62 of file spa_2d_conversion.cpp.
| Affine3d pose_graph::poseToWorldTransform | ( | const Pose & | pose | ) |
Return transformation from pose frame to world frame.
Definition at line 63 of file transforms.cpp.
| Pose2D pose_graph::projectToPose2D | ( | const Pose & | pose | ) |
Project Pose to Pose2D.
Definition at line 164 of file transforms.cpp.
| Eigen3::Vector4d pose_graph::quaternionMsgToVector4d | ( | const geometry_msgs::Quaternion & | m | ) |
Convert ROS quaternion to Eigen Vector4d in x,y,z,w order.
Definition at line 152 of file transforms.cpp.
| PoseGraph pose_graph::readFromFile | ( | const string & | filename | ) |
Reads a PoseGraph from a file.
| FileOpenException |
Definition at line 224 of file pose_graph_message.cpp.
| Pose pose_graph::relativePose | ( | const Pose & | pose1, |
| const Pose & | pose2 | ||
| ) |
pose1 and pose2 are poses specified in some reference frame. This function returns the representation of pose1 in the frame at pose2.
Definition at line 182 of file transforms.cpp.
| Affine3d pose_graph::relativeTransform | ( | const Pose & | pose1, |
| const Pose & | pose2 | ||
| ) |
Return transformation from frame of pose1 to frame of pose2.
Definition at line 72 of file transforms.cpp.
| Affine3d pose_graph::relativeTransform | ( | const Pose2D & | pose1, |
| const Pose2D & | pose2 | ||
| ) |
Return transformation from frame of pose1 to frame of pose2.
Definition at line 175 of file transforms.cpp.
| string pose_graph::toString | ( | const gm::Pose2D & | pose | ) | [inline] |
| string pose_graph::toString | ( | const gm::Point & | p | ) | [inline] |
| string pose_graph::toString | ( | const Eigen3::Affine3d & | trans | ) | [inline] |
| string pose_graph::toString | ( | const PoseConstraint & | constraint | ) | [inline] |
| string pose_graph::toString | ( | const gm::Pose & | pose | ) | [inline] |
| std::string pose_graph::toString | ( | const std::set< T > & | set | ) |
| std::string pose_graph::toString | ( | const std::vector< T > & | v | ) |
| std::string pose_graph::toString | ( | const PrecisionMatrix & | m | ) | [inline] |
| string pose_graph::toString2D | ( | const gm::Pose & | pose | ) | [inline] |
| string pose_graph::toString2D | ( | const Eigen3::Affine3d & | trans | ) | [inline] |
| Affine3d pose_graph::transformBetween | ( | const Pose & | pose1, |
| const Pose & | pose2 | ||
| ) |
Return transformation that sends pose1 to pose2. As opposed to relativeTransform which maps coordinates w.r.t pose1 to coordinates w.r.t pose2
Definition at line 80 of file transforms.cpp.
| nav_msgs::OccupancyGrid pose_graph::transformGrid | ( | const Affine3d & | trans, |
| const nav_msgs::OccupancyGrid & | g | ||
| ) |
Transform an occupancy grid (just transforms the origin)
| OccupancyGrid pose_graph::transformGrid | ( | const Affine3d & | trans, |
| const OccupancyGrid & | g | ||
| ) |
Definition at line 206 of file transforms.cpp.
| Point32 pose_graph::transformPoint | ( | const Affine3d & | trans, |
| const Point32 & | pt | ||
| ) |
Apply an Eigen transformation to a ROS point.
Definition at line 106 of file transforms.cpp.
| Point pose_graph::transformPoint | ( | const Affine3d & | trans, |
| const Point & | pt | ||
| ) |
Apply an Eigen transformation to a ROS point.
Definition at line 118 of file transforms.cpp.
| Eigen3::Vector4d pose_graph::translationToVector4d | ( | const geometry_msgs::Point & | p | ) |
Convert a ROS point to an Eigen 4d vector with 1 at the end.
Definition at line 157 of file transforms.cpp.
| NodeId pose_graph::vertexId | ( | const PoseGraphAdjacencyList & | graph, |
| const PoseGraphVertex & | v | ||
| ) |
Definition at line 106 of file pose_graph.cpp.
| void pose_graph::writeToFile | ( | const PoseGraph & | g, |
| const string & | filename | ||
| ) |
Writes a PoseGraph to a file.
| FileOpenException |