28 #ifndef SRC_OCTOMAP_H_ 29 #define SRC_OCTOMAP_H_ 36 #include <pcl/pcl_base.h> 37 #include <pcl/point_types.h> 48 class RtabmapColorOcTree;
114 return setNodeColor(key,r,g,b);
126 return averageNodeColor(key,r,g,b);
138 return integrateNodeColor(key,r,g,b);
142 void updateInnerOccupancy();
173 OctoMap(
float cellSize = 0.1
f,
float occupancyThr = 0.5
f,
bool fullUpdate =
false,
float updateError=0.01
f);
175 const std::map<int, Transform> &
addedNodes()
const {
return addedNodes_;}
176 void addToCache(
int nodeId,
177 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
178 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
179 const pcl::PointXYZ & viewPoint);
180 void addToCache(
int nodeId,
181 const cv::Mat & ground,
182 const cv::Mat & obstacles,
183 const cv::Mat & empty,
184 const cv::Point3f & viewPoint);
185 bool update(
const std::map<int, Transform> & poses);
189 pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
190 unsigned int treeDepth = 0,
191 std::vector<int> * obstacleIndices = 0,
192 std::vector<int> * emptyIndices = 0,
193 std::vector<int> * groundIndices = 0,
194 bool originalRefPoints =
true,
195 std::vector<int> * frontierIndices = 0,
196 std::vector<double> * cloudProb = 0)
const;
198 cv::Mat createProjectionMap(
201 float & gridCellSize,
202 float minGridSize = 0.0
f,
203 unsigned int treeDepth = 0);
205 bool writeBinary(
const std::string & path);
210 void getGridMin(
double & x,
double & y,
double & z)
const {x=minValues_[0];y=minValues_[1];z=minValues_[2];}
211 void getGridMax(
double & x,
double & y,
double & z)
const {x=maxValues_[0];y=maxValues_[1];z=maxValues_[2];}
221 std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >
cache_;
222 std::map<int, std::pair<const pcl::PointCloud<pcl::PointXYZRGB>::Ptr,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >
cacheClouds_;
231 double minValues_[3];
232 double maxValues_[3];
RtabmapColorOcTreeNode * integrateNodeColor(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
RtabmapColorOcTree * create() const
virtual ~RtabmapColorOcTree()
octomap::point3d pointRef_
static StaticMemberInitializer RtabmapColorOcTreeMemberInit
static member to ensure static initialization (only once)
std::map< int, Transform > addedNodes_
bool createChild(unsigned int i)
std::map< int, cv::Point3f > cacheViewPoints_
RtabmapColorOcTreeNode * getChild(unsigned int i)
std::map< std::string, std::string > ParametersMap
RtabmapColorOcTreeNode(const RtabmapColorOcTreeNode &rhs)
void setPointRef(const octomap::point3d &point)
void setNodeRefId(int nodeRefId)
RtabmapColorOcTreeNode * setNodeColor(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
std::string getTreeType() const
const std::map< int, Transform > & addedNodes() const
std::map< int, std::pair< std::pair< cv::Mat, cv::Mat >, cv::Mat > > cache_
const octomap::point3d & getPointRef() const
int getOccupancyType() const
std::map< int, std::pair< const pcl::PointCloud< pcl::PointXYZRGB >::Ptr, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr > > cacheClouds_
friend class RtabmapColorOcTree
void getGridMax(double &x, double &y, double &z) const
RtabmapColorOcTree * octree_
const RtabmapColorOcTree * octree() const
void setOccupancyType(char type)
void setMaxRange(float value)
void getGridMin(double &x, double &y, double &z) const
RtabmapColorOcTreeNode * averageNodeColor(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
void setRayTracing(bool enabled)