├── CMakeLists.txt
├── include
    └── plan_env
    │   ├── grid_map.h
    │   └── raycast.h
├── launch
    ├── grid_map.launch
    └── plan_env.rviz
├── package.xml
├── readme.md
└── src
    ├── grid_map.cpp
    ├── node.cpp
    └── raycast.cpp


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(plan_env)
 3 | 
 4 | set(CMAKE_BUILD_TYPE "Release")
 5 | ADD_COMPILE_OPTIONS(-std=c++11 )
 6 | ADD_COMPILE_OPTIONS(-std=c++14 )
 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
 8 | 
 9 | find_package(OpenCV REQUIRED)
10 | 
11 | 
12 | find_package(catkin REQUIRED COMPONENTS
13 |   roscpp
14 |   rospy
15 |   std_msgs
16 |   visualization_msgs
17 |   cv_bridge
18 |   message_filters
19 | )
20 | 
21 | find_package(Eigen3 REQUIRED)
22 | find_package(PCL 1.7 REQUIRED)
23 | 
24 | catkin_package(
25 |  INCLUDE_DIRS include
26 |  LIBRARIES plan_env
27 |  CATKIN_DEPENDS roscpp std_msgs
28 | #  DEPENDS system_lib
29 | )
30 | 
31 | include_directories( 
32 |     SYSTEM 
33 |     include 
34 |     ${catkin_INCLUDE_DIRS}
35 |     ${Eigen3_INCLUDE_DIRS} 
36 |     ${PCL_INCLUDE_DIRS}
37 |     ${OpenCV_INCLUDE_DIRS}
38 | )
39 | 
40 | link_directories(${PCL_LIBRARY_DIRS})
41 | 
42 | add_library( plan_env 
43 |     src/grid_map.cpp 
44 |     src/raycast.cpp
45 |     )
46 | target_link_libraries( plan_env
47 |     ${catkin_LIBRARIES} 
48 |     ${PCL_LIBRARIES}
49 |     )  
50 | 
51 | add_executable(gridmap_node src/node.cpp)
52 | target_link_libraries(gridmap_node ${catkin_LIBRARIES} plan_env)
53 | add_dependencies(gridmap_node ${catkin_EXPORTED_TARGETS})
54 | 


--------------------------------------------------------------------------------
/include/plan_env/grid_map.h:
--------------------------------------------------------------------------------
  1 | #ifndef _GRID_MAP_H
  2 | #define _GRID_MAP_H
  3 | 
  4 | #include <Eigen/Eigen>
  5 | #include <Eigen/StdVector>
  6 | #include <cv_bridge/cv_bridge.h>
  7 | #include <geometry_msgs/PoseStamped.h>
  8 | #include <iostream>
  9 | #include <random>
 10 | #include <nav_msgs/Odometry.h>
 11 | #include <queue>
 12 | #include <ros/ros.h>
 13 | #include <tuple>
 14 | #include <visualization_msgs/Marker.h>
 15 | 
 16 | #include <pcl/point_cloud.h>
 17 | #include <pcl/point_types.h>
 18 | #include <pcl_conversions/pcl_conversions.h>
 19 | 
 20 | #include <message_filters/subscriber.h>
 21 | #include <message_filters/sync_policies/approximate_time.h>
 22 | #include <message_filters/sync_policies/exact_time.h>
 23 | #include <message_filters/time_synchronizer.h>
 24 | 
 25 | #include <plan_env/raycast.h>
 26 | 
 27 | #define logit(x) (log((x) / (1 - (x))))
 28 | 
 29 | using namespace std;
 30 | 
 31 | // voxel hashing
 32 | template <typename T>
 33 | struct matrix_hash : std::unary_function<T, size_t> {
 34 |   std::size_t operator()(T const& matrix) const {
 35 |     size_t seed = 0;
 36 |     for (size_t i = 0; i < matrix.size(); ++i) {
 37 |       auto elem = *(matrix.data() + i);
 38 |       seed ^= std::hash<typename T::Scalar>()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
 39 |     }
 40 |     return seed;
 41 |   }
 42 | };
 43 | 
 44 | // constant parameters
 45 | 
 46 | struct MappingParameters {
 47 | 
 48 |   /* map properties */
 49 |   Eigen::Vector3d map_origin_, map_size_;
 50 |   Eigen::Vector3d map_min_boundary_, map_max_boundary_;  // map range in pos
 51 |   Eigen::Vector3i map_voxel_num_;                        // map range in index
 52 |   Eigen::Vector3d local_update_range_;
 53 |   double resolution_, resolution_inv_;
 54 |   double obstacles_inflation_;
 55 |   string frame_id_;
 56 |   int pose_type_;
 57 | 
 58 |   /* camera parameters */
 59 |   double cx_, cy_, fx_, fy_;
 60 | 
 61 |   /* depth image projection filtering */
 62 |   double depth_filter_maxdist_, depth_filter_mindist_, depth_filter_tolerance_;
 63 |   int depth_filter_margin_;
 64 |   bool use_depth_filter_;
 65 |   double k_depth_scaling_factor_;
 66 |   int skip_pixel_;
 67 | 
 68 |   /* raycasting */
 69 |   double p_hit_, p_miss_, p_min_, p_max_, p_occ_;  // occupancy probability
 70 |   double prob_hit_log_, prob_miss_log_, clamp_min_log_, clamp_max_log_,
 71 |       min_occupancy_log_;                   // logit of occupancy probability
 72 |   double min_ray_length_, max_ray_length_;  // range of doing raycasting
 73 | 
 74 |   /* local map update and clear */
 75 |   int local_map_margin_;
 76 | 
 77 |   /* visualization and computation time display */
 78 |   double visualization_truncate_height_, virtual_ceil_height_, ground_height_;
 79 |   bool show_occ_time_;
 80 | 
 81 |   /* active mapping */
 82 |   double unknown_flag_;
 83 | };
 84 | 
 85 | // intermediate mapping data for fusion
 86 | 
 87 | struct MappingData {
 88 |   // main map data, occupancy of each voxel and Euclidean distance
 89 | 
 90 |   std::vector<double> occupancy_buffer_;
 91 |   std::vector<char> occupancy_buffer_inflate_;
 92 | 
 93 |   // camera position and pose data
 94 | 
 95 |   Eigen::Vector3d camera_pos_, last_camera_pos_;
 96 |   Eigen::Quaterniond camera_q_, last_camera_q_;
 97 | 
 98 |   // depth image data
 99 | 
100 |   cv::Mat depth_image_, last_depth_image_;
101 |   int image_cnt_;
102 | 
103 |   Eigen::Matrix4d cam2body_;
104 | 
105 |   // flags of map state
106 | 
107 |   bool occ_need_update_, local_updated_;
108 |   bool has_first_depth_;
109 |   bool has_odom_, has_cloud_;
110 | 
111 |   // depth image projected point cloud
112 | 
113 |   vector<Eigen::Vector3d> proj_points_;
114 |   int proj_points_cnt;
115 | 
116 |   // flag buffers for speeding up raycasting
117 | 
118 |   vector<short> count_hit_, count_hit_and_miss_;
119 |   vector<char> flag_traverse_, flag_rayend_;
120 |   char raycast_num_;
121 |   queue<Eigen::Vector3i> cache_voxel_;
122 | 
123 |   // range of updating grid
124 | 
125 |   Eigen::Vector3i local_bound_min_, local_bound_max_;
126 | 
127 |   // computation time
128 | 
129 |   double fuse_time_, max_fuse_time_;
130 |   int update_num_;
131 | 
132 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
133 | };
134 | 
135 | class GridMap {
136 | public:
137 |   GridMap() {}
138 |   ~GridMap() {}
139 | 
140 |   enum { POSE_STAMPED = 1, ODOMETRY = 2, INVALID_IDX = -10000 };
141 | 
142 |   // occupancy map management
143 |   void resetBuffer();
144 |   void resetBuffer(Eigen::Vector3d min, Eigen::Vector3d max);
145 | 
146 |   inline void posToIndex(const Eigen::Vector3d& pos, Eigen::Vector3i& id);
147 |   inline void indexToPos(const Eigen::Vector3i& id, Eigen::Vector3d& pos);
148 |   inline int toAddress(const Eigen::Vector3i& id);
149 |   inline int toAddress(int& x, int& y, int& z);
150 |   inline bool isInMap(const Eigen::Vector3d& pos);
151 |   inline bool isInMap(const Eigen::Vector3i& idx);
152 | 
153 |   inline void setOccupancy(Eigen::Vector3d pos, double occ = 1);
154 |   inline void setOccupied(Eigen::Vector3d pos);
155 |   inline int getOccupancy(Eigen::Vector3d pos);
156 |   inline int getOccupancy(Eigen::Vector3i id);
157 |   inline int getInflateOccupancy(Eigen::Vector3d pos);
158 | 
159 |   inline void boundIndex(Eigen::Vector3i& id);
160 |   inline bool isUnknown(const Eigen::Vector3i& id);
161 |   inline bool isUnknown(const Eigen::Vector3d& pos);
162 |   inline bool isKnownFree(const Eigen::Vector3i& id);
163 |   inline bool isKnownOccupied(const Eigen::Vector3i& id);
164 | 
165 |   void initMap(ros::NodeHandle& nh);
166 | 
167 |   void publishMap();
168 |   void publishMapInflate(bool all_info = false);
169 | 
170 |   void publishUnknown();
171 |   void publishDepth();
172 | 
173 |   bool hasDepthObservation();
174 |   bool odomValid();
175 |   void getRegion(Eigen::Vector3d& ori, Eigen::Vector3d& size);
176 |   inline double getResolution();
177 |   Eigen::Vector3d getOrigin();
178 |   int getVoxelNum();
179 | 
180 |   typedef std::shared_ptr<GridMap> Ptr;
181 | 
182 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
183 | 
184 | private:
185 |   MappingParameters mp_;
186 |   MappingData md_;
187 | 
188 |   // get depth image and camera pose
189 |   void depthPoseCallback(const sensor_msgs::ImageConstPtr& img,
190 |                          const geometry_msgs::PoseStampedConstPtr& pose);
191 |   void depthOdomCallback(const sensor_msgs::ImageConstPtr& img, const nav_msgs::OdometryConstPtr& odom);
192 |   void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& img);
193 |   void odomCallback(const nav_msgs::OdometryConstPtr& odom);
194 | 
195 |   // update occupancy by raycasting
196 |   void updateOccupancyCallback(const ros::TimerEvent& /*event*/);
197 |   void visCallback(const ros::TimerEvent& /*event*/);
198 | 
199 |   // main update process
200 |   void projectDepthImage();
201 |   void raycastProcess();
202 |   void clearAndInflateLocalMap();
203 | 
204 |   inline void inflatePoint(const Eigen::Vector3i& pt, int step, vector<Eigen::Vector3i>& pts);
205 |   int setCacheOccupancy(Eigen::Vector3d pos, int occ);
206 |   Eigen::Vector3d closetPointInMap(const Eigen::Vector3d& pt, const Eigen::Vector3d& camera_pt);
207 | 
208 |   // typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image,
209 |   // nav_msgs::Odometry> SyncPolicyImageOdom; typedef
210 |   // message_filters::sync_policies::ExactTime<sensor_msgs::Image,
211 |   // geometry_msgs::PoseStamped> SyncPolicyImagePose;
212 |   typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, nav_msgs::Odometry>
213 |       SyncPolicyImageOdom;
214 |   typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, geometry_msgs::PoseStamped>
215 |       SyncPolicyImagePose;
216 |   typedef shared_ptr<message_filters::Synchronizer<SyncPolicyImagePose>> SynchronizerImagePose;
217 |   typedef shared_ptr<message_filters::Synchronizer<SyncPolicyImageOdom>> SynchronizerImageOdom;
218 | 
219 |   ros::NodeHandle node_;
220 |   shared_ptr<message_filters::Subscriber<sensor_msgs::Image>> depth_sub_;
221 |   shared_ptr<message_filters::Subscriber<geometry_msgs::PoseStamped>> pose_sub_;
222 |   shared_ptr<message_filters::Subscriber<nav_msgs::Odometry>> odom_sub_;
223 |   SynchronizerImagePose sync_image_pose_;
224 |   SynchronizerImageOdom sync_image_odom_;
225 | 
226 |   ros::Subscriber indep_cloud_sub_, indep_odom_sub_;
227 |   ros::Publisher map_pub_, map_inf_pub_;
228 |   ros::Publisher unknown_pub_;
229 |   ros::Timer occ_timer_, vis_timer_;
230 | 
231 |   //
232 |   uniform_real_distribution<double> rand_noise_;
233 |   normal_distribution<double> rand_noise2_;
234 |   default_random_engine eng_;
235 | };
236 | 
237 | /* ============================== definition of inline function
238 |  * ============================== */
239 | 
240 | inline int GridMap::toAddress(const Eigen::Vector3i& id) {
241 |   return id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + id(1) * mp_.map_voxel_num_(2) + id(2);
242 | }
243 | 
244 | inline int GridMap::toAddress(int& x, int& y, int& z) {
245 |   return x * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + y * mp_.map_voxel_num_(2) + z;
246 | }
247 | 
248 | inline void GridMap::boundIndex(Eigen::Vector3i& id) {
249 |   Eigen::Vector3i id1;
250 |   id1(0) = max(min(id(0), mp_.map_voxel_num_(0) - 1), 0);
251 |   id1(1) = max(min(id(1), mp_.map_voxel_num_(1) - 1), 0);
252 |   id1(2) = max(min(id(2), mp_.map_voxel_num_(2) - 1), 0);
253 |   id = id1;
254 | }
255 | 
256 | inline bool GridMap::isUnknown(const Eigen::Vector3i& id) {
257 |   Eigen::Vector3i id1 = id;
258 |   boundIndex(id1);
259 |   return md_.occupancy_buffer_[toAddress(id1)] < mp_.clamp_min_log_ - 1e-3;
260 | }
261 | 
262 | inline bool GridMap::isUnknown(const Eigen::Vector3d& pos) {
263 |   Eigen::Vector3i idc;
264 |   posToIndex(pos, idc);
265 |   return isUnknown(idc);
266 | }
267 | 
268 | inline bool GridMap::isKnownFree(const Eigen::Vector3i& id) {
269 |   Eigen::Vector3i id1 = id;
270 |   boundIndex(id1);
271 |   int adr = toAddress(id1);
272 | 
273 |   // return md_.occupancy_buffer_[adr] >= mp_.clamp_min_log_ &&
274 |   //     md_.occupancy_buffer_[adr] < mp_.min_occupancy_log_;
275 |   return md_.occupancy_buffer_[adr] >= mp_.clamp_min_log_ && md_.occupancy_buffer_inflate_[adr] == 0;
276 | }
277 | 
278 | inline bool GridMap::isKnownOccupied(const Eigen::Vector3i& id) {
279 |   Eigen::Vector3i id1 = id;
280 |   boundIndex(id1);
281 |   int adr = toAddress(id1);
282 | 
283 |   return md_.occupancy_buffer_inflate_[adr] == 1;
284 | }
285 | 
286 | inline void GridMap::setOccupied(Eigen::Vector3d pos) {
287 |   if (!isInMap(pos)) return;
288 | 
289 |   Eigen::Vector3i id;
290 |   posToIndex(pos, id);
291 | 
292 |   md_.occupancy_buffer_inflate_[id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) +
293 |                                 id(1) * mp_.map_voxel_num_(2) + id(2)] = 1;
294 | }
295 | 
296 | inline void GridMap::setOccupancy(Eigen::Vector3d pos, double occ) {
297 |   if (occ != 1 && occ != 0) {
298 |     cout << "occ value error!" << endl;
299 |     return;
300 |   }
301 | 
302 |   if (!isInMap(pos)) return;
303 | 
304 |   Eigen::Vector3i id;
305 |   posToIndex(pos, id);
306 | 
307 |   md_.occupancy_buffer_[toAddress(id)] = occ;
308 | }
309 | 
310 | inline int GridMap::getOccupancy(Eigen::Vector3d pos) {
311 |   if (!isInMap(pos)) return -1;
312 | 
313 |   Eigen::Vector3i id;
314 |   posToIndex(pos, id);
315 | 
316 |   return md_.occupancy_buffer_[toAddress(id)] > mp_.min_occupancy_log_ ? 1 : 0;
317 | }
318 | 
319 | inline int GridMap::getInflateOccupancy(Eigen::Vector3d pos) {
320 |   if (!isInMap(pos)) return -1;
321 | 
322 |   Eigen::Vector3i id;
323 |   posToIndex(pos, id);
324 | 
325 |   return int(md_.occupancy_buffer_inflate_[toAddress(id)]);
326 | }
327 | 
328 | inline int GridMap::getOccupancy(Eigen::Vector3i id) {
329 |   if (id(0) < 0 || id(0) >= mp_.map_voxel_num_(0) || id(1) < 0 || id(1) >= mp_.map_voxel_num_(1) ||
330 |       id(2) < 0 || id(2) >= mp_.map_voxel_num_(2))
331 |     return -1;
332 | 
333 |   return md_.occupancy_buffer_[toAddress(id)] > mp_.min_occupancy_log_ ? 1 : 0;
334 | }
335 | 
336 | inline bool GridMap::isInMap(const Eigen::Vector3d& pos) {
337 |   if (pos(0) < mp_.map_min_boundary_(0) + 1e-4 || pos(1) < mp_.map_min_boundary_(1) + 1e-4 ||
338 |       pos(2) < mp_.map_min_boundary_(2) + 1e-4) {
339 |     // cout << "less than min range!" << endl;
340 |     return false;
341 |   }
342 |   if (pos(0) > mp_.map_max_boundary_(0) - 1e-4 || pos(1) > mp_.map_max_boundary_(1) - 1e-4 ||
343 |       pos(2) > mp_.map_max_boundary_(2) - 1e-4) {
344 |     return false;
345 |   }
346 |   return true;
347 | }
348 | 
349 | inline bool GridMap::isInMap(const Eigen::Vector3i& idx) {
350 |   if (idx(0) < 0 || idx(1) < 0 || idx(2) < 0) {
351 |     return false;
352 |   }
353 |   if (idx(0) > mp_.map_voxel_num_(0) - 1 || idx(1) > mp_.map_voxel_num_(1) - 1 ||
354 |       idx(2) > mp_.map_voxel_num_(2) - 1) {
355 |     return false;
356 |   }
357 |   return true;
358 | }
359 | 
360 | inline void GridMap::posToIndex(const Eigen::Vector3d& pos, Eigen::Vector3i& id) {
361 |   for (int i = 0; i < 3; ++i) id(i) = floor((pos(i) - mp_.map_origin_(i)) * mp_.resolution_inv_);
362 | }
363 | 
364 | inline void GridMap::indexToPos(const Eigen::Vector3i& id, Eigen::Vector3d& pos) {
365 |   for (int i = 0; i < 3; ++i) pos(i) = (id(i) + 0.5) * mp_.resolution_ + mp_.map_origin_(i);
366 | }
367 | 
368 | inline void GridMap::inflatePoint(const Eigen::Vector3i& pt, int step, vector<Eigen::Vector3i>& pts) {
369 |   int num = 0;
370 | 
371 |   /* ---------- + shape inflate ---------- */
372 |   // for (int x = -step; x <= step; ++x)
373 |   // {
374 |   //   if (x == 0)
375 |   //     continue;
376 |   //   pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1), pt(2));
377 |   // }
378 |   // for (int y = -step; y <= step; ++y)
379 |   // {
380 |   //   if (y == 0)
381 |   //     continue;
382 |   //   pts[num++] = Eigen::Vector3i(pt(0), pt(1) + y, pt(2));
383 |   // }
384 |   // for (int z = -1; z <= 1; ++z)
385 |   // {
386 |   //   pts[num++] = Eigen::Vector3i(pt(0), pt(1), pt(2) + z);
387 |   // }
388 | 
389 |   /* ---------- all inflate ---------- */
390 |   for (int x = -step; x <= step; ++x)
391 |     for (int y = -step; y <= step; ++y)
392 |       for (int z = -step; z <= step; ++z) {
393 |         pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1) + y, pt(2) + z);
394 |       }
395 | }
396 | 
397 | inline double GridMap::getResolution() { return mp_.resolution_; }
398 | 
399 | #endif


