2 #ifndef _TUW_VORONOI_SERIALIZER 3 #define _TUW_VORONOI_SERIALIZER 5 #include <nav_msgs/OccupancyGrid.h> 7 #include <opencv/cv.hpp> 11 #include <boost/archive/xml_oarchive.hpp> 12 #include <boost/archive/xml_iarchive.hpp> 14 #define DEFAULT_MAP_NAME "voronoi_map" 15 #include <eigen3/Eigen/Dense> 16 #include <boost/filesystem.hpp> 17 #include <boost/functional/hash.hpp> 41 template<
class archive>
void serialize(archive & ar,
const unsigned int version)
43 using boost::serialization::make_nvp;
44 ar & boost::serialization::make_nvp(
"x", x);
45 ar & boost::serialization::make_nvp(
"y", y);
54 GraphInfo(Eigen::Vector2d _pt,
float _resolution,
int _nrSegments_) : Origin(_pt)
56 Resolution = _resolution;
57 SegmentLength = _nrSegments_;
64 friend class boost::serialization::access;
65 template<
class archive>
void serialize(archive & ar,
const unsigned int version)
67 using boost::serialization::make_nvp;
68 ar & boost::serialization::make_nvp(
"Origin", Origin);
69 ar & boost::serialization::make_nvp(
"Resolution", Resolution);
70 ar & boost::serialization::make_nvp(
"SegmentLength", SegmentLength);
79 for(uint32_t i = 0; i < _segs.size(); i++)
81 int * pred = Predecessors.get();
82 int * succ = Successors.get();
83 int * point = Points.get();
84 pred[i] = _segs[i].getPredecessors().size();
85 succ[i] = _segs[i].getSuccessors().size();
86 point[i] = _segs[i].getPath().size();
91 Predecessors.reset(
new int[_length]);
92 Successors.reset(
new int[_length]);
93 Points.reset(
new int[_length]);
107 friend class boost::serialization::access;
108 template<
class archive>
void serialize(archive & ar,
const unsigned int version)
110 using boost::serialization::make_nvp;
111 ar & boost::serialization::make_nvp(
"Length", Length);
112 ar & boost::serialization::make_array<int>(Predecessors.get(), Length);
113 ar & boost::serialization::make_array<int>(Successors.get(), Length);
114 ar & boost::serialization::make_array<int>(Points.get(), Length);
129 predecessorLength = _predLength;
130 successorLength = _succLength;
131 pointLength = _pointLength;
132 predecessors.reset(
new int[predecessorLength]);
133 successors.reset(
new int[successorLength]);
143 SegmentSerializer(
const uint32_t _id, std::vector<uint32_t> _predecessors, std::vector<uint32_t> _successors,
float _minDistance, std::vector<Eigen::Vector2d> _points):
147 minDistance = _minDistance;
149 int *pred = predecessors.get();
150 int *succ = successors.get();
154 for(
int i = 0; i < predecessorLength; i++)
156 pred[i] = _predecessors[i];
159 for(
int i = 0; i < successorLength; i++)
161 succ[i] = _successors[i];
164 for(uint32_t i = 0; i < _points.size(); i++)
181 friend class boost::serialization::access;
182 template<
class archive>
void serialize(archive & ar,
const unsigned int version)
184 using boost::serialization::make_nvp;
185 ar & boost::serialization::make_nvp(
"id",
id);
186 ar & boost::serialization::make_nvp(
"predecessorLength", predecessorLength);
187 ar & boost::serialization::make_nvp(
"successorLength", successorLength);
188 ar & boost::serialization::make_nvp(
"minDistance", minDistance);
189 ar & boost::serialization::make_nvp(
"pointLength", pointLength);
190 ar & boost::serialization::make_array<int>(predecessors.get(), predecessorLength);
191 ar & boost::serialization::make_array<int>(successors.get(), successorLength);
192 ar & boost::serialization::make_array<PointSerializer>(points.get(), pointLength);
202 segments_ = &_segments[0];
203 Length = _segments.size();
208 friend class boost::serialization::access;
209 template<
class archive>
void serialize(archive & ar,
const unsigned int version)
211 using boost::serialization::make_nvp;
212 ar & boost::serialization::make_nvp(
"pointLength", Length);
213 ar & boost::serialization::make_array<SegmentSerializer>(segments_, Length);
228 void save(
const std::string &_mapPath,
const std::vector<Segment> &_segs,
const Eigen::Vector2d &_origin,
const float &_resolution);
237 void save(
const std::string &_mapPath,
const std::vector<Segment> &_segs,
const Eigen::Vector2d &_origin,
const float &_resolution,
const cv::Mat &_map);
245 bool load(
const std::string &_mapPath, std::vector<Segment> &_segs, Eigen::Vector2d &_origin,
float &_resolution);
254 bool load(
const std::string &_mapPath, std::vector<Segment> &_segs, Eigen::Vector2d &_origin,
float &_resolution, cv::Mat &_map);
260 size_t getHash(
const std::vector<signed char> &_map,
const std::vector<double> &_parameters)
const;
void serialize(archive &ar, const unsigned int version)
friend class boost::serialization::access
GraphInfo(Eigen::Vector2d _pt, float _resolution, int _nrSegments_)
GraphSerializer(std::vector< SegmentSerializer > &_segments)
TreeInfo(std::vector< Segment > _segs)
PointSerializer(Eigen::Vector2d p)
std::unique_ptr< int > Predecessors
void serialize(archive &ar, const unsigned int version)
SegmentSerializer(const uint32_t _id, std::vector< uint32_t > _predecessors, std::vector< uint32_t > _successors, float _minDistance, std::vector< Eigen::Vector2d > _points)
std::unique_ptr< int > predecessors
void serialize(archive &ar, const unsigned int version)
SegmentSerializer(const Segment &_s)
std::unique_ptr< PointSerializer > points
SegmentSerializer(int _predLength, int _succLength, int _pointLength)
std::unique_ptr< int > successors
ROSLIB_DECL std::string getPath(const std::string &package_name)
void serialize(archive &ar, const unsigned int version)
PointSerializer(float _x, float _y)
void serialize(archive &ar, const unsigned int version)
std::unique_ptr< int > Successors
std::unique_ptr< int > Points
SegmentSerializer * segments_