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
00039 #include <flirtlib_ros/conversions.h>
00040 #include <tf/transform_datatypes.h>
00041 #include <boost/foreach.hpp>
00042
00043 namespace sm=sensor_msgs;
00044 namespace gm=geometry_msgs;
00045 namespace vm=visualization_msgs;
00046
00047 using std::vector;
00048
00049 typedef boost::shared_ptr<LaserReading> LaserPtr;
00050 typedef vector<double> DoubleVec;
00051 typedef vector<InterestPoint*> InterestPointVec;
00052
00053 namespace flirtlib_ros
00054 {
00055
00057 LaserPtr fromRos(const sm::LaserScan& m)
00058 {
00059 const unsigned n = m.ranges.size();
00060 DoubleVec angles(n);
00061 DoubleVec ranges(n);
00062 for (unsigned i=0; i<n; i++)
00063 {
00064 angles[i] = m.angle_min + i*m.angle_increment;
00065 ranges[i] = m.ranges[i];
00066 }
00067 LaserPtr reading(new LaserReading(angles, ranges, m.header.stamp.toSec(),
00068 "ros_laser", "ros_robot"));
00069 return reading;
00070 }
00071
00072
00073
00074 typedef vector<std_msgs::ColorRGBA> ColorVec;
00075 ColorVec initColors ()
00076 {
00077 ColorVec colors(2);
00078 colors[0].r = 0.5;
00079 colors[0].g = 1.0;
00080 colors[0].a = 1.0;
00081 colors[1].r = 1.0;
00082 colors[1].g = 1.0;
00083 colors[1].a = 1.0;
00084 return colors;
00085 }
00086
00087
00088
00089 vector<vm::Marker> poseMarkers (const vector<gm::Pose>& poses)
00090 {
00091 vector<vm::Marker> markers;
00092
00093 for (unsigned id=0; id<100; id++)
00094 {
00095 vm::Marker m;
00096 m.header.frame_id = "/map";
00097 m.header.stamp = ros::Time::now();
00098 m.ns="flirtlib.poses";
00099 m.id = id;
00100 m.type = vm::Marker::ARROW;
00101 m.color.r = m.color.b = .15;
00102 m.color.g = .65;
00103 m.color.a = 1;
00104 m.scale.x = m.scale.y = 2.0;
00105 m.scale.z = 1.0;
00106 if (id < poses.size())
00107 {
00108 m.pose = poses[id];
00109 }
00110 else
00111 {
00112 m.pose.orientation.w = 1.0;
00113 m.action = vm::Marker::DELETE;
00114 }
00115
00116 markers.push_back(m);
00117 }
00118
00119 return markers;
00120 }
00121
00122 gm::Point toPoint (const tf::Vector3& p)
00123 {
00124 gm::Point pt;
00125 pt.x = p.x();
00126 pt.y = p.y();
00127 pt.z = p.z();
00128 return pt;
00129 }
00130
00131
00132
00133
00134
00135 vm::Marker interestPointMarkers (const InterestPointVec& pts, const gm::Pose& pose, const unsigned id)
00136 {
00137 tf::Transform trans;
00138 tf::poseMsgToTF(pose, trans);
00139 static ColorVec colors = initColors();
00140 vm::Marker m;
00141 m.header.frame_id = "/map";
00142 m.header.stamp = ros::Time::now();
00143 m.ns = "flirtlib";
00144 m.id = id;
00145 m.type = vm::Marker::LINE_LIST;
00146 m.scale.x = 0.02;
00147 m.color = colors[id];
00148 BOOST_FOREACH (const InterestPoint* p, pts)
00149 {
00150 const double x0 = p->getPosition().x;
00151 const double y0 = p->getPosition().y;
00152 const double d = 0.1;
00153 double dx[4];
00154 double dy[4];
00155 if (id==0)
00156 {
00157 dx[0] = dx[3] = -d;
00158 dx[1] = dx[2] = d;
00159 dy[0] = dy[1] = -d;
00160 dy[2] = dy[3] = d;
00161 }
00162 else
00163 {
00164 ROS_ASSERT(id==1);
00165 const double r2 = sqrt(2);
00166 dx[0] = dx[2] = dy[1] = dy[3] = 0;
00167 dx[1] = dy[0] = -r2*d;
00168 dx[3] = dy[2] = r2*d;
00169 }
00170
00171 for (unsigned i=0; i<4; i++)
00172 {
00173 const unsigned j = (i==0) ? 3 : i-1;
00174 const tf::Point pt0(x0+dx[i], y0+dy[i], 0);
00175 const tf::Point pt1(x0+dx[j], y0+dy[j], 0);
00176 m.points.push_back(toPoint(trans*pt0));
00177 m.points.push_back(toPoint(trans*pt1));
00178 }
00179 }
00180
00181 return m;
00182 }
00183
00184 inline
00185 gm::Point toPoint (const Point2D& p)
00186 {
00187 gm::Point pt;
00188 pt.x = p.x;
00189 pt.y = p.y;
00190 return pt;
00191 }
00192
00193 inline
00194 Point2D toPoint2D (const gm::Point& p)
00195 {
00196 return Point2D(p.x, p.y);
00197 }
00198
00199 vector<Vector> toRos (const vector<vector<double> >& h)
00200 {
00201 vector<Vector> m;
00202 m.resize(h.size());
00203 for (unsigned i=0; i<h.size(); i++)
00204 {
00205 const vector<double>& v = h[i];
00206 Vector& v2 = m[i];
00207 vector<double>& vec = v2.vec;
00208 vec = v;
00209 }
00210 return m;
00211 }
00212
00213 vector<vector<double> > fromRos (const vector<Vector>& m)
00214 {
00215 vector<vector<double> > h;
00216 h.resize(m.size());
00217 for (unsigned i=0; i<m.size(); i++)
00218 h[i] = m[i].vec;
00219 return h;
00220 }
00221
00222 DescriptorRos toRos (const Descriptor* descriptor)
00223 {
00224 const BetaGrid* desc = dynamic_cast<const BetaGrid*>(descriptor);
00225 ROS_ASSERT_MSG(desc,"Descriptor was not of type BetaGrid");
00226 ROS_ASSERT_MSG(dynamic_cast<const SymmetricChi2Distance<double>*>
00227 (desc->getDistanceFunction()),
00228 "Distance function was not of type SymmetricChi2Distance");
00229 DescriptorRos m;
00230 m.hist = toRos(desc->getHistogram());
00231 m.variance = toRos(desc->getVariance());
00232 m.hit = toRos(desc->getHit());
00233 m.miss = toRos(desc->getMiss());
00234 return m;
00235 }
00236
00237 Descriptor* fromRos (const DescriptorRos& m)
00238 {
00239 BetaGrid* desc = new BetaGrid();
00240 desc->setDistanceFunction(new SymmetricChi2Distance<double>());
00241 desc->getHistogram() = fromRos(m.hist);
00242 desc->getVariance() = fromRos(m.variance);
00243 desc->getHit() = fromRos(m.hit);
00244 desc->getMiss() = fromRos(m.miss);
00245 return desc;
00246 }
00247
00248 InterestPointRos toRos (const InterestPoint& pt)
00249 {
00250 InterestPointRos m;
00251
00252 m.pose.x = pt.getPosition().x;
00253 m.pose.y = pt.getPosition().y;
00254 m.pose.theta = pt.getPosition().theta;
00255
00256 m.support_points.reserve(pt.getSupport().size());
00257 BOOST_FOREACH (const Point2D& p, pt.getSupport())
00258 m.support_points.push_back(toPoint(p));
00259
00260 m.scale = pt.getScale();
00261 m.scale_level = pt.getScaleLevel();
00262 m.descriptor = toRos(pt.getDescriptor());
00263
00264 return m;
00265 }
00266
00267
00268 InterestPoint* fromRos (const InterestPointRos& m)
00269 {
00270 OrientedPoint2D pose(m.pose.x, m.pose.y, m.pose.theta);
00271 Descriptor* descriptor = fromRos(m.descriptor);
00272 InterestPoint* pt = new InterestPoint(pose, m.scale, descriptor);
00273 pt->setScaleLevel(m.scale_level);
00274 vector<Point2D> support_points(m.support_points.size());
00275 transform(m.support_points.begin(), m.support_points.end(),
00276 support_points.begin(), toPoint2D);
00277 pt->setSupport(support_points);
00278 return pt;
00279 }
00280
00281 RefScan fromRos (const RefScanRos& m)
00282 {
00283 sm::LaserScan::ConstPtr scan(new sm::LaserScan(m.scan));
00284 InterestPointVec pts;
00285 BOOST_FOREACH (const InterestPointRos& p, m.pts)
00286 pts.push_back(fromRos(p));
00287 return RefScan(scan, m.pose, pts);
00288 }
00289
00290 RefScanRos toRos (const RefScan& ref)
00291 {
00292 RefScanRos m;
00293
00294 m.scan = *ref.scan;
00295 m.pose = ref.pose;
00296 BOOST_FOREACH (boost::shared_ptr<InterestPoint> p, ref.pts)
00297 m.pts.push_back(toRos(*p));
00298 return m;
00299 }
00300
00301 vector<RefScan> fromRos (const ScanMap& scan_map)
00302 {
00303 vector<RefScan> scans;
00304 BOOST_FOREACH (const RefScanRos& scan, scan_map.scans)
00305 scans.push_back(fromRos(scan));
00306 return scans;
00307 }
00308
00309 ScanMap toRos (const vector<RefScan>& scans)
00310 {
00311 ScanMap m;
00312 BOOST_FOREACH (const RefScan& scan, scans)
00313 m.scans.push_back(toRos(scan));
00314 return m;
00315 }
00316
00317
00318 RefScan::RefScan (sm::LaserScan::ConstPtr scan, const gm::Pose& pose,
00319 vector<InterestPoint*>& points) :
00320 scan(scan), pose(pose), raw_pts(points)
00321 {
00322 pts.reserve(points.size());
00323 BOOST_FOREACH (InterestPoint* p, points)
00324 pts.push_back(boost::shared_ptr<InterestPoint>(p));
00325 }
00326
00327 }