--------------------------------------------------------------------------------
/include/plan_env/raycast.h:
--------------------------------------------------------------------------------
 1 | #ifndef RAYCAST_H_
 2 | #define RAYCAST_H_
 3 | 
 4 | #include <Eigen/Eigen>
 5 | #include <vector>
 6 | 
 7 | double signum(double x);
 8 | 
 9 | double mod(double value, double modulus);
10 | 
11 | double intbound(double s, double ds);
12 | 
13 | void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min,
14 |              const Eigen::Vector3d& max, int& output_points_cnt, Eigen::Vector3d* output);
15 | 
16 | void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min,
17 |              const Eigen::Vector3d& max, std::vector<Eigen::Vector3d>* output);
18 | 
19 | class RayCaster {
20 | private:
21 |   /* data */
22 |   Eigen::Vector3d start_;
23 |   Eigen::Vector3d end_;
24 |   Eigen::Vector3d direction_;
25 |   Eigen::Vector3d min_;
26 |   Eigen::Vector3d max_;
27 |   int x_;
28 |   int y_;
29 |   int z_;
30 |   int endX_;
31 |   int endY_;
32 |   int endZ_;
33 |   double maxDist_;
34 |   double dx_;
35 |   double dy_;
36 |   double dz_;
37 |   int stepX_;
38 |   int stepY_;
39 |   int stepZ_;
40 |   double tMaxX_;
41 |   double tMaxY_;
42 |   double tMaxZ_;
43 |   double tDeltaX_;
44 |   double tDeltaY_;
45 |   double tDeltaZ_;
46 |   double dist_;
47 | 
48 |   int step_num_;
49 | 
50 | public:
51 |   RayCaster(/* args */) {
52 |   }
53 |   ~RayCaster() {
54 |   }
55 | 
56 |   bool setInput(const Eigen::Vector3d& start,
57 |                 const Eigen::Vector3d& end /* , const Eigen::Vector3d& min,
58 |                 const Eigen::Vector3d& max */);
59 | 
60 |   bool step(Eigen::Vector3d& ray_pt);
61 | };
62 | 
63 | #endif  // RAYCAST_H_


--------------------------------------------------------------------------------
/launch/grid_map.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 | <node pkg="plan_env" name="gridmap_node" type="gridmap_node" output="screen">
 4 | 
 5 |     <remap from="~grid_map/odom" to="/vins_estimator/imu_propagate"/>
 6 |     <remap from="~grid_map/cloud" to="nouse2"/>
 7 |     <remap from="~grid_map/pose"   to = "nouse1"/> 
 8 |     <remap from="~grid_map/depth" to = "/camera/aligned_depth_to_color/image_raw"/>
 9 | 
10 |     <param name="grid_map/resolution"      value="0.15" /> 
11 |     <param name="grid_map/map_size_x"   value="50" /> 
12 |     <param name="grid_map/map_size_y"   value="25" /> 
13 |     <param name="grid_map/map_size_z"   value="3.0" /> 
14 | 
15 |     <param name="grid_map/local_update_range_x"  value="5.5" /> 
16 |     <param name="grid_map/local_update_range_y"  value="5.5" /> 
17 |     <param name="grid_map/local_update_range_z"  value="4.5" /> 
18 |     <param name="grid_map/obstacles_inflation"     value="0.299" /> 
19 |     <param name="grid_map/local_map_margin" value="10"/>
20 |     <param name="grid_map/ground_height"        value="-0.01"/>
21 |     <!-- camera parameter -->
22 | 
23 |     <param name="grid_map/cx" value="376.0"/>
24 |     <param name="grid_map/cy" value="240.0"/>
25 |     <param name="grid_map/fx" value="319.9988245765257"/>
26 |     <param name="grid_map/fy" value="319.9988245765257"/>
27 |     
28 |     <!-- depth filter -->
29 |     <param name="grid_map/use_depth_filter" value="true"/>
30 |     <param name="grid_map/depth_filter_tolerance" value="0.15"/>
31 |     <param name="grid_map/depth_filter_maxdist"   value="5.0"/>
32 |     <param name="grid_map/depth_filter_mindist"   value="0.2"/>
33 |     <param name="grid_map/depth_filter_margin"    value="2"/>
34 |     <param name="grid_map/k_depth_scaling_factor" value="1000.0"/>
35 |     <param name="grid_map/skip_pixel" value="2"/>
36 |     <!-- local fusion -->
37 |     <param name="grid_map/p_hit"  value="0.65"/>
38 |     <param name="grid_map/p_miss" value="0.35"/>
39 |     <param name="grid_map/p_min"  value="0.12"/>
40 |     <param name="grid_map/p_max"  value="0.90"/>
41 |     <param name="grid_map/p_occ"  value="0.80"/>
42 |     <param name="grid_map/min_ray_length" value="0.3"/>
43 |     <param name="grid_map/max_ray_length" value="5.0"/>
44 | 
45 |     <param name="grid_map/visualization_truncate_height"   value="1.8"/>
46 |     <param name="grid_map/show_occ_time"  value="false"/>
47 |     <param name="grid_map/pose_type"     value="2"/>  
48 |     <param name="grid_map/frame_id"      value="world"/>
49 | 	
50 |  </node>
51 | 
52 |     <node pkg="rviz" type="rviz" name="rviz" args="-d $(find plan_env)/launch/plan_env.rviz" required="true" />
53 | 
54 | </launch>
55 | 
56 | 


