00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include <pose_graph/spa_2d_conversion.h>
00033 #include <graph_mapping_utils/geometry.h>
00034 #include <graph_mapping_utils/general.h>
00035 #include <pose_graph/exception.h>
00036 #include <pose_graph/graph_search.h>
00037 #include <tf/transform_datatypes.h>
00038 #include <boost/foreach.hpp>
00039 #include <boost/tuple/tuple.hpp>
00040 #include <boost/graph/connected_components.hpp>
00041 #include <set>
00042
00043 namespace pose_graph
00044 {
00045
00046 namespace util=graph_mapping_utils;
00047
00048 using std::set;
00049 using boost::tie;
00050 using sba::Node2d;
00051 using sba::SysSPA2d;
00052 using sba::Con2dP2;
00053
00054 typedef std::pair<unsigned, unsigned> NodePair;
00055
00056
00057 Eigen::Vector4d quaternionMsgToVector4d (const geometry_msgs::Quaternion& m)
00058 {
00059 return Eigen::Vector4d(m.x, m.y, m.z, m.w);
00060 }
00061
00062 Eigen::Vector4d translationToVector4d(const geometry_msgs::Point& p)
00063 {
00064 return Eigen::Vector4d(p.x, p.y, p.z, 1.0);
00065 }
00066
00067
00068
00069 Node2d makeNode (const double x, const double y, const double theta)
00070 {
00071 Node2d n;
00072 n.trans(0) = x;
00073 n.trans(1) = y;
00074 n.trans(2) = 1.0;
00075 n.arot = theta;
00076 n.setTransform();
00077 n.setDr();
00078 return n;
00079 }
00080
00081
00082
00083 double getYaw (const Quaterniond& q)
00084 {
00085 const tf::Quaternion qt(q.x(), q.y(), q.z(), q.w());
00086 return tf::getYaw(qt);
00087 }
00088
00089 unsigned index(const unsigned i, const unsigned j)
00090 {
00091 return 6*i + j;
00092 }
00093
00094 Con2dP2 makeSpa2DConstraint (const PoseWithPrecision& constraint)
00095 {
00096 Con2dP2 spa;
00097 spa.tmean[0] = constraint.pose.position.x;
00098 spa.tmean[1] = constraint.pose.position.y;
00099 spa.amean = tf::getYaw(constraint.pose.orientation);
00100
00101 spa.prec = Matrix3d::Zero();
00102 spa.prec(0,0) = constraint.precision[index(0,0)];
00103 spa.prec(1,1) = constraint.precision[index(1,1)];
00104
00105
00106
00107
00108 spa.prec(2,2) = constraint.precision[index(5,5)]*4;
00109
00110 return spa;
00111 }
00112
00113
00114 void verifyConnected (const ConstraintGraph& g, const NodeSet& nodes)
00115 {
00116 if (nodes.size() > 0) {
00117 const unsigned n = *nodes.begin();
00118 const NodeSet comp = componentContaining(g, n);
00119 BOOST_FOREACH (const unsigned n2, nodes) {
00120 if (!util::contains(comp, n2))
00121 throw DisconnectedComponentException(n, n2);
00122 }
00123 }
00124 }
00125
00126
00127
00128 Spa2DConversionResult constraintGraphToSpa2D (const ConstraintGraph& g, const NodePoseMap& init,
00129 const NodeSet& nodes)
00130 {
00131 Spa2DPtr spa(new SysSPA2d());
00132 map<unsigned, unsigned> node_indices;
00133 unsigned next_index=0;
00134
00135
00136 verifyConnected(g, nodes);
00137
00138
00139 BOOST_FOREACH (const unsigned n, nodes) {
00140 const tf::Pose p = util::keyValue(init, n);
00141 spa->nodes.push_back(makeNode(p.getOrigin().x(), p.getOrigin().y(), tf::getYaw(p.getRotation())));
00142 node_indices[n] = next_index++;
00143 }
00144
00145
00146
00147 BOOST_FOREACH (const unsigned e, g.allEdges()) {
00148
00149 NodePair ids = g.incidentNodes(e);
00150 if (util::contains(nodes, ids.first) && util::contains(nodes, ids.second)) {
00151 const PoseWithPrecision constraint = g.getConstraint(e);
00152 Con2dP2 spa_constraint = makeSpa2DConstraint(constraint);
00153
00154 spa_constraint.ndr = node_indices[ids.first];
00155 spa_constraint.nd1 = node_indices[ids.second];
00156 spa->p2cons.push_back(spa_constraint);
00157 ROS_DEBUG_STREAM_NAMED ("pose_graph_spa_int", "Adding constraint tmean "
00158 << spa_constraint.tmean << " and amean "
00159 << spa_constraint.amean << " from "
00160 << spa_constraint.ndr << " to "
00161 << spa_constraint.nd1);
00162 }
00163 }
00164
00165
00166 return Spa2DConversionResult(spa, node_indices);
00167 }
00168
00169
00170
00171 tf::Pose getNodePose (const sba::Node2d& n)
00172 {
00173 return util::makePose(n.trans(0), n.trans(1), n.arot);
00174 }
00175
00176
00177
00178 NodePoseMap optimizeGraph2D (const ConstraintGraph& g, const NodePoseMap& init)
00179 {
00180 return optimizeGraph2D(g, init, g.allNodes());
00181 }
00182
00184 NodePoseMap optimizeGraph2D (const ConstraintGraph& g, const NodePoseMap& init,
00185 const NodeSet& nodes)
00186 {
00187
00188
00189
00190
00191 ROS_DEBUG_NAMED ("pose_graph_spa", "Converting component with %zu nodes to spa", nodes.size());
00192 Spa2DConversionResult res=constraintGraphToSpa2D(g, init, nodes);
00193
00194
00195
00196
00197
00198 const size_t num_nodes = res.spa->nodes.size();
00199 const size_t num_constraints = res.spa->p2cons.size();
00200
00201
00202 if (num_nodes>=2 && num_constraints>=1) {
00203 ROS_DEBUG_NAMED ("pose_graph_spa", "Performing spa with %zu nodes and %zu constraints",
00204 num_nodes, num_constraints);
00205 res.spa->doSPA(10, 1.0e-4, SBA_DENSE_CHOLESKY);
00206 ROS_DEBUG_NAMED ("pose_graph_spa", "Graph optimization complete");
00207 }
00208 else
00209 ROS_DEBUG_NAMED ("pose_graph_spa", "Not optimizing graph with %zu nodes and %zu constraints",
00210 num_nodes, num_constraints);
00211
00212
00213
00214
00215
00216 NodePoseMap optimized_poses;
00217 BOOST_FOREACH (const NodeIndexMap::value_type entry, res.node_index_map) {
00218 optimized_poses[entry.first] = getNodePose(res.spa->nodes[entry.second]);
00219 }
00220 ROS_ASSERT(optimized_poses.size() == nodes.size());
00221 return optimized_poses;
00222 }
00223
00224
00225
00226
00227 }