conversions.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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 // Generate visualization markers for the interest points
00133 // id is 0 or 1, and controls color and orientation to distinguish between
00134 // the two scans
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 } // namespace


flirtlib_ros
Author(s): Bhaskara Marthi
autogenerated on Thu Nov 28 2013 11:21:50