--------------------------------------------------------------------------------
/launch/plan_env.rviz:
--------------------------------------------------------------------------------
  1 | Panels:
  2 |   - Class: rviz/Displays
  3 |     Help Height: 78
  4 |     Name: Displays
  5 |     Property Tree Widget:
  6 |       Expanded:
  7 |         - /Global Options1
  8 |         - /Status1
  9 |         - /PointCloud21
 10 |         - /PointCloud21/Status1
 11 |       Splitter Ratio: 0.5
 12 |     Tree Height: 411
 13 |   - Class: rviz/Selection
 14 |     Name: Selection
 15 |   - Class: rviz/Tool Properties
 16 |     Expanded:
 17 |       - /2D Pose Estimate1
 18 |       - /2D Nav Goal1
 19 |       - /Publish Point1
 20 |     Name: Tool Properties
 21 |     Splitter Ratio: 0.5886790156364441
 22 |   - Class: rviz/Views
 23 |     Expanded:
 24 |       - /Current View1
 25 |     Name: Views
 26 |     Splitter Ratio: 0.5
 27 |   - Class: rviz/Time
 28 |     Experimental: false
 29 |     Name: Time
 30 |     SyncMode: 0
 31 |     SyncSource: PointCloud2
 32 | Preferences:
 33 |   PromptSaveOnExit: true
 34 | Toolbars:
 35 |   toolButtonStyle: 2
 36 | Visualization Manager:
 37 |   Class: ""
 38 |   Displays:
 39 |     - Alpha: 0.5
 40 |       Cell Size: 1
 41 |       Class: rviz/Grid
 42 |       Color: 160; 160; 164
 43 |       Enabled: true
 44 |       Line Style:
 45 |         Line Width: 0.029999999329447746
 46 |         Value: Lines
 47 |       Name: Grid
 48 |       Normal Cell Count: 0
 49 |       Offset:
 50 |         X: 0
 51 |         Y: 0
 52 |         Z: 0
 53 |       Plane: XY
 54 |       Plane Cell Count: 10
 55 |       Reference Frame: <Fixed Frame>
 56 |       Value: true
 57 |     - Alpha: 1
 58 |       Autocompute Intensity Bounds: true
 59 |       Autocompute Value Bounds:
 60 |         Max Value: 1.715000033378601
 61 |         Min Value: 0.06499999761581421
 62 |         Value: true
 63 |       Axis: Z
 64 |       Channel Name: intensity
 65 |       Class: rviz/PointCloud2
 66 |       Color: 255; 255; 255
 67 |       Color Transformer: AxisColor
 68 |       Decay Time: 0
 69 |       Enabled: true
 70 |       Invert Rainbow: false
 71 |       Max Color: 255; 255; 255
 72 |       Min Color: 0; 0; 0
 73 |       Name: PointCloud2
 74 |       Position Transformer: XYZ
 75 |       Queue Size: 10
 76 |       Selectable: true
 77 |       Size (Pixels): 3
 78 |       Size (m): 0.10000000149011612
 79 |       Style: Flat Squares
 80 |       Topic: /grid_map/occupancy_inflate
 81 |       Unreliable: false
 82 |       Use Fixed Frame: true
 83 |       Use rainbow: true
 84 |       Value: true
 85 |     - Class: rviz/Image
 86 |       Enabled: true
 87 |       Image Topic: /camera/aligned_depth_to_color/image_raw
 88 |       Max Value: 1
 89 |       Median window: 5
 90 |       Min Value: 0
 91 |       Name: Image
 92 |       Normalize Range: true
 93 |       Queue Size: 2
 94 |       Transport Hint: raw
 95 |       Unreliable: false
 96 |       Value: true
 97 |   Enabled: true
 98 |   Global Options:
 99 |     Background Color: 48; 48; 48
100 |     Default Light: true
101 |     Fixed Frame: world
102 |     Frame Rate: 30
103 |   Name: root
104 |   Tools:
105 |     - Class: rviz/Interact
106 |       Hide Inactive Objects: true
107 |     - Class: rviz/MoveCamera
108 |     - Class: rviz/Select
109 |     - Class: rviz/FocusCamera
110 |     - Class: rviz/Measure
111 |     - Class: rviz/SetInitialPose
112 |       Theta std deviation: 0.2617993950843811
113 |       Topic: /initialpose
114 |       X std deviation: 0.5
115 |       Y std deviation: 0.5
116 |     - Class: rviz/SetGoal
117 |       Topic: /move_base_simple/goal
118 |     - Class: rviz/PublishPoint
119 |       Single click: true
120 |       Topic: /clicked_point
121 |   Value: true
122 |   Views:
123 |     Current:
124 |       Class: rviz/Orbit
125 |       Distance: 19.19228744506836
126 |       Enable Stereo Rendering:
127 |         Stereo Eye Separation: 0.05999999865889549
128 |         Stereo Focal Distance: 1
129 |         Swap Stereo Eyes: false
130 |         Value: false
131 |       Focal Point:
132 |         X: 3.0705432891845703
133 |         Y: -1.90718674659729
134 |         Z: 2.3840394020080566
135 |       Focal Shape Fixed Size: true
136 |       Focal Shape Size: 0.05000000074505806
137 |       Invert Z Axis: false
138 |       Name: Current View
139 |       Near Clip Distance: 0.009999999776482582
140 |       Pitch: 0.6097968816757202
141 |       Target Frame: <Fixed Frame>
142 |       Value: Orbit (rviz)
143 |       Yaw: 2.420395612716675
144 |     Saved: ~
145 | Window Geometry:
146 |   Displays:
147 |     collapsed: false
148 |   Height: 1025
149 |   Hide Left Dock: false
150 |   Hide Right Dock: false
151 |   Image:
152 |     collapsed: false
153 |   QMainWindow State: 000000ff00000000fd00000004000000000000019200000363fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000226000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000269000001370000001600ffffff000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004900000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
154 |   Selection:
155 |     collapsed: false
156 |   Time:
157 |     collapsed: false
158 |   Tool Properties:
159 |     collapsed: false
160 |   Views:
161 |     collapsed: false
162 |   Width: 1853
163 |   X: 67
164 |   Y: 27
165 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>plan_env</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The plan_env package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag -->
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="iszhouxin@zju.edu.cn">iszhouxin</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>TODO</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but multiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/plan_env</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, multiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintainers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 | 
30 | 
31 |   <!-- The *depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
35 |   <!--   <depend>roscpp</depend> -->
36 |   <!--   Note that this is equivalent to the following: -->
37 |   <!--   <build_depend>roscpp</build_depend> -->
38 |   <!--   <exec_depend>roscpp</exec_depend> -->
39 |   <!-- Use build_depend for packages you need at compile time: -->
40 |   <!--   <build_depend>message_generation</build_depend> -->
41 |   <!-- Use build_export_depend for packages you need in order to build against this package: -->
42 |   <!--   <build_export_depend>message_generation</build_export_depend> -->
43 |   <!-- Use buildtool_depend for build tool packages: -->
44 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
45 |   <!-- Use exec_depend for packages you need at runtime: -->
46 |   <!--   <exec_depend>message_runtime</exec_depend> -->
47 |   <!-- Use test_depend for packages you need only for testing: -->
48 |   <!--   <test_depend>gtest</test_depend> -->
49 |   <!-- Use doc_depend for packages you need only for building documentation: -->
50 |   <!--   <doc_depend>doxygen</doc_depend> -->
51 |   <buildtool_depend>catkin</buildtool_depend>
52 |   <build_depend>roscpp</build_depend>
53 |   <build_depend>rospy</build_depend>
54 |   <build_depend>std_msgs</build_depend>
55 |   <build_export_depend>roscpp</build_export_depend>
56 |   <build_export_depend>rospy</build_export_depend>
57 |   <build_export_depend>std_msgs</build_export_depend>
58 |   <exec_depend>roscpp</exec_depend>
59 |   <exec_depend>rospy</exec_depend>
60 |   <exec_depend>std_msgs</exec_depend>
61 | 
62 | 
63 |   <!-- The export tag contains other, unspecified, tags -->
64 |   <export>
65 |     <!-- Other tools can request additional information be placed here -->
66 | 
67 |   </export>
68 | </package>
69 | 


--------------------------------------------------------------------------------
/readme.md:
--------------------------------------------------------------------------------
 1 | 
 2 | 把ego-planner的plan_env模块单独拿出来，可以单独作为功能包编译通过。  
 3 | 
 4 | 参考https://blog.csdn.net/chunchun2021/article/details/134535140 添加node.cpp和launch文件。  
 5 | 
 6 | 
 7 | 订阅位姿和深度图或者位姿和点云，发出的是融合后的地图PiontCloud2点云类型话题，包括/grid_map/occupancy_inflate话题和/grid_map/occupancy话题。  
 8 | 
 9 | 启动命令
