diff --git a/autonomy_core/client/client_launch/rviz/client.rviz b/autonomy_core/client/client_launch/rviz/client.rviz index bb62ccfe..18507839 100644 --- a/autonomy_core/client/client_launch/rviz/client.rviz +++ b/autonomy_core/client/client_launch/rviz/client.rviz @@ -8,8 +8,6 @@ Panels: - /Status1 - /TF1/Frames1 - /Env1/Environment1 - - /Planner1 - - /Maps1 - /Maps1/Local1 - /Estimation1/ground_truth_odom1/Shape1 - /Estimation1/vio_corrected1/Shape1 @@ -20,7 +18,7 @@ Panels: - /Estimation1/graphslam1/Shape1 - /Estimation1/ukf_odom1/Shape1 Splitter Ratio: 0.5211267471313477 - Tree Height: 323 + Tree Height: 574 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -36,10 +34,9 @@ Panels: Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 - SyncSource: Local + SyncSource: StorageMap Preferences: PromptSaveOnExit: true Toolbars: @@ -216,8 +213,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 10.125 - Min Value: -0.125 + Max Value: 3.625 + Min Value: 0.125 Value: true Axis: Z BoundScale: 0.5 @@ -279,7 +276,7 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true - Enabled: true + Enabled: false Name: Maps - Class: rviz/Group Displays: @@ -485,6 +482,39 @@ Visualization Manager: Show Visual Aids: false Update Topic: /quadrotor/waypoint_nav/update Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 4.125 + Min Value: 0.125 + Value: true + Axis: Z + BoundScale: 0.5 + Channel Name: intensity + Class: planning_rviz_plugins/VoxelMap + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + MeshAlpha: 0.019999999552965164 + MeshColor: 0; 170; 255 + MeshHeight: 0.5 + Min Color: 0; 0; 0 + Name: StorageMap + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.5 + State: Occupied + Style: Boxes + Topic: /quadrotor/mapper/storage_voxel_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 189; 234; 201 @@ -518,7 +548,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 60.19675827026367 + Distance: 175.9504852294922 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -534,17 +564,17 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.1453958749771118 + Pitch: 0.7797964811325073 Target Frame: ground_truth/quadrotor/quadrotor - Yaw: 3.170363426208496 + Yaw: 3.120365619659424 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 752 + Height: 1136 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001d300000252fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000180000000c900fffffffb000000260057006100790070006f0069006e00740020004e0061007600690067006100740069006f006e01000001c3000000cc000000be00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000252000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000056a0000003efc0100000002fb0000000800540069006d006501000000000000056a000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003910000025200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001fc000003d2fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000027b000000c900fffffffb000000260057006100790070006f0069006e00740020004e0061007600690067006100740069006f006e01000002be00000151000000be00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000252000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000536000003d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -555,6 +585,6 @@ Window Geometry: collapsed: true Waypoint Navigation: collapsed: false - Width: 1386 - X: 274 - Y: 277 + Width: 1848 + X: 72 + Y: 27 diff --git a/autonomy_core/map_plan/mapper/include/mapper/local_global_mapper.h b/autonomy_core/map_plan/mapper/include/mapper/local_global_mapper.h index 12bd0801..6e31663d 100644 --- a/autonomy_core/map_plan/mapper/include/mapper/local_global_mapper.h +++ b/autonomy_core/map_plan/mapper/include/mapper/local_global_mapper.h @@ -13,6 +13,17 @@ #include "mapper/tf_listener.h" #include "mapper/voxel_mapper.h" +// Define storage map reinitialize direction +#define CENTER 0 +#define RIGHT_BOTTOM 1 +#define RIGHT_TOP 2 +#define LEFT_BOTTOM 3 +#define LEFT_TOP 4 +#define LEFT 5 +#define RIGHT 6 +#define TOP 7 +#define BOTTOM 8 + // Timer stuff using boost::timer::cpu_timer; using boost::timer::cpu_times; @@ -31,6 +42,12 @@ class LocalGlobalMapperNode { */ void initParams(); + /** + * @brief Check if storage map needs reinitialize + * + */ + int reinitializeStorageMapFlag(const Eigen::Vector3d& center_position_map); + /** * @brief Crops the local map from the storage map, transforms it to odometry * frame and publishes @@ -78,6 +95,12 @@ class LocalGlobalMapperNode { */ void storageMapInit(); + /**unique_ptr + * @brief Re allocates storage map + * + */ + void storageMapReInit(int direction); + /** * @brief Initializes the array for inflation for the local map */ @@ -129,12 +152,14 @@ class LocalGlobalMapperNode { bool global_use_robot_dim_z_; double global_map_dim_d_x_, global_map_dim_d_y_, global_map_dim_d_z_; double local_map_dim_d_x_, local_map_dim_d_y_, local_map_dim_d_z_; + double storage_ori_offset_x_, storage_ori_offset_y_; + double prev_storage_center_x_, prev_storage_center_y_; double local_max_raycast_, global_max_raycast_; // maximum raycasting range double occ_map_height_; Eigen::Vector3d local_ori_offset_; bool pub_storage_map_ = - false; // don't set this as true unless you're debugging, it's very slow + true; //false; // don't set this as true unless you're debugging, it's very slow int update_interval_; int counter_ = 0; diff --git a/autonomy_core/map_plan/mapper/include/mapper/voxel_mapper.h b/autonomy_core/map_plan/mapper/include/mapper/voxel_mapper.h index 52d53ed9..3c2c2cca 100644 --- a/autonomy_core/map_plan/mapper/include/mapper/voxel_mapper.h +++ b/autonomy_core/map_plan/mapper/include/mapper/voxel_mapper.h @@ -134,7 +134,7 @@ class VoxelMapper { vec_Vec3d getCloud(); /** - * @brief Get a vector of points that are located at the center of occupied + * @brief Get a vector of points that are located at ctheenter of occupied * voxels in the inflated map */ vec_Vec3d getInflatedCloud(); @@ -185,6 +185,19 @@ class VoxelMapper { */ void freeCloud(const vec_Vec3d& pts, const Eigen::Affine3d& tf); + mapper::VoxelMap get_voxel_map() { + return map_; + }; + + mapper::VoxelMap get_inflated_voxel_map() { + return inflated_map_; + }; + + bool reAllocate(const std::vector old_map, + const Eigen::Vector3d& new_ori_d, + const Eigen::Vector3d& new_dim_d, + int direction); + private: /** * @brief Initialize a space for the map. If this method is called after diff --git a/autonomy_core/map_plan/mapper/src/local_global_mapper.cpp b/autonomy_core/map_plan/mapper/src/local_global_mapper.cpp index 70066c57..883f8591 100644 --- a/autonomy_core/map_plan/mapper/src/local_global_mapper.cpp +++ b/autonomy_core/map_plan/mapper/src/local_global_mapper.cpp @@ -23,13 +23,27 @@ LocalGlobalMapperNode::LocalGlobalMapperNode(const ros::NodeHandle& nh) storage_map_info_.dim.z = local_map_info_.dim.z; // storage map should have same x y z center and x_dim y_dim as global map - storage_map_info_.origin.x = global_map_info_.origin.x; - storage_map_info_.origin.y = global_map_info_.origin.y; - storage_map_info_.origin.z = global_map_info_.origin.z; + /* storage_map_info_.dim.x = static_cast( ceil((global_map_dim_d_x_) / storage_map_info_.resolution)); storage_map_info_.dim.y = static_cast( ceil((global_map_dim_d_y_) / storage_map_info_.resolution)); + */ + + storage_map_info_.dim.x = static_cast( + ceil((local_map_dim_d_x_) * 3 / local_map_info_.resolution)); + storage_map_info_.dim.y = static_cast( + ceil((local_map_dim_d_y_) * 3 / local_map_info_.resolution)); + + storage_ori_offset_x_ = - local_map_dim_d_x_ * 3 / 2; + storage_ori_offset_y_ = - local_map_dim_d_y_ * 3 / 2; + + storage_map_info_.origin.x = local_map_info_.origin.x + storage_ori_offset_x_; // global_map_info_.origin.x; + storage_map_info_.origin.y = local_map_info_.origin.y + storage_ori_offset_y_; // global_map_info_.origin.y; + storage_map_info_.origin.z = global_map_info_.origin.z; + + prev_storage_center_x_ = local_map_info_.origin.x; + prev_storage_center_y_ = local_map_info_.origin.y; local_map_info_.dim.x = static_cast(ceil((local_map_dim_d_x_) / local_map_info_.resolution)); @@ -155,6 +169,7 @@ void LocalGlobalMapperNode::storageMapInit() { storage_map_info_.dim.z * storage_map_info_.resolution); const double res = storage_map_info_.resolution; int8_t storage_val_default = 0; + // Initialize the mapper storage_voxel_mapper_.reset( new mapper::VoxelMapper(storage_origin, @@ -164,6 +179,30 @@ void LocalGlobalMapperNode::storageMapInit() { local_decay_times_to_empty_)); } +void LocalGlobalMapperNode::storageMapReInit(int direction) { + const Eigen::Vector3d storage_origin(storage_map_info_.origin.x, + storage_map_info_.origin.y, + storage_map_info_.origin.z); + const Eigen::Vector3d storage_dim_d( + storage_map_info_.dim.x * storage_map_info_.resolution, + storage_map_info_.dim.y * storage_map_info_.resolution, + storage_map_info_.dim.z * storage_map_info_.resolution); + const double res = storage_map_info_.resolution; + int8_t storage_val_default = 0; + + std::vector old_map = storage_voxel_mapper_->get_voxel_map().getMap(); + + storage_voxel_mapper_.reset( + new mapper::VoxelMapper(storage_origin, + storage_dim_d, + res, + storage_val_default, + local_decay_times_to_empty_)); + + storage_voxel_mapper_->reAllocate(old_map, storage_origin, storage_dim_d, direction); + +} + void LocalGlobalMapperNode::localInflaInit() { const double res = storage_map_info_.resolution; local_infla_array_.clear(); @@ -262,6 +301,34 @@ void LocalGlobalMapperNode::getLidarPoses( } } +int LocalGlobalMapperNode::reinitializeStorageMapFlag(const Eigen::Vector3d& center_position_map) { + // TODO: Check if we need to reinitialize the storage map + int x = center_position_map[0] - prev_storage_center_x_; + int y = center_position_map[1] - prev_storage_center_y_; + double thresh_x = local_map_dim_d_x_ * 0.8; + double thresh_y = local_map_dim_d_y_ * 0.8; + + if (x < -thresh_x && y < -thresh_y) { + return RIGHT_BOTTOM; + } else if (x < -thresh_x && y > thresh_y) { + return RIGHT_TOP; + } else if (x > thresh_x && y < -thresh_y) { + return LEFT_BOTTOM; + } else if (x > thresh_x && y > thresh_y) { + return LEFT_TOP; + } else if (y < -thresh_y) { + return BOTTOM; + } else if (y > thresh_y) { + return TOP; + } else if (x > thresh_x) { + return LEFT; + } else if (x < -thresh_x) { + return RIGHT; + } else { + return CENTER; + } +} + void LocalGlobalMapperNode::processCloud( const sensor_msgs::PointCloud& cloud) { if ((storage_voxel_mapper_ == nullptr) || (global_voxel_mapper_ == nullptr)) { @@ -303,6 +370,63 @@ void LocalGlobalMapperNode::processCloud( ROS_DEBUG("[storage map addCloud]: %f", static_cast(timer.elapsed().wall) / 1e6); + // Reinitiliaze storage map to follow local map + int pos_to_storage_ori = reinitializeStorageMapFlag(lidar_position_map); + + if (pos_to_storage_ori == LEFT_BOTTOM) { + // TODO: Reset storage map origin + prev_storage_center_x_ = prev_storage_center_x_ + local_map_dim_d_x_; + prev_storage_center_y_ = prev_storage_center_y_ - local_map_dim_d_y_; + storage_map_info_.origin.x = prev_storage_center_x_ + storage_ori_offset_x_; + storage_map_info_.origin.y = prev_storage_center_y_ + storage_ori_offset_y_; + // storageMapInit(); + } else if (pos_to_storage_ori == LEFT_TOP) { + // TODO: Reset storage map origin + prev_storage_center_x_ = prev_storage_center_x_ + local_map_dim_d_x_; + prev_storage_center_y_ = prev_storage_center_y_ + local_map_dim_d_y_; + storage_map_info_.origin.x = prev_storage_center_x_ + storage_ori_offset_x_; + storage_map_info_.origin.y = prev_storage_center_y_ + storage_ori_offset_y_; + // storageMapInit(); + } else if (pos_to_storage_ori == RIGHT_BOTTOM) { + // TODO: Reset storage map origin + prev_storage_center_x_ = prev_storage_center_x_ - local_map_dim_d_x_; + prev_storage_center_y_ = prev_storage_center_y_ - local_map_dim_d_y_; + storage_map_info_.origin.x = prev_storage_center_x_ + storage_ori_offset_x_; + storage_map_info_.origin.y = prev_storage_center_y_ + storage_ori_offset_y_; + // storageMapInit(); + } else if (pos_to_storage_ori == RIGHT_TOP) { + // TODO: Reset storage map origin + prev_storage_center_x_ = prev_storage_center_x_ - local_map_dim_d_x_; + prev_storage_center_y_ = prev_storage_center_y_ + local_map_dim_d_y_; + storage_map_info_.origin.x = prev_storage_center_x_+ storage_ori_offset_x_; + storage_map_info_.origin.y = prev_storage_center_y_ + storage_ori_offset_y_; + // storageMapInit(); + } else if (pos_to_storage_ori == LEFT) { + // TODO: Reset storage map origin + prev_storage_center_x_ = prev_storage_center_x_ + local_map_dim_d_x_; + storage_map_info_.origin.x = prev_storage_center_x_ + storage_ori_offset_x_; + // storageMapInit(); + storageMapReInit(LEFT); + } else if (pos_to_storage_ori == RIGHT) { + // TODO: Reset storage map origin + prev_storage_center_x_ = prev_storage_center_x_ - local_map_dim_d_x_; + storage_map_info_.origin.x = prev_storage_center_x_ + storage_ori_offset_x_; + // storageMapInit(); + storageMapReInit(RIGHT); + } else if (pos_to_storage_ori == TOP) { + // TODO: Reset storage map origin + prev_storage_center_y_ = prev_storage_center_y_ + local_map_dim_d_y_; + storage_map_info_.origin.y = prev_storage_center_y_ + storage_ori_offset_y_; + // storageMapInit(); + storageMapReInit(TOP); + } else if (pos_to_storage_ori == BOTTOM) { + // TODO: Reset storage map origin + prev_storage_center_y_ = prev_storage_center_y_ - local_map_dim_d_y_; + storage_map_info_.origin.y = prev_storage_center_y_ + storage_ori_offset_y_; + // storageMapReInit(); + storageMapReInit(BOTTOM); + } + // get and publish storage map (this is very slow) if (pub_storage_map_) { planning_ros_msgs::VoxelMap storage_map = diff --git a/autonomy_core/map_plan/mapper/src/voxel_mapper.cpp b/autonomy_core/map_plan/mapper/src/voxel_mapper.cpp index 2c2eff12..b5a3e8c1 100644 --- a/autonomy_core/map_plan/mapper/src/voxel_mapper.cpp +++ b/autonomy_core/map_plan/mapper/src/voxel_mapper.cpp @@ -479,4 +479,175 @@ bool VoxelMapper::allocate(const Eigen::Vector3d& new_ori_d, } } +bool VoxelMapper::reAllocate(const std::vector old_map, + const Eigen::Vector3d& new_ori_d, + const Eigen::Vector3d& new_dim_d, + int direction) { + + static Eigen::Vector3i prev_origin = Eigen::Vector3i::Zero(); + + Eigen::Vector3i new_dim(new_dim_d(0) / res_, + new_dim_d(1) / res_, + new_dim_d(2) / res_); + Eigen::Vector3i new_ori(new_ori_d(0) / res_, + new_ori_d(1) / res_, + new_ori_d(2) / res_); + + if (new_dim(2) == 0) // 2d case, set the z dimension to be 1 + new_dim(2) = 1; + + // Check if new dimensions and origin are the same as before, in which case + // do not perform reallocation + if (new_dim == dim_ && new_ori == prev_origin) { + return false; + } else { + int size = new_dim(0) * new_dim(1) * new_dim(2); + std::vector new_map(size, val_default_); + + // Iterate through all elements in new map + Eigen::Vector3i new_vox; + int x_trim = ceil(new_dim(0) / 3); + int y_trim = ceil(new_dim(1) / 3); + + for (new_vox(0) = 0; new_vox(0) < new_dim(0); new_vox(0)++) { + for (new_vox(1) = 0; new_vox(1) < new_dim(1); new_vox(1)++) { + for (new_vox(2) = 0; new_vox(2) < new_dim(2); new_vox(2)++) { + // Move old storage map voxels to new map + switch (direction) + { + case 1: // RIGHT_BOTTOM + if (new_vox(0) < new_dim(0) - x_trim && new_vox(1) < new_dim(1) - y_trim) { + int old_vox_idx = new_vox(0) + + new_dim(0) * new_vox(1) + + new_dim(0) * new_dim(1) * new_vox(2); + int new_vox_trim_0 = new_vox(0) + x_trim; + int new_vox_trim_1 = new_vox(1) + y_trim; + int new_vox_idx = new_vox_trim_0 + + new_dim(0) * new_vox_trim_1 + + new_dim(0) * new_dim(1) * new_vox(2); + + new_map[new_vox_idx] = old_map[old_vox_idx]; + } + break; + + case 2: // RIGHT_TOP + if (new_vox(0) < new_dim(0) - x_trim && new_vox(1) > y_trim) { + int old_vox_idx = new_vox(0) + + new_dim(0) * new_vox(1) + + new_dim(0) * new_dim(1) * new_vox(2); + int new_vox_trim_0 = new_vox(0) + x_trim; + int new_vox_trim_1 = new_vox(1) - y_trim; + int new_vox_idx = new_vox_trim_0 + + new_dim(0) * new_vox_trim_1 + + new_dim(0) * new_dim(1) * new_vox(2); + + new_map[new_vox_idx] = old_map[old_vox_idx]; + } + break; + + case 3: // LEFT_BOTTOM + if (new_vox(0) > x_trim && new_vox(1) < new_dim(1) - y_trim) { + int old_vox_idx = new_vox(0) + + new_dim(0) * new_vox(1) + + new_dim(0) * new_dim(1) * new_vox(2); + int new_vox_trim_0 = new_vox(0) - x_trim; + int new_vox_trim_1 = new_vox(1) + y_trim; + int new_vox_idx = new_vox_trim_0 + + new_dim(0) * new_vox_trim_1 + + new_dim(0) * new_dim(1) * new_vox(2); + + new_map[new_vox_idx] = old_map[old_vox_idx]; + } + break; + + case 4: // LEFT_TOP + if (new_vox(0) > x_trim && new_vox(1) > y_trim) { + int old_vox_idx = new_vox(0) + + new_dim(0) * new_vox(1) + + new_dim(0) * new_dim(1) * new_vox(2); + int new_vox_trim_0 = new_vox(0) - x_trim; + int new_vox_trim_1 = new_vox(1) - y_trim; + int new_vox_idx = new_vox_trim_0 + + new_dim(0) * new_vox_trim_1 + + new_dim(0) * new_dim(1) * new_vox(2); + + new_map[new_vox_idx] = old_map[old_vox_idx]; + } + break; + + case 5: // LEFT + if (new_vox(0) > x_trim) { + int old_vox_idx = new_vox(0) + + new_dim(0) * new_vox(1) + + new_dim(0) * new_dim(1) * new_vox(2); + int new_vox_trim = new_vox(0) - x_trim; + int new_vox_idx = new_vox_trim + + new_dim(0) * new_vox(1) + + new_dim(0) * new_dim(1) * new_vox(2); + + new_map[new_vox_idx] = old_map[old_vox_idx]; + } + break; + + case 6: // RIGHT + if (new_vox(0) < new_dim(0) - x_trim) { + int old_vox_idx = new_vox(0) + + new_dim(0) * new_vox(1) + + new_dim(0) * new_dim(1) * new_vox(2); + int new_vox_trim = new_vox(0) + x_trim; + int new_vox_idx = new_vox_trim + + new_dim(0) * new_vox(1) + + new_dim(0) * new_dim(1) * new_vox(2); + + new_map[new_vox_idx] = old_map[old_vox_idx]; + } + break; + + case 7: // TOP + if (new_vox(1) > y_trim) { + int old_vox_idx = new_vox(0) + + new_dim(0) * new_vox(1) + + new_dim(0) * new_dim(1) * new_vox(2); + int new_vox_trim = new_vox(1) - y_trim; + int new_vox_idx = new_vox(0) + + new_dim(0) * new_vox_trim + + new_dim(0) * new_dim(1) * new_vox(2); + + new_map[new_vox_idx] = old_map[old_vox_idx]; + } + break; + + case 8: // BOTTOM + if (new_vox(1) < new_dim(1) - y_trim) { + int old_vox_idx = new_vox(0) + + new_dim(0) * new_vox(1) + + new_dim(0) * new_dim(1) * new_vox(2); + int new_vox_trim = new_vox(1) + y_trim; + int new_vox_idx = new_vox(0) + + new_dim(0) * new_vox_trim + + new_dim(0) * new_dim(1) * new_vox(2); + + new_map[new_vox_idx] = old_map[old_vox_idx]; + } + break; + + default: + break; + } + + } + } + } + + origin_d_ = new_ori_d; + dim_ = new_dim; + prev_origin = new_ori; + + map_.setMap(origin_d_, dim_, new_map, res_); + inflated_map_.setMap(origin_d_, dim_, new_map, res_); + + return true; + } +} + } // namespace mapper