10 | ```
11 | roslaunch plan_env grid_map.launch
12 | ```
13 | 可以配合densesurfelmapping提供的带有odom和深度图的surfel_color02.bag一起跑。  
14 | surfel_color02.bag官方下载地址：[surfel_color02.bag](https://hkustconnect-my.sharepoint.com/personal/cliuci_connect_ust_hk/_layouts/15/onedrive.aspx?id=%2Fpersonal%2Fcliuci%5Fconnect%5Fust%5Fhk%2FDocuments%2Fdataset%2Fsurfel%5Fcolor02%2Ebag%2Etar%2Egz&parent=%2Fpersonal%2Fcliuci%5Fconnect%5Fust%5Fhk%2FDocuments%2Fdataset&ga=1)  
15 | surfel_color02.bag有备份至csdn，可以这里下载：https://download.csdn.net/download/sinat_16643223/88979285  
16 | 
17 | 注意实际订阅的odom话题以及深度图话题需要在grid_map.cpp里面订阅话题的位置改，目前没有通过launch文件传参的形式，所以grid_map.launch文件里面设定的odom话题和深度图话名称是不生效的。  
18 | 比如订阅的是odometry类型的位姿话题和深度图话题时，修改grid_map.cpp里面下面两句话里订阅的话题名称为实际的深度图话题名称和位姿话题名称，修改完重新catkin_make编译生效。  
19 | 
20 | ```
21 | depth_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(node_, "/camera/aligned_depth_to_color/image_raw", 50));
22 | ```
23 | ```
24 | odom_sub_.reset(new message_filters::Subscriber<nav_msgs::Odometry>(node_, "/vins_estimator/imu_propagate", 100));
25 | ```
26 | 
27 | 订阅的位姿话题是odometry类型还是posestamped类型，由launch文件中pose_type参数决定，1为posestamped类型，2为odometry类型。  
28 | 


--------------------------------------------------------------------------------
/src/grid_map.cpp:
--------------------------------------------------------------------------------
   1 | #include "plan_env/grid_map.h"
   2 | 
   3 | // #define current_img_ md_.depth_image_[image_cnt_ & 1]
   4 | // #define last_img_ md_.depth_image_[!(image_cnt_ & 1)]
   5 | 
   6 | void GridMap::initMap(ros::NodeHandle &nh)
   7 | {
   8 |   node_ = nh;
   9 | 
  10 |   /* get parameter */
  11 |   double x_size, y_size, z_size;
  12 |   node_.param("grid_map/resolution", mp_.resolution_, -1.0);
  13 |   node_.param("grid_map/map_size_x", x_size, -1.0);
  14 |   node_.param("grid_map/map_size_y", y_size, -1.0);
  15 |   node_.param("grid_map/map_size_z", z_size, -1.0);
  16 |   node_.param("grid_map/local_update_range_x", mp_.local_update_range_(0), -1.0);
  17 |   node_.param("grid_map/local_update_range_y", mp_.local_update_range_(1), -1.0);
  18 |   node_.param("grid_map/local_update_range_z", mp_.local_update_range_(2), -1.0);
  19 |   node_.param("grid_map/obstacles_inflation", mp_.obstacles_inflation_, -1.0);
  20 | 
  21 |   node_.param("grid_map/fx", mp_.fx_, -1.0);
  22 |   node_.param("grid_map/fy", mp_.fy_, -1.0);
  23 |   node_.param("grid_map/cx", mp_.cx_, -1.0);
  24 |   node_.param("grid_map/cy", mp_.cy_, -1.0);
  25 | 
  26 |   node_.param("grid_map/use_depth_filter", mp_.use_depth_filter_, true);
  27 |   node_.param("grid_map/depth_filter_tolerance", mp_.depth_filter_tolerance_, -1.0);
  28 |   node_.param("grid_map/depth_filter_maxdist", mp_.depth_filter_maxdist_, -1.0);
  29 |   node_.param("grid_map/depth_filter_mindist", mp_.depth_filter_mindist_, -1.0);
  30 |   node_.param("grid_map/depth_filter_margin", mp_.depth_filter_margin_, -1);
  31 |   node_.param("grid_map/k_depth_scaling_factor", mp_.k_depth_scaling_factor_, -1.0);
  32 |   node_.param("grid_map/skip_pixel", mp_.skip_pixel_, -1);
  33 | 
  34 |   node_.param("grid_map/p_hit", mp_.p_hit_, 0.70);
  35 |   node_.param("grid_map/p_miss", mp_.p_miss_, 0.35);
  36 |   node_.param("grid_map/p_min", mp_.p_min_, 0.12);
  37 |   node_.param("grid_map/p_max", mp_.p_max_, 0.97);
  38 |   node_.param("grid_map/p_occ", mp_.p_occ_, 0.80);
  39 |   node_.param("grid_map/min_ray_length", mp_.min_ray_length_, -0.1);
  40 |   node_.param("grid_map/max_ray_length", mp_.max_ray_length_, -0.1);
  41 | 
  42 |   node_.param("grid_map/visualization_truncate_height", mp_.visualization_truncate_height_, 999.0);
  43 |   node_.param("grid_map/virtual_ceil_height", mp_.virtual_ceil_height_, -0.1);
  44 | 
  45 |   node_.param("grid_map/show_occ_time", mp_.show_occ_time_, false);
  46 |   node_.param("grid_map/pose_type", mp_.pose_type_, 1);
  47 | 
  48 |   node_.param("grid_map/frame_id", mp_.frame_id_, string("world"));
  49 |   node_.param("grid_map/local_map_margin", mp_.local_map_margin_, 1);
  50 |   node_.param("grid_map/ground_height", mp_.ground_height_, 1.0);
  51 | 
  52 |   mp_.resolution_inv_ = 1 / mp_.resolution_;
  53 |   mp_.map_origin_ = Eigen::Vector3d(-x_size / 2.0, -y_size / 2.0, mp_.ground_height_);
  54 |   mp_.map_size_ = Eigen::Vector3d(x_size, y_size, z_size);
  55 | 
  56 |   mp_.prob_hit_log_ = logit(mp_.p_hit_);
  57 |   mp_.prob_miss_log_ = logit(mp_.p_miss_);
  58 |   mp_.clamp_min_log_ = logit(mp_.p_min_);
  59 |   mp_.clamp_max_log_ = logit(mp_.p_max_);
  60 |   mp_.min_occupancy_log_ = logit(mp_.p_occ_);
  61 |   mp_.unknown_flag_ = 0.01;
  62 | 
  63 |   cout << "hit: " << mp_.prob_hit_log_ << endl;
  64 |   cout << "miss: " << mp_.prob_miss_log_ << endl;
  65 |   cout << "min log: " << mp_.clamp_min_log_ << endl;
  66 |   cout << "max: " << mp_.clamp_max_log_ << endl;
  67 |   cout << "thresh log: " << mp_.min_occupancy_log_ << endl;
  68 | 
  69 |   for (int i = 0; i < 3; ++i)
  70 |     mp_.map_voxel_num_(i) = ceil(mp_.map_size_(i) / mp_.resolution_);
  71 | 
  72 |   mp_.map_min_boundary_ = mp_.map_origin_;
  73 |   mp_.map_max_boundary_ = mp_.map_origin_ + mp_.map_size_;
  74 | 
  75 |   // initialize data buffers
  76 | 
  77 |   int buffer_size = mp_.map_voxel_num_(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2);
  78 | 
  79 |   md_.occupancy_buffer_ = vector<double>(buffer_size, mp_.clamp_min_log_ - mp_.unknown_flag_);
  80 |   md_.occupancy_buffer_inflate_ = vector<char>(buffer_size, 0);
  81 | 
  82 |   md_.count_hit_and_miss_ = vector<short>(buffer_size, 0);
  83 |   md_.count_hit_ = vector<short>(buffer_size, 0);
  84 |   md_.flag_rayend_ = vector<char>(buffer_size, -1);
  85 |   md_.flag_traverse_ = vector<char>(buffer_size, -1);
  86 | 
  87 |   md_.raycast_num_ = 0;
  88 | 
  89 |   md_.proj_points_.resize(640 * 480 / mp_.skip_pixel_ / mp_.skip_pixel_);
  90 |   md_.proj_points_cnt = 0;
  91 |   md_.cam2body_ << 0.0, 0.0, 1.0, 0.0,
  92 |       -1.0, 0.0, 0.0, 0.0,
  93 |       0.0, -1.0, 0.0, -0.02,
  94 |       0.0, 0.0, 0.0, 1.0;
  95 | 
  96 |   /* init callback */
  97 | 
  98 |   //depth_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(node_, "/grid_map/depth", 50));
  99 |   depth_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(node_, "/camera/aligned_depth_to_color/image_raw", 50));
 100 | 
 101 |   if (mp_.pose_type_ == POSE_STAMPED)
 102 |   {
 103 |     pose_sub_.reset(
 104 |         new message_filters::Subscriber<geometry_msgs::PoseStamped>(node_, "/grid_map/pose", 25));
 105 | 
 106 |     sync_image_pose_.reset(new message_filters::Synchronizer<SyncPolicyImagePose>(
 107 |         SyncPolicyImagePose(100), *depth_sub_, *pose_sub_));
 108 |     sync_image_pose_->registerCallback(boost::bind(&GridMap::depthPoseCallback, this, _1, _2));
 109 |   }
 110 |   else if (mp_.pose_type_ == ODOMETRY)
 111 |   {
 112 |     //odom_sub_.reset(new message_filters::Subscriber<nav_msgs::Odometry>(node_, "/grid_map/odom", 100));
 113 |     odom_sub_.reset(new message_filters::Subscriber<nav_msgs::Odometry>(node_, "/vins_estimator/imu_propagate", 100));
 114 | 
 115 |     sync_image_odom_.reset(new message_filters::Synchronizer<SyncPolicyImageOdom>(
 116 |         SyncPolicyImageOdom(100), *depth_sub_, *odom_sub_));
 117 |     sync_image_odom_->registerCallback(boost::bind(&GridMap::depthOdomCallback, this, _1, _2));
 118 |   }
 119 | 
 120 |   // use odometry and point cloud
 121 |   indep_cloud_sub_ =
 122 |       node_.subscribe<sensor_msgs::PointCloud2>("/grid_map/cloud", 10, &GridMap::cloudCallback, this);
 123 |   indep_odom_sub_ =
 124 |       node_.subscribe<nav_msgs::Odometry>("/grid_map/odom", 10, &GridMap::odomCallback, this);
 125 | 
 126 |   occ_timer_ = node_.createTimer(ros::Duration(0.05), &GridMap::updateOccupancyCallback, this);
 127 |   vis_timer_ = node_.createTimer(ros::Duration(0.05), &GridMap::visCallback, this);
 128 | 
 129 |   map_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/grid_map/occupancy", 10);
 130 |   map_inf_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/grid_map/occupancy_inflate", 10);
 131 | 
 132 |   unknown_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/grid_map/unknown", 10);
 133 | 
 134 |   md_.occ_need_update_ = false;
 135 |   md_.local_updated_ = false;
 136 |   md_.has_first_depth_ = false;
 137 |   md_.has_odom_ = false;
 138 |   md_.has_cloud_ = false;
 139 |   md_.image_cnt_ = 0;
 140 | 
 141 |   md_.fuse_time_ = 0.0;
 142 |   md_.update_num_ = 0;
 143 |   md_.max_fuse_time_ = 0.0;
 144 | 
 145 |   // rand_noise_ = uniform_real_distribution<double>(-0.2, 0.2);
 146 |   // rand_noise2_ = normal_distribution<double>(0, 0.2);
 147 |   // random_device rd;
 148 |   // eng_ = default_random_engine(rd());
 149 | }
 150 | 
 151 | void GridMap::resetBuffer()
 152 | {
 153 |   Eigen::Vector3d min_pos = mp_.map_min_boundary_;
 154 |   Eigen::Vector3d max_pos = mp_.map_max_boundary_;
 155 | 
 156 |   resetBuffer(min_pos, max_pos);
 157 | 
 158 |   md_.local_bound_min_ = Eigen::Vector3i::Zero();
 159 |   md_.local_bound_max_ = mp_.map_voxel_num_ - Eigen::Vector3i::Ones();
 160 | }
 161 | 
 162 | void GridMap::resetBuffer(Eigen::Vector3d min_pos, Eigen::Vector3d max_pos)
 163 | {
 164 | 
 165 |   Eigen::Vector3i min_id, max_id;
 166 |   posToIndex(min_pos, min_id);
 167 |   posToIndex(max_pos, max_id);
 168 | 
 169 |   boundIndex(min_id);
 170 |   boundIndex(max_id);
 171 | 
 172 |   /* reset occ and dist buffer */
 173 |   for (int x = min_id(0); x <= max_id(0); ++x)
 174 |     for (int y = min_id(1); y <= max_id(1); ++y)
 175 |       for (int z = min_id(2); z <= max_id(2); ++z)
 176 |       {
 177 |         md_.occupancy_buffer_inflate_[toAddress(x, y, z)] = 0;
 178 |       }
 179 | }
 180 | 
 181 | int GridMap::setCacheOccupancy(Eigen::Vector3d pos, int occ)
 182 | {
 183 |   if (occ != 1 && occ != 0)
 184 |     return INVALID_IDX;
 185 | 
 186 |   Eigen::Vector3i id;
 187 |   posToIndex(pos, id);
 188 |   int idx_ctns = toAddress(id);
 189 | 
 190 |   md_.count_hit_and_miss_[idx_ctns] += 1;
 191 | 
 192 |   if (md_.count_hit_and_miss_[idx_ctns] == 1)
 193 |   {
 194 |     md_.cache_voxel_.push(id);
 195 |   }
 196 | 
 197 |   if (occ == 1)
 198 |     md_.count_hit_[idx_ctns] += 1;
 199 | 
 200 |   return idx_ctns;
 201 | }
 202 | 
 203 | void GridMap::projectDepthImage()
 204 | {
 205 |   // md_.proj_points_.clear();
 206 |   md_.proj_points_cnt = 0;
 207 | 
 208 |   uint16_t *row_ptr;
 209 |   // int cols = current_img_.cols, rows = current_img_.rows;
 210 |   int cols = md_.depth_image_.cols;
 211 |   int rows = md_.depth_image_.rows;
 212 | 
 213 |   double depth;
 214 | 
 215 |   Eigen::Matrix3d camera_r = md_.camera_q_.toRotationMatrix();
 216 | 
 217 |   // cout << "rotate: " << md_.camera_q_.toRotationMatrix() << endl;
 218 |   // std::cout << "pos in proj: " << md_.camera_pos_ << std::endl;
 219 | 
 220 |   if (!mp_.use_depth_filter_)
 221 |   {
 222 |     for (int v = 0; v < rows; v++)
 223 |     {
 224 |       row_ptr = md_.depth_image_.ptr<uint16_t>(v);
 225 | 
 226 |       for (int u = 0; u < cols; u++)
 227 |       {
 228 | 
 229 |         Eigen::Vector3d proj_pt;
 230 |         depth = (*row_ptr++) / mp_.k_depth_scaling_factor_;
 231 |         proj_pt(0) = (u - mp_.cx_) * depth / mp_.fx_;
 232 |         proj_pt(1) = (v - mp_.cy_) * depth / mp_.fy_;
 233 |         proj_pt(2) = depth;
 234 | 
 235 |         proj_pt = camera_r * proj_pt + md_.camera_pos_;
 236 | 
 237 |         if (u == 320 && v == 240)
 238 |           std::cout << "depth: " << depth << std::endl;
 239 |         md_.proj_points_[md_.proj_points_cnt++] = proj_pt;
 240 |       }
 241 |     }
 242 |   }
 243 |   /* use depth filter */
 244 |   else
 245 |   {
 246 | 
 247 |     if (!md_.has_first_depth_)
 248 |       md_.has_first_depth_ = true;
 249 |     else
 250 |     {
 251 |       Eigen::Vector3d pt_cur, pt_world, pt_reproj;
 252 | 
 253 |       Eigen::Matrix3d last_camera_r_inv;
 254 |       last_camera_r_inv = md_.last_camera_q_.inverse();
 255 |       const double inv_factor = 1.0 / mp_.k_depth_scaling_factor_;
 256 | 
 257 |       for (int v = mp_.depth_filter_margin_; v < rows - mp_.depth_filter_margin_; v += mp_.skip_pixel_)
 258 |       {
 259 |         row_ptr = md_.depth_image_.ptr<uint16_t>(v) + mp_.depth_filter_margin_;
 260 | 
 261 |         for (int u = mp_.depth_filter_margin_; u < cols - mp_.depth_filter_margin_;
 262 |              u += mp_.skip_pixel_)
 263 |         {
 264 | 
 265 |           depth = (*row_ptr) * inv_factor;
 266 |           row_ptr = row_ptr + mp_.skip_pixel_;
 267 | 
 268 |           // filter depth
 269 |           // depth += rand_noise_(eng_);
 270 |           // if (depth > 0.01) depth += rand_noise2_(eng_);
 271 | 
 272 |           if (*row_ptr == 0)
 273 |           {
 274 |             depth = mp_.max_ray_length_ + 0.1;
 275 |           }
 276 |           else if (depth < mp_.depth_filter_mindist_)
 277 |           {
 278 |             continue;
 279 |           }
 280 |           else if (depth > mp_.depth_filter_maxdist_)
 281 |           {
 282 |             depth = mp_.max_ray_length_ + 0.1;
 283 |           }
 284 | 
 285 |           // project to world frame
 286 |           pt_cur(0) = (u - mp_.cx_) * depth / mp_.fx_;
 287 |           pt_cur(1) = (v - mp_.cy_) * depth / mp_.fy_;
 288 |           pt_cur(2) = depth;
 289 | 
 290 |           pt_world = camera_r * pt_cur + md_.camera_pos_;
 291 |           // if (!isInMap(pt_world)) {
 292 |           //   pt_world = closetPointInMap(pt_world, md_.camera_pos_);
 293 |           // }
 294 | 
 295 |           md_.proj_points_[md_.proj_points_cnt++] = pt_world;
 296 | 
 297 |           // check consistency with last image, disabled...
 298 |           if (false)
 299 |           {
 300 |             pt_reproj = last_camera_r_inv * (pt_world - md_.last_camera_pos_);
 301 |             double uu = pt_reproj.x() * mp_.fx_ / pt_reproj.z() + mp_.cx_;
 302 |             double vv = pt_reproj.y() * mp_.fy_ / pt_reproj.z() + mp_.cy_;
 303 | 
 304 |             if (uu >= 0 && uu < cols && vv >= 0 && vv < rows)
 305 |             {
 306 |               if (fabs(md_.last_depth_image_.at<uint16_t>((int)vv, (int)uu) * inv_factor -
 307 |                        pt_reproj.z()) < mp_.depth_filter_tolerance_)
 308 |               {
 309 |                 md_.proj_points_[md_.proj_points_cnt++] = pt_world;
 310 |               }
 311 |             }
 312 |             else
 313 |             {
 314 |               md_.proj_points_[md_.proj_points_cnt++] = pt_world;
 315 |             }
 316 |           }
 317 |         }
 318 |       }
 319 |     }
 320 |   }
 321 | 
 322 |   /* maintain camera pose for consistency check */
 323 | 
 324 |   md_.last_camera_pos_ = md_.camera_pos_;
 325 |   md_.last_camera_q_ = md_.camera_q_;
 326 |   md_.last_depth_image_ = md_.depth_image_;
 327 | }
 328 | 
 329 | void GridMap::raycastProcess()
 330 | {
 331 |   // if (md_.proj_points_.size() == 0)
 332 |   if (md_.proj_points_cnt == 0)
 333 |     return;
 334 | 
 335 |   ros::Time t1, t2;
 336 | 
 337 |   md_.raycast_num_ += 1;
 338 | 
 339 |   int vox_idx;
 340 |   double length;
 341 | 
 342 |   // bounding box of updated region
 343 |   double min_x = mp_.map_max_boundary_(0);
 344 |   double min_y = mp_.map_max_boundary_(1);
 345 |   double min_z = mp_.map_max_boundary_(2);
 346 | 
 347 |   double max_x = mp_.map_min_boundary_(0);
 348 |   double max_y = mp_.map_min_boundary_(1);
 349 |   double max_z = mp_.map_min_boundary_(2);
 350 | 
 351 |   RayCaster raycaster;
 352 |   Eigen::Vector3d half = Eigen::Vector3d(0.5, 0.5, 0.5);
 353 |   Eigen::Vector3d ray_pt, pt_w;
 354 | 
 355 |   for (int i = 0; i < md_.proj_points_cnt; ++i)
 356 |   {
 357 |     pt_w = md_.proj_points_[i];
 358 | 
 359 |     // set flag for projected point
 360 | 
 361 |     if (!isInMap(pt_w))
 362 |     {
 363 |       pt_w = closetPointInMap(pt_w, md_.camera_pos_);
 364 | 
 365 |       length = (pt_w - md_.camera_pos_).norm();
 366 |       if (length > mp_.max_ray_length_)
 367 |       {
 368 |         pt_w = (pt_w - md_.camera_pos_) / length * mp_.max_ray_length_ + md_.camera_pos_;
 369 |       }
 370 |       vox_idx = setCacheOccupancy(pt_w, 0);
 371 |     }
 372 |     else
 373 |     {
 374 |       length = (pt_w - md_.camera_pos_).norm();
 375 | 
 376 |       if (length > mp_.max_ray_length_)
 377 |       {
 378 |         pt_w = (pt_w - md_.camera_pos_) / length * mp_.max_ray_length_ + md_.camera_pos_;
 379 |         vox_idx = setCacheOccupancy(pt_w, 0);
 380 |       }
 381 |       else
 382 |       {
 383 |         vox_idx = setCacheOccupancy(pt_w, 1);
 384 |       }
 385 |     }
 386 | 
 387 |     max_x = max(max_x, pt_w(0));
 388 |     max_y = max(max_y, pt_w(1));
 389 |     max_z = max(max_z, pt_w(2));
 390 | 
 391 |     min_x = min(min_x, pt_w(0));
 392 |     min_y = min(min_y, pt_w(1));
 393 |     min_z = min(min_z, pt_w(2));
 394 | 
 395 |     // raycasting between camera center and point
 396 | 
 397 |     if (vox_idx != INVALID_IDX)
 398 |     {
 399 |       if (md_.flag_rayend_[vox_idx] == md_.raycast_num_)
 400 |       {
 401 |         continue;
 402 |       }
 403 |       else
 404 |       {
 405 |         md_.flag_rayend_[vox_idx] = md_.raycast_num_;
 406 |       }
 407 |     }
 408 | 
 409 |     raycaster.setInput(pt_w / mp_.resolution_, md_.camera_pos_ / mp_.resolution_);
 410 | 
 411 |     while (raycaster.step(ray_pt))
 412 |     {
 413 |       Eigen::Vector3d tmp = (ray_pt + half) * mp_.resolution_;
 414 |       length = (tmp - md_.camera_pos_).norm();
 415 | 
 416 |       // if (length < mp_.min_ray_length_) break;
 417 | 
 418 |       vox_idx = setCacheOccupancy(tmp, 0);
 419 | 
 420 |       if (vox_idx != INVALID_IDX)
 421 |       {
 422 |         if (md_.flag_traverse_[vox_idx] == md_.raycast_num_)
 423 |         {
 424 |           break;
 425 |         }
 426 |         else
 427 |         {
 428 |           md_.flag_traverse_[vox_idx] = md_.raycast_num_;
 429 |         }
 430 |       }
 431 |     }
 432 |   }
 433 | 
 434 |   min_x = min(min_x, md_.camera_pos_(0));
 435 |   min_y = min(min_y, md_.camera_pos_(1));
 436 |   min_z = min(min_z, md_.camera_pos_(2));
 437 | 
 438 |   max_x = max(max_x, md_.camera_pos_(0));
 439 |   max_y = max(max_y, md_.camera_pos_(1));
 440 |   max_z = max(max_z, md_.camera_pos_(2));
 441 |   max_z = max(max_z, mp_.ground_height_);
 442 | 
 443 |   posToIndex(Eigen::Vector3d(max_x, max_y, max_z), md_.local_bound_max_);
 444 |   posToIndex(Eigen::Vector3d(min_x, min_y, min_z), md_.local_bound_min_);
 445 |   boundIndex(md_.local_bound_min_);
 446 |   boundIndex(md_.local_bound_max_);
 447 | 
 448 |   md_.local_updated_ = true;
 449 | 
 450 |   // update occupancy cached in queue
 451 |   Eigen::Vector3d local_range_min = md_.camera_pos_ - mp_.local_update_range_;
 452 |   Eigen::Vector3d local_range_max = md_.camera_pos_ + mp_.local_update_range_;
 453 | 
 454 |   Eigen::Vector3i min_id, max_id;
 455 |   posToIndex(local_range_min, min_id);
 456 |   posToIndex(local_range_max, max_id);
 457 |   boundIndex(min_id);
 458 |   boundIndex(max_id);
 459 | 
 460 |   // std::cout << "cache all: " << md_.cache_voxel_.size() << std::endl;
 461 | 
 462 |   while (!md_.cache_voxel_.empty())
 463 |   {
 464 | 
 465 |     Eigen::Vector3i idx = md_.cache_voxel_.front();
 466 |     int idx_ctns = toAddress(idx);
 467 |     md_.cache_voxel_.pop();
 468 | 
 469 |     double log_odds_update =
 470 |         md_.count_hit_[idx_ctns] >= md_.count_hit_and_miss_[idx_ctns] - md_.count_hit_[idx_ctns] ? mp_.prob_hit_log_ : mp_.prob_miss_log_;
 471 | 
 472 |     md_.count_hit_[idx_ctns] = md_.count_hit_and_miss_[idx_ctns] = 0;
 473 | 
 474 |     if (log_odds_update >= 0 && md_.occupancy_buffer_[idx_ctns] >= mp_.clamp_max_log_)
 475 |     {
 476 |       continue;
 477 |     }
 478 |     else if (log_odds_update <= 0 && md_.occupancy_buffer_[idx_ctns] <= mp_.clamp_min_log_)
 479 |     {
 480 |       md_.occupancy_buffer_[idx_ctns] = mp_.clamp_min_log_;
 481 |       continue;
 482 |     }
 483 | 
 484 |     bool in_local = idx(0) >= min_id(0) && idx(0) <= max_id(0) && idx(1) >= min_id(1) &&
 485 |                     idx(1) <= max_id(1) && idx(2) >= min_id(2) && idx(2) <= max_id(2);
 486 |     if (!in_local)
 487 |     {
 488 |       md_.occupancy_buffer_[idx_ctns] = mp_.clamp_min_log_;
 489 |     }
 490 | 
 491 |     md_.occupancy_buffer_[idx_ctns] =
 492 |         std::min(std::max(md_.occupancy_buffer_[idx_ctns] + log_odds_update, mp_.clamp_min_log_),
 493 |                  mp_.clamp_max_log_);
 494 |   }
 495 | }
 496 | 
 497 | Eigen::Vector3d GridMap::closetPointInMap(const Eigen::Vector3d &pt, const Eigen::Vector3d &camera_pt)
 498 | {
 499 |   Eigen::Vector3d diff = pt - camera_pt;
 500 |   Eigen::Vector3d max_tc = mp_.map_max_boundary_ - camera_pt;
 501 |   Eigen::Vector3d min_tc = mp_.map_min_boundary_ - camera_pt;
 502 | 
 503 |   double min_t = 1000000;
 504 | 
 505 |   for (int i = 0; i < 3; ++i)
 506 |   {
 507 |     if (fabs(diff[i]) > 0)
 508 |     {
 509 | 
 510 |       double t1 = max_tc[i] / diff[i];
 511 |       if (t1 > 0 && t1 < min_t)
 512 |         min_t = t1;
 513 | 
 514 |       double t2 = min_tc[i] / diff[i];
 515 |       if (t2 > 0 && t2 < min_t)
 516 |         min_t = t2;
 517 |     }
 518 |   }
 519 | 
 520 |   return camera_pt + (min_t - 1e-3) * diff;
 521 | }
 522 | 
 523 | void GridMap::clearAndInflateLocalMap()
 524 | {
 525 |   /*clear outside local*/
 526 |   const int vec_margin = 5;
 527 |   // Eigen::Vector3i min_vec_margin = min_vec - Eigen::Vector3i(vec_margin,
 528 |   // vec_margin, vec_margin); Eigen::Vector3i max_vec_margin = max_vec +
 529 |   // Eigen::Vector3i(vec_margin, vec_margin, vec_margin);
 530 | 
 531 |   Eigen::Vector3i min_cut = md_.local_bound_min_ -
 532 |                             Eigen::Vector3i(mp_.local_map_margin_, mp_.local_map_margin_, mp_.local_map_margin_);
 533 |   Eigen::Vector3i max_cut = md_.local_bound_max_ +
 534 |                             Eigen::Vector3i(mp_.local_map_margin_, mp_.local_map_margin_, mp_.local_map_margin_);
 535 |   boundIndex(min_cut);
 536 |   boundIndex(max_cut);
 537 | 
 538 |   Eigen::Vector3i min_cut_m = min_cut - Eigen::Vector3i(vec_margin, vec_margin, vec_margin);
 539 |   Eigen::Vector3i max_cut_m = max_cut + Eigen::Vector3i(vec_margin, vec_margin, vec_margin);
 540 |   boundIndex(min_cut_m);
 541 |   boundIndex(max_cut_m);
 542 | 
 543 |   // clear data outside the local range
 544 | 
 545 |   for (int x = min_cut_m(0); x <= max_cut_m(0); ++x)
 546 |     for (int y = min_cut_m(1); y <= max_cut_m(1); ++y)
 547 |     {
 548 | 
 549 |       for (int z = min_cut_m(2); z < min_cut(2); ++z)
 550 |       {
 551 |         int idx = toAddress(x, y, z);
 552 |         md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_;
 553 |       }
 554 | 
 555 |       for (int z = max_cut(2) + 1; z <= max_cut_m(2); ++z)
 556 |       {
 557 |         int idx = toAddress(x, y, z);
 558 |         md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_;
 559 |       }
 560 |     }
 561 | 
 562 |   for (int z = min_cut_m(2); z <= max_cut_m(2); ++z)
 563 |     for (int x = min_cut_m(0); x <= max_cut_m(0); ++x)
 564 |     {
 565 | 
 566 |       for (int y = min_cut_m(1); y < min_cut(1); ++y)
 567 |       {
 568 |         int idx = toAddress(x, y, z);
 569 |         md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_;
 570 |       }
 571 | 
 572 |       for (int y = max_cut(1) + 1; y <= max_cut_m(1); ++y)
 573 |       {
 574 |         int idx = toAddress(x, y, z);
 575 |         md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_;
 576 |       }
 577 |     }
 578 | 
 579 |   for (int y = min_cut_m(1); y <= max_cut_m(1); ++y)
 580 |     for (int z = min_cut_m(2); z <= max_cut_m(2); ++z)
 581 |     {
 582 | 
 583 |       for (int x = min_cut_m(0); x < min_cut(0); ++x)
 584 |       {
 585 |         int idx = toAddress(x, y, z);
 586 |         md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_;
 587 |       }
 588 | 
 589 |       for (int x = max_cut(0) + 1; x <= max_cut_m(0); ++x)
 590 |       {
 591 |         int idx = toAddress(x, y, z);
 592 |         md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_;
 593 |       }
 594 |     }
 595 | 
 596 |   // inflate occupied voxels to compensate robot size
 597 | 
 598 |   int inf_step = ceil(mp_.obstacles_inflation_ / mp_.resolution_);
 599 |   // int inf_step_z = 1;
 600 |   vector<Eigen::Vector3i> inf_pts(pow(2 * inf_step + 1, 3));
 601 |   // inf_pts.resize(4 * inf_step + 3);
 602 |   Eigen::Vector3i inf_pt;
 603 | 
 604 |   // clear outdated data
 605 |   for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x)
 606 |     for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y)
 607 |       for (int z = md_.local_bound_min_(2); z <= md_.local_bound_max_(2); ++z)
 608 |       {
 609 |         md_.occupancy_buffer_inflate_[toAddress(x, y, z)] = 0;
 610 |       }
 611 | 
 612 |   // inflate obstacles
 613 |   for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x)
 614 |     for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y)
 615 |       for (int z = md_.local_bound_min_(2); z <= md_.local_bound_max_(2); ++z)
 616 |       {
 617 | 
 618 |         if (md_.occupancy_buffer_[toAddress(x, y, z)] > mp_.min_occupancy_log_)
 619 |         {
 620 |           inflatePoint(Eigen::Vector3i(x, y, z), inf_step, inf_pts);
 621 | 
 622 |           for (int k = 0; k < (int)inf_pts.size(); ++k)
 623 |           {
 624 |             inf_pt = inf_pts[k];
 625 |             int idx_inf = toAddress(inf_pt);
 626 |             if (idx_inf < 0 ||
 627 |                 idx_inf >= mp_.map_voxel_num_(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2))
 628 |             {
 629 |               continue;
 630 |             }
 631 |             md_.occupancy_buffer_inflate_[idx_inf] = 1;
 632 |           }
 633 |         }
 634 |       }
 635 | 
 636 |   // add virtual ceiling to limit flight height
 637 |   if (mp_.virtual_ceil_height_ > -0.5)
 638 |   {
 639 |     int ceil_id = floor((mp_.virtual_ceil_height_ - mp_.map_origin_(2)) * mp_.resolution_inv_);
 640 |     for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x)
 641 |       for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y)
 642 |       {
 643 |         md_.occupancy_buffer_inflate_[toAddress(x, y, ceil_id)] = 1;
 644 |       }
 645 |   }
 646 | }
 647 | 
 648 | void GridMap::visCallback(const ros::TimerEvent & /*event*/)
 649 | {
 650 | 
 651 |   publishMap();
 652 |   publishMapInflate(true);
 653 | }
 654 | 
 655 | void GridMap::updateOccupancyCallback(const ros::TimerEvent & /*event*/)
 656 | {
 657 |   if (!md_.occ_need_update_)
 658 |     return;
 659 | 
 660 |   /* update occupancy */
 661 |   // ros::Time t1, t2, t3, t4;
 662 |   // t1 = ros::Time::now();
 663 | 
 664 |   projectDepthImage();
 665 |   // t2 = ros::Time::now();
 666 |   raycastProcess();
 667 |   // t3 = ros::Time::now();
 668 | 
 669 |   if (md_.local_updated_)
 670 |     clearAndInflateLocalMap();
 671 | 
 672 |   // t4 = ros::Time::now();
 673 | 
 674 |   // cout << setprecision(7);
 675 |   // cout << "t2=" << (t2-t1).toSec() << " t3=" << (t3-t2).toSec() << " t4=" << (t4-t3).toSec() << endl;;
 676 | 
 677 |   // md_.fuse_time_ += (t2 - t1).toSec();
 678 |   // md_.max_fuse_time_ = max(md_.max_fuse_time_, (t2 - t1).toSec());
 679 | 
 680 |   // if (mp_.show_occ_time_)
 681 |   //   ROS_WARN("Fusion: cur t = %lf, avg t = %lf, max t = %lf", (t2 - t1).toSec(),
 682 |   //            md_.fuse_time_ / md_.update_num_, md_.max_fuse_time_);
 683 | 
 684 |   md_.occ_need_update_ = false;
 685 |   md_.local_updated_ = false;
 686 | }
 687 | 
 688 | void GridMap::depthPoseCallback(const sensor_msgs::ImageConstPtr &img,
 689 |                                 const geometry_msgs::PoseStampedConstPtr &pose)
 690 | {
 691 |   /* get depth image */
 692 |   cv_bridge::CvImagePtr cv_ptr;
 693 |   cv_ptr = cv_bridge::toCvCopy(img, img->encoding);
 694 | 
 695 |   if (img->encoding == sensor_msgs::image_encodings::TYPE_32FC1)
 696 |   {
 697 |     (cv_ptr->image).convertTo(cv_ptr->image, CV_16UC1, mp_.k_depth_scaling_factor_);
 698 |   }
 699 |   cv_ptr->image.copyTo(md_.depth_image_);
 700 | 
 701 |   // std::cout << "depth: " << md_.depth_image_.cols << ", " << md_.depth_image_.rows << std::endl;
 702 | 
 703 |   /* get pose */
 704 |   md_.camera_pos_(0) = pose->pose.position.x;
 705 |   md_.camera_pos_(1) = pose->pose.position.y;
 706 |   md_.camera_pos_(2) = pose->pose.position.z;
 707 |   md_.camera_q_ = Eigen::Quaterniond(pose->pose.orientation.w, pose->pose.orientation.x,
 708 |                                      pose->pose.orientation.y, pose->pose.orientation.z);
 709 |   if (isInMap(md_.camera_pos_))
 710 |   {
 711 |     md_.has_odom_ = true;
 712 |     md_.update_num_ += 1;
 713 |     md_.occ_need_update_ = true;
 714 |   }
 715 |   else
 716 |   {
 717 |     md_.occ_need_update_ = false;
 718 |   }
 719 | }
 720 | void GridMap::odomCallback(const nav_msgs::OdometryConstPtr &odom)
 721 | {
 722 |   if (md_.has_first_depth_)
 723 |     return;
 724 | 
 725 |   md_.camera_pos_(0) = odom->pose.pose.position.x;
 726 |   md_.camera_pos_(1) = odom->pose.pose.position.y;
 727 |   md_.camera_pos_(2) = odom->pose.pose.position.z;
 728 | 
 729 |   md_.has_odom_ = true;
 730 | }
 731 | 
 732 | void GridMap::cloudCallback(const sensor_msgs::PointCloud2ConstPtr &img)
 733 | {
 734 | 
 735 |   pcl::PointCloud<pcl::PointXYZ> latest_cloud;
 736 |   pcl::fromROSMsg(*img, latest_cloud);
 737 | 
 738 |   md_.has_cloud_ = true;
 739 | 
 740 |   if (!md_.has_odom_)
 741 |   {
 742 |     std::cout << "no odom!" << std::endl;
 743 |     return;
 744 |   }
 745 | 
 746 |   if (latest_cloud.points.size() == 0)
 747 |     return;
 748 | 
 749 |   if (isnan(md_.camera_pos_(0)) || isnan(md_.camera_pos_(1)) || isnan(md_.camera_pos_(2)))
 750 |     return;
 751 | 
 752 |   this->resetBuffer(md_.camera_pos_ - mp_.local_update_range_,
 753 |                     md_.camera_pos_ + mp_.local_update_range_);
 754 | 
 755 |   pcl::PointXYZ pt;
 756 |   Eigen::Vector3d p3d, p3d_inf;
 757 | 
 758 |   int inf_step = ceil(mp_.obstacles_inflation_ / mp_.resolution_);
 759 |   int inf_step_z = 1;
 760 | 
 761 |   double max_x, max_y, max_z, min_x, min_y, min_z;
 762 | 
 763 |   min_x = mp_.map_max_boundary_(0);
 764 |   min_y = mp_.map_max_boundary_(1);
 765 |   min_z = mp_.map_max_boundary_(2);
 766 | 
 767 |   max_x = mp_.map_min_boundary_(0);
 768 |   max_y = mp_.map_min_boundary_(1);
 769 |   max_z = mp_.map_min_boundary_(2);
 770 | 
 771 |   for (size_t i = 0; i < latest_cloud.points.size(); ++i)
 772 |   {
 773 |     pt = latest_cloud.points[i];
 774 |     p3d(0) = pt.x, p3d(1) = pt.y, p3d(2) = pt.z;
 775 | 
 776 |     /* point inside update range */
 777 |     Eigen::Vector3d devi = p3d - md_.camera_pos_;
 778 |     Eigen::Vector3i inf_pt;
 779 | 
 780 |     if (fabs(devi(0)) < mp_.local_update_range_(0) && fabs(devi(1)) < mp_.local_update_range_(1) &&
 781 |         fabs(devi(2)) < mp_.local_update_range_(2))
 782 |     {
 783 | 
 784 |       /* inflate the point */
 785 |       for (int x = -inf_step; x <= inf_step; ++x)
 786 |         for (int y = -inf_step; y <= inf_step; ++y)
 787 |           for (int z = -inf_step_z; z <= inf_step_z; ++z)
 788 |           {
 789 | 
 790 |             p3d_inf(0) = pt.x + x * mp_.resolution_;
 791 |             p3d_inf(1) = pt.y + y * mp_.resolution_;
 792 |             p3d_inf(2) = pt.z + z * mp_.resolution_;
 793 | 
 794 |             max_x = max(max_x, p3d_inf(0));
 795 |             max_y = max(max_y, p3d_inf(1));
 796 |             max_z = max(max_z, p3d_inf(2));
 797 | 
 798 |             min_x = min(min_x, p3d_inf(0));
 799 |             min_y = min(min_y, p3d_inf(1));
 800 |             min_z = min(min_z, p3d_inf(2));
 801 | 
 802 |             posToIndex(p3d_inf, inf_pt);
 803 | 
 804 |             if (!isInMap(inf_pt))
 805 |               continue;
 806 | 
 807 |             int idx_inf = toAddress(inf_pt);
 808 | 
 809 |             md_.occupancy_buffer_inflate_[idx_inf] = 1;
 810 |           }
 811 |     }
 812 |   }
 813 | 
 814 |   min_x = min(min_x, md_.camera_pos_(0));
 815 |   min_y = min(min_y, md_.camera_pos_(1));
 816 |   min_z = min(min_z, md_.camera_pos_(2));
 817 | 
 818 |   max_x = max(max_x, md_.camera_pos_(0));
 819 |   max_y = max(max_y, md_.camera_pos_(1));
 820 |   max_z = max(max_z, md_.camera_pos_(2));
 821 | 
 822 |   max_z = max(max_z, mp_.ground_height_);
 823 | 
 824 |   posToIndex(Eigen::Vector3d(max_x, max_y, max_z), md_.local_bound_max_);
 825 |   posToIndex(Eigen::Vector3d(min_x, min_y, min_z), md_.local_bound_min_);
 826 | 
 827 |   boundIndex(md_.local_bound_min_);
 828 |   boundIndex(md_.local_bound_max_);
 829 | }
 830 | 
 831 | void GridMap::publishMap()
 832 | {
 833 | 
 834 |   if (map_pub_.getNumSubscribers() <= 0)
 835 |     return;
 836 | 
 837 |   pcl::PointXYZ pt;
 838 |   pcl::PointCloud<pcl::PointXYZ> cloud;
 839 | 
 840 |   Eigen::Vector3i min_cut = md_.local_bound_min_;
 841 |   Eigen::Vector3i max_cut = md_.local_bound_max_;
 842 | 
 843 |   int lmm = mp_.local_map_margin_ / 2;
 844 |   min_cut -= Eigen::Vector3i(lmm, lmm, lmm);
 845 |   max_cut += Eigen::Vector3i(lmm, lmm, lmm);
 846 | 
 847 |   boundIndex(min_cut);
 848 |   boundIndex(max_cut);
 849 | 
 850 |   for (int x = min_cut(0); x <= max_cut(0); ++x)
 851 |     for (int y = min_cut(1); y <= max_cut(1); ++y)
 852 |       for (int z = min_cut(2); z <= max_cut(2); ++z)
 853 |       {
 854 |         if (md_.occupancy_buffer_[toAddress(x, y, z)] < mp_.min_occupancy_log_)
 855 |           continue;
 856 | 
 857 |         Eigen::Vector3d pos;
 858 |         indexToPos(Eigen::Vector3i(x, y, z), pos);
 859 |         if (pos(2) > mp_.visualization_truncate_height_)
 860 |           continue;
 861 |         pt.x = pos(0);
 862 |         pt.y = pos(1);
 863 |         pt.z = pos(2);
 864 |         cloud.push_back(pt);
 865 |       }
 866 | 
 867 |   cloud.width = cloud.points.size();
 868 |   cloud.height = 1;
 869 |   cloud.is_dense = true;
 870 |   cloud.header.frame_id = mp_.frame_id_;
 871 |   sensor_msgs::PointCloud2 cloud_msg;
 872 | 
 873 |   pcl::toROSMsg(cloud, cloud_msg);
 874 |   map_pub_.publish(cloud_msg);
 875 | }
 876 | 
 877 | void GridMap::publishMapInflate(bool all_info)
 878 | {
 879 | 
 880 |   if (map_inf_pub_.getNumSubscribers() <= 0)
 881 |     return;
 882 | 
 883 |   pcl::PointXYZ pt;
 884 |   pcl::PointCloud<pcl::PointXYZ> cloud;
 885 | 
 886 |   Eigen::Vector3i min_cut = md_.local_bound_min_;
 887 |   Eigen::Vector3i max_cut = md_.local_bound_max_;
 888 | 
 889 |   if (all_info)
 890 |   {
 891 |     int lmm = mp_.local_map_margin_;
 892 |     min_cut -= Eigen::Vector3i(lmm, lmm, lmm);
 893 |     max_cut += Eigen::Vector3i(lmm, lmm, lmm);
 894 |   }
 895 | 
 896 |   boundIndex(min_cut);
 897 |   boundIndex(max_cut);
 898 | 
 899 |   for (int x = min_cut(0); x <= max_cut(0); ++x)
 900 |     for (int y = min_cut(1); y <= max_cut(1); ++y)
 901 |       for (int z = min_cut(2); z <= max_cut(2); ++z)
 902 |       {
 903 |         if (md_.occupancy_buffer_inflate_[toAddress(x, y, z)] == 0)
 904 |           continue;
 905 | 
 906 |         Eigen::Vector3d pos;
 907 |         indexToPos(Eigen::Vector3i(x, y, z), pos);
 908 |         if (pos(2) > mp_.visualization_truncate_height_)
 909 |           continue;
 910 | 
 911 |         pt.x = pos(0);
 912 |         pt.y = pos(1);
 913 |         pt.z = pos(2);
 914 |         cloud.push_back(pt);
 915 |       }
 916 | 
 917 |   cloud.width = cloud.points.size();
 918 |   cloud.height = 1;
 919 |   cloud.is_dense = true;
 920 |   cloud.header.frame_id = mp_.frame_id_;
 921 |   sensor_msgs::PointCloud2 cloud_msg;
 922 | 
 923 |   pcl::toROSMsg(cloud, cloud_msg);
 924 |   map_inf_pub_.publish(cloud_msg);
 925 | 
 926 |   // ROS_INFO("pub map");
 927 | }
 928 | 
 929 | void GridMap::publishUnknown()
 930 | {
 931 |   pcl::PointXYZ pt;
 932 |   pcl::PointCloud<pcl::PointXYZ> cloud;
 933 | 
 934 |   Eigen::Vector3i min_cut = md_.local_bound_min_;
 935 |   Eigen::Vector3i max_cut = md_.local_bound_max_;
 936 | 
 937 |   boundIndex(max_cut);
 938 |   boundIndex(min_cut);
 939 | 
 940 |   for (int x = min_cut(0); x <= max_cut(0); ++x)
 941 |     for (int y = min_cut(1); y <= max_cut(1); ++y)
 942 |       for (int z = min_cut(2); z <= max_cut(2); ++z)
 943 |       {
 944 | 
 945 |         if (md_.occupancy_buffer_[toAddress(x, y, z)] < mp_.clamp_min_log_ - 1e-3)
 946 |         {
 947 |           Eigen::Vector3d pos;
 948 |           indexToPos(Eigen::Vector3i(x, y, z), pos);
 949 |           if (pos(2) > mp_.visualization_truncate_height_)
 950 |             continue;
 951 | 
 952 |           pt.x = pos(0);
 953 |           pt.y = pos(1);
 954 |           pt.z = pos(2);
 955 |           cloud.push_back(pt);
 956 |         }
 957 |       }
 958 | 
 959 |   cloud.width = cloud.points.size();
 960 |   cloud.height = 1;
 961 |   cloud.is_dense = true;
 962 |   cloud.header.frame_id = mp_.frame_id_;
 963 | 
 964 |   sensor_msgs::PointCloud2 cloud_msg;
 965 |   pcl::toROSMsg(cloud, cloud_msg);
 966 |   unknown_pub_.publish(cloud_msg);
 967 | }
 968 | 
 969 | bool GridMap::odomValid() { return md_.has_odom_; }
 970 | 
 971 | bool GridMap::hasDepthObservation() { return md_.has_first_depth_; }
 972 | 
 973 | Eigen::Vector3d GridMap::getOrigin() { return mp_.map_origin_; }
 974 | 
 975 | // int GridMap::getVoxelNum() {
 976 | //   return mp_.map_voxel_num_[0] * mp_.map_voxel_num_[1] * mp_.map_voxel_num_[2];
 977 | // }
 978 | 
 979 | void GridMap::getRegion(Eigen::Vector3d &ori, Eigen::Vector3d &size)
 980 | {
 981 |   ori = mp_.map_origin_, size = mp_.map_size_;
 982 | }
 983 | 
 984 | void GridMap::depthOdomCallback(const sensor_msgs::ImageConstPtr &img,
 985 |                                 const nav_msgs::OdometryConstPtr &odom)
 986 | {
 987 |   /* get pose */
 988 |   Eigen::Quaterniond body_q = Eigen::Quaterniond(odom->pose.pose.orientation.w,
 989 |                                                  odom->pose.pose.orientation.x,
 990 |                                                  odom->pose.pose.orientation.y,
 991 |                                                  odom->pose.pose.orientation.z);    
 992 |   Eigen::Matrix3d body_r_m = body_q.toRotationMatrix();   
 993 |   Eigen::Matrix4d body2world;
 994 |   body2world.block<3, 3>(0, 0) = body_r_m;
 995 |   body2world(0, 3) = odom->pose.pose.position.x;
 996 |   body2world(1, 3) = odom->pose.pose.position.y;
 997 |   body2world(2, 3) = odom->pose.pose.position.z;
 998 |   body2world(3, 3) = 1.0;
 999 |   
1000 |   Eigen::Matrix4d cam_T = body2world * md_.cam2body_;
1001 |   md_.camera_pos_(0) = cam_T(0, 3);
1002 |   md_.camera_pos_(1) = cam_T(1, 3);
1003 |   md_.camera_pos_(2) = cam_T(2, 3);
1004 |   md_.camera_q_ = Eigen::Quaterniond(cam_T.block<3, 3>(0, 0));
1005 | 
1006 |   /* get depth image */
1007 |   cv_bridge::CvImagePtr cv_ptr;
1008 |   cv_ptr = cv_bridge::toCvCopy(img, img->encoding);
1009 |   if (img->encoding == sensor_msgs::image_encodings::TYPE_32FC1)
1010 |   {
1011 |     (cv_ptr->image).convertTo(cv_ptr->image, CV_16UC1, mp_.k_depth_scaling_factor_);
1012 |   }
1013 |   cv_ptr->image.copyTo(md_.depth_image_);
1014 | 
1015 |   md_.occ_need_update_ = true;
1016 | }
1017 | 
1018 | // GridMap
1019 | 


--------------------------------------------------------------------------------
/src/node.cpp:
--------------------------------------------------------------------------------
 1 | #include <ros/ros.h>
 2 | #include "plan_env/grid_map.h"
 3 | 
 4 | 
 5 | GridMap::Ptr   grid_map_test_ ; 
 6 | 
 7 | int main(int argc, char** argv)
 8 | {
 9 |     ros::init(argc, argv, "gridmap_test_node");
10 |     ros::NodeHandle nh_("~");
11 | 
12 |     grid_map_test_.reset(new GridMap) ;
13 |     grid_map_test_->initMap(nh_) ;
14 | 
15 |     //grid_map_test_->publishMap(); 
16 | 
17 |     ros::spin();
18 |   
19 |     return 0;
20 | }
21 | 
22 | 


--------------------------------------------------------------------------------
/src/raycast.cpp:
--------------------------------------------------------------------------------
  1 | #include <Eigen/Eigen>
  2 | #include <cmath>
  3 | #include <iostream>
  4 | #include <plan_env/raycast.h>
  5 | 
  6 | int signum(int x) {
  7 |   return x == 0 ? 0 : x < 0 ? -1 : 1;
  8 | }
  9 | 
 10 | double mod(double value, double modulus) {
 11 |   return fmod(fmod(value, modulus) + modulus, modulus);
 12 | }
 13 | 
 14 | double intbound(double s, double ds) {
 15 |   // Find the smallest positive t such that s+t*ds is an integer.
 16 |   if (ds < 0) {
 17 |     return intbound(-s, -ds);
 18 |   } else {
 19 |     s = mod(s, 1);
 20 |     // problem is now s+t*ds = 1
 21 |     return (1 - s) / ds;
 22 |   }
 23 | }
 24 | 
 25 | void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min,
 26 |              const Eigen::Vector3d& max, int& output_points_cnt, Eigen::Vector3d* output) {
 27 |   //    std::cout << start << ' ' << end << std::endl;
 28 |   // From "A Fast Voxel Traversal Algorithm for Ray Tracing"
 29 |   // by John Amanatides and Andrew Woo, 1987
 30 |   // <http://www.cse.yorku.ca/~amana/research/grid.pdf>
 31 |   // <http://citeseer.ist.psu.edu/viewdoc/summary?doi=10.1.1.42.3443>
 32 |   // Extensions to the described algorithm:
 33 |   //   • Imposed a distance limit.
 34 |   //   • The face passed through to reach the current cube is provided to
 35 |   //     the callback.
 36 | 
 37 |   // The foundation of this algorithm is a parameterized representation of
 38 |   // the provided ray,
 39 |   //                    origin + t * direction,
 40 |   // except that t is not actually stored; rather, at any given point in the
 41 |   // traversal, we keep track of the *greater* t values which we would have
 42 |   // if we took a step sufficient to cross a cube boundary along that axis
 43 |   // (i.e. change the integer part of the coordinate) in the variables
 44 |   // tMaxX, tMaxY, and tMaxZ.
 45 | 
 46 |   // Cube containing origin point.
 47 |   int x = (int)std::floor(start.x());
 48 |   int y = (int)std::floor(start.y());
 49 |   int z = (int)std::floor(start.z());
 50 |   int endX = (int)std::floor(end.x());
 51 |   int endY = (int)std::floor(end.y());
 52 |   int endZ = (int)std::floor(end.z());
 53 |   Eigen::Vector3d direction = (end - start);
 54 |   double maxDist = direction.squaredNorm();
 55 | 
 56 |   // Break out direction vector.
 57 |   double dx = endX - x;
 58 |   double dy = endY - y;
 59 |   double dz = endZ - z;
 60 | 
 61 |   // Direction to increment x,y,z when stepping.
 62 |   int stepX = (int)signum((int)dx);
 63 |   int stepY = (int)signum((int)dy);
 64 |   int stepZ = (int)signum((int)dz);
 65 | 
 66 |   // See description above. The initial values depend on the fractional
 67 |   // part of the origin.
 68 |   double tMaxX = intbound(start.x(), dx);
 69 |   double tMaxY = intbound(start.y(), dy);
 70 |   double tMaxZ = intbound(start.z(), dz);
 71 | 
 72 |   // The change in t when taking a step (always positive).
 73 |   double tDeltaX = ((double)stepX) / dx;
 74 |   double tDeltaY = ((double)stepY) / dy;
 75 |   double tDeltaZ = ((double)stepZ) / dz;
 76 | 
 77 |   // Avoids an infinite loop.
 78 |   if (stepX == 0 && stepY == 0 && stepZ == 0) return;
 79 | 
 80 |   double dist = 0;
 81 |   while (true) {
 82 |     if (x >= min.x() && x < max.x() && y >= min.y() && y < max.y() && z >= min.z() && z < max.z()) {
 83 |       output[output_points_cnt](0) = x;
 84 |       output[output_points_cnt](1) = y;
 85 |       output[output_points_cnt](2) = z;
 86 | 
 87 |       output_points_cnt++;
 88 |       dist = sqrt((x - start(0)) * (x - start(0)) + (y - start(1)) * (y - start(1)) +
 89 |                   (z - start(2)) * (z - start(2)));
 90 | 
 91 |       if (dist > maxDist) return;
 92 | 
 93 |       /*            if (output_points_cnt > 1500) {
 94 |                       std::cerr << "Error, too many racyast voxels." <<
 95 |          std::endl;
 96 |                       throw std::out_of_range("Too many raycast voxels");
 97 |                   }*/
 98 |     }
 99 | 
100 |     if (x == endX && y == endY && z == endZ) break;
101 | 
102 |     // tMaxX stores the t-value at which we cross a cube boundary along the
103 |     // X axis, and similarly for Y and Z. Therefore, choosing the least tMax
104 |     // chooses the closest cube boundary. Only the first case of the four
105 |     // has been commented in detail.
106 |     if (tMaxX < tMaxY) {
107 |       if (tMaxX < tMaxZ) {
108 |         // Update which cube we are now in.
109 |         x += stepX;
110 |         // Adjust tMaxX to the next X-oriented boundary crossing.
111 |         tMaxX += tDeltaX;
112 |       } else {
113 |         z += stepZ;
114 |         tMaxZ += tDeltaZ;
115 |       }
116 |     } else {
117 |       if (tMaxY < tMaxZ) {
118 |         y += stepY;
119 |         tMaxY += tDeltaY;
120 |       } else {
121 |         z += stepZ;
122 |         tMaxZ += tDeltaZ;
123 |       }
124 |     }
125 |   }
126 | }
127 | 
128 | void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min,
129 |              const Eigen::Vector3d& max, std::vector<Eigen::Vector3d>* output) {
130 |   //    std::cout << start << ' ' << end << std::endl;
131 |   // From "A Fast Voxel Traversal Algorithm for Ray Tracing"
132 |   // by John Amanatides and Andrew Woo, 1987
133 |   // <http://www.cse.yorku.ca/~amana/research/grid.pdf>
134 |   // <http://citeseer.ist.psu.edu/viewdoc/summary?doi=10.1.1.42.3443>
135 |   // Extensions to the described algorithm:
136 |   //   • Imposed a distance limit.
137 |   //   • The face passed through to reach the current cube is provided to
138 |   //     the callback.
139 | 
140 |   // The foundation of this algorithm is a parameterized representation of
141 |   // the provided ray,
142 |   //                    origin + t * direction,
143 |   // except that t is not actually stored; rather, at any given point in the
144 |   // traversal, we keep track of the *greater* t values which we would have
145 |   // if we took a step sufficient to cross a cube boundary along that axis
146 |   // (i.e. change the integer part of the coordinate) in the variables
147 |   // tMaxX, tMaxY, and tMaxZ.
148 | 
149 |   // Cube containing origin point.
150 |   int x = (int)std::floor(start.x());
151 |   int y = (int)std::floor(start.y());
152 |   int z = (int)std::floor(start.z());
153 |   int endX = (int)std::floor(end.x());
154 |   int endY = (int)std::floor(end.y());
155 |   int endZ = (int)std::floor(end.z());
156 |   Eigen::Vector3d direction = (end - start);
157 |   double maxDist = direction.squaredNorm();
158 | 
159 |   // Break out direction vector.
160 |   double dx = endX - x;
161 |   double dy = endY - y;
162 |   double dz = endZ - z;
163 | 
164 |   // Direction to increment x,y,z when stepping.
165 |   int stepX = (int)signum((int)dx);
166 |   int stepY = (int)signum((int)dy);
167 |   int stepZ = (int)signum((int)dz);
168 | 
169 |   // See description above. The initial values depend on the fractional
170 |   // part of the origin.
171 |   double tMaxX = intbound(start.x(), dx);
172 |   double tMaxY = intbound(start.y(), dy);
173 |   double tMaxZ = intbound(start.z(), dz);
174 | 
175 |   // The change in t when taking a step (always positive).
176 |   double tDeltaX = ((double)stepX) / dx;
177 |   double tDeltaY = ((double)stepY) / dy;
178 |   double tDeltaZ = ((double)stepZ) / dz;
179 | 
180 |   output->clear();
181 | 
182 |   // Avoids an infinite loop.
183 |   if (stepX == 0 && stepY == 0 && stepZ == 0) return;
184 | 
185 |   double dist = 0;
186 |   while (true) {
187 |     if (x >= min.x() && x < max.x() && y >= min.y() && y < max.y() && z >= min.z() && z < max.z()) {
188 |       output->push_back(Eigen::Vector3d(x, y, z));
189 | 
190 |       dist = (Eigen::Vector3d(x, y, z) - start).squaredNorm();
191 | 
192 |       if (dist > maxDist) return;
193 | 
194 |       if (output->size() > 1500) {
195 |         std::cerr << "Error, too many racyast voxels." << std::endl;
196 |         throw std::out_of_range("Too many raycast voxels");
197 |       }
198 |     }
199 | 
200 |     if (x == endX && y == endY && z == endZ) break;
201 | 
202 |     // tMaxX stores the t-value at which we cross a cube boundary along the
203 |     // X axis, and similarly for Y and Z. Therefore, choosing the least tMax
204 |     // chooses the closest cube boundary. Only the first case of the four
205 |     // has been commented in detail.
206 |     if (tMaxX < tMaxY) {
207 |       if (tMaxX < tMaxZ) {
208 |         // Update which cube we are now in.
209 |         x += stepX;
210 |         // Adjust tMaxX to the next X-oriented boundary crossing.
211 |         tMaxX += tDeltaX;
212 |       } else {
213 |         z += stepZ;
214 |         tMaxZ += tDeltaZ;
215 |       }
216 |     } else {
217 |       if (tMaxY < tMaxZ) {
218 |         y += stepY;
219 |         tMaxY += tDeltaY;
220 |       } else {
221 |         z += stepZ;
222 |         tMaxZ += tDeltaZ;
223 |       }
224 |     }
225 |   }
226 | }
227 | 
228 | bool RayCaster::setInput(const Eigen::Vector3d& start,
229 |                          const Eigen::Vector3d& end /* , const Eigen::Vector3d& min,
230 |                          const Eigen::Vector3d& max */) {
231 |   start_ = start;
232 |   end_ = end;
233 |   // max_ = max;
234 |   // min_ = min;
235 | 
236 |   x_ = (int)std::floor(start_.x());
237 |   y_ = (int)std::floor(start_.y());
238 |   z_ = (int)std::floor(start_.z());
239 |   endX_ = (int)std::floor(end_.x());
240 |   endY_ = (int)std::floor(end_.y());
241 |   endZ_ = (int)std::floor(end_.z());
242 |   direction_ = (end_ - start_);
243 |   maxDist_ = direction_.squaredNorm();
244 | 
245 |   // Break out direction vector.
246 |   dx_ = endX_ - x_;
247 |   dy_ = endY_ - y_;
248 |   dz_ = endZ_ - z_;
249 | 
250 |   // Direction to increment x,y,z when stepping.
251 |   stepX_ = (int)signum((int)dx_);
252 |   stepY_ = (int)signum((int)dy_);
253 |   stepZ_ = (int)signum((int)dz_);
254 | 
255 |   // See description above. The initial values depend on the fractional
256 |   // part of the origin.
257 |   tMaxX_ = intbound(start_.x(), dx_);
258 |   tMaxY_ = intbound(start_.y(), dy_);
259 |   tMaxZ_ = intbound(start_.z(), dz_);
260 | 
261 |   // The change in t when taking a step (always positive).
262 |   tDeltaX_ = ((double)stepX_) / dx_;
263 |   tDeltaY_ = ((double)stepY_) / dy_;
264 |   tDeltaZ_ = ((double)stepZ_) / dz_;
265 | 
266 |   dist_ = 0;
267 | 
268 |   step_num_ = 0;
269 | 
270 |   // Avoids an infinite loop.
271 |   if (stepX_ == 0 && stepY_ == 0 && stepZ_ == 0)
272 |     return false;
273 |   else
274 |     return true;
275 | }
276 | 
277 | bool RayCaster::step(Eigen::Vector3d& ray_pt) {
278 |   // if (x_ >= min_.x() && x_ < max_.x() && y_ >= min_.y() && y_ < max_.y() &&
279 |   // z_ >= min_.z() && z_ <
280 |   // max_.z())
281 |   ray_pt = Eigen::Vector3d(x_, y_, z_);
282 | 
283 |   // step_num_++;
284 | 
285 |   // dist_ = (Eigen::Vector3d(x_, y_, z_) - start_).squaredNorm();
286 | 
287 |   if (x_ == endX_ && y_ == endY_ && z_ == endZ_) {
288 |     return false;
289 |   }
290 | 
291 |   // if (dist_ > maxDist_)
292 |   // {
293 |   //   return false;
294 |   // }
295 | 
296 |   // tMaxX stores the t-value at which we cross a cube boundary along the
297 |   // X axis, and similarly for Y and Z. Therefore, choosing the least tMax
298 |   // chooses the closest cube boundary. Only the first case of the four
299 |   // has been commented in detail.
300 |   if (tMaxX_ < tMaxY_) {
301 |     if (tMaxX_ < tMaxZ_) {
302 |       // Update which cube we are now in.
303 |       x_ += stepX_;
304 |       // Adjust tMaxX to the next X-oriented boundary crossing.
305 |       tMaxX_ += tDeltaX_;
306 |     } else {
307 |       z_ += stepZ_;
308 |       tMaxZ_ += tDeltaZ_;
309 |     }
310 |   } else {
311 |     if (tMaxY_ < tMaxZ_) {
312 |       y_ += stepY_;
313 |       tMaxY_ += tDeltaY_;
314 |     } else {
315 |       z_ += stepZ_;
316 |       tMaxZ_ += tDeltaZ_;
317 |     }
318 |   }
319 | 
320 |   return true;
321 | }


--------------------------------------------------------------------------------