void find_minimums(array A, int *mins) { if (height[A]==1) { mins[0]=min_index(A,0,0,A.n); } else { array evens=half(A); int even_minimums[height(evens)]; find_minimums(evens,even_minimums); int leftmost=0; for (int i=0; i<height(evens); i++) { leftmost=min_index(A, 2*i, leftmost, even_minimums[i]+1); mins[2*i] =leftmost; mins[2*i+1]=even_minimums[i]; } if (height(A)%2) { mins[height(A)-1]=min_index(A,height(A)-1,leftmost,A.n); } } }
void CostMapCalculation::sysMessageCallback(const std_msgs::String& string) { ROS_DEBUG("HectorCM: sysMsgCallback, msg contents: %s", string.data.c_str()); if (string.data == "reset") { ROS_INFO("HectorCM: reset"); // set default values cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL); elevation_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL); cloud_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL); // loop through starting area min_index(0) = cost_map.info.width/2-floor(initial_free_cells_radius/cost_map.info.resolution); min_index(1) = cost_map.info.height/2-floor(initial_free_cells_radius/cost_map.info.resolution); max_index(0) = cost_map.info.width/2+floor(initial_free_cells_radius/cost_map.info.resolution); max_index(1) = cost_map.info.height/2+floor(initial_free_cells_radius/cost_map.info.resolution); for (int v = min_index(1); v < max_index(1); ++v) for (int u = min_index(0); u < max_index(0); ++u) cost_map.data[MAP_IDX(cost_map.info.width, u, v)] = FREE_CELL; received_elevation_map = false; received_grid_map = false; received_point_cloud = false; } }
unsigned int get_max_idle_FP(tasks_schedule * t_sched, unsigned int kth_task) { unsigned int max_idle = 0; task_timeline *tl; tl = &t_sched->timeline; max_idle += t_sched->hyperperiod; /*computing W_min */ { unsigned int W_min = 0; unsigned int prev_time, prev_duration; do { next_task(t_sched); } while (has_priority_FP(t_sched->ts, kth_task, tl->id)); prev_time = t_sched->timeline.release_time; prev_duration = min_index(&t_sched->ts->task_list[tl->id]->sd); W_min = prev_duration; while (next_task(t_sched)) { if (!has_priority_FP(t_sched->ts, kth_task, tl->id)) { W_min = (W_min > tl->release_time - prev_time) ? W_min - (tl->release_time - prev_time) : 0; prev_duration = min_index(&t_sched->ts->task_list[tl->id]->sd); prev_time = t_sched->timeline.release_time; W_min += prev_duration; } } W_min = (W_min > t_sched->hyperperiod - prev_time) ? W_min - (t_sched->hyperperiod - prev_time) : 0; max_idle += W_min; } /*computing C_min */ { unsigned int C_min = 0; while (next_task(t_sched)) { if (!has_priority_FP(t_sched->ts, kth_task, tl->id)) { C_min += min_index(&t_sched->ts->task_list[tl->id]->sd); } } max_idle -= C_min; } return max_idle; }
void CostMapCalculation::updateMapParamsCallback(const nav_msgs::MapMetaData& map_meta_data) { ROS_DEBUG("HectorCM: received new map meta data -> overwrite old parameters"); // check if new parameters differ and abort otherwise if (cost_map.info.width == map_meta_data.width && cost_map.info.height == map_meta_data.height && cost_map.info.resolution == map_meta_data.resolution && cost_map.info.origin.position.x == map_meta_data.origin.position.x && cost_map.info.origin.position.y == map_meta_data.origin.position.y && cost_map.info.origin.position.z == map_meta_data.origin.position.z && cost_map.info.origin.orientation.x == map_meta_data.origin.orientation.x && cost_map.info.origin.orientation.y == map_meta_data.origin.orientation.y && cost_map.info.origin.orientation.z == map_meta_data.origin.orientation.z && cost_map.info.origin.orientation.w == map_meta_data.origin.orientation.w) { return; } // set new parameters cost_map.info.width = map_meta_data.width; cost_map.info.height = map_meta_data.height; cost_map.info.resolution = map_meta_data.resolution; cost_map.info.origin.position.x = map_meta_data.origin.position.x; cost_map.info.origin.position.y = map_meta_data.origin.position.y; cost_map.info.origin.position.z = map_meta_data.origin.position.z; cost_map.info.origin.orientation.x = map_meta_data.origin.orientation.x; cost_map.info.origin.orientation.y = map_meta_data.origin.orientation.y; cost_map.info.origin.orientation.z = map_meta_data.origin.orientation.z; cost_map.info.origin.orientation.w = map_meta_data.origin.orientation.w; world_map_transform.setTransforms(cost_map.info); // allocate memory cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL); elevation_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL); cloud_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL); // loop through starting area min_index(0) = cost_map.info.width/2-floor(initial_free_cells_radius/cost_map.info.resolution); min_index(1) = cost_map.info.height/2-floor(initial_free_cells_radius/cost_map.info.resolution); max_index(0) = cost_map.info.width/2+floor(initial_free_cells_radius/cost_map.info.resolution); max_index(1) = cost_map.info.height/2+floor(initial_free_cells_radius/cost_map.info.resolution); for (int v = min_index(1); v < max_index(1); ++v) for (int u = min_index(0); u < max_index(0); ++u) cost_map.data[MAP_IDX(cost_map.info.width, u, v)] = FREE_CELL; // update flags received_elevation_map = false; received_grid_map = false; use_cloud_map = false; }
template <typename VoxelT, typename WeightT> bool pcl::TSDFVolume<VoxelT, WeightT>::extractNeighborhood (const Eigen::Vector3i &voxel_coord, int neighborhood_size, VoxelTVec &neighborhood) const { // point_index is at the center of a cube of scale_ x scale_ x scale_ voxels int shift = (neighborhood_size - 1) / 2; Eigen::Vector3i min_index = voxel_coord.array() - shift; Eigen::Vector3i max_index = voxel_coord.array() + shift; // check that index is within range if (getLinearVoxelIndex(min_index) < 0 || getLinearVoxelIndex(max_index) >= (int)size()) { pcl::console::print_info ("[extractNeighborhood] Skipping voxel with coord (%d, %d, %d).\n", voxel_coord(0), voxel_coord(1), voxel_coord(2)); return false; } static const int descriptor_size = neighborhood_size*neighborhood_size*neighborhood_size; neighborhood.resize (descriptor_size); const Eigen::RowVector3i offset_vector (1, neighborhood_size, neighborhood_size*neighborhood_size); // loop over all voxels in 3D neighborhood #pragma omp parallel for for (int z = min_index(2); z <= max_index(2); ++z) { for (int y = min_index(1); y <= max_index(1); ++y) { for (int x = min_index(0); x <= max_index(0); ++x) { // linear voxel index in volume and index in descriptor vector Eigen::Vector3i point (x,y,z); int volume_idx = getLinearVoxelIndex (point); int descr_idx = offset_vector * (point - min_index); /* std::cout << "linear index " << volume_idx << std::endl; std::cout << "weight " << weights_->at (volume_idx) << std::endl; std::cout << "volume " << volume_->at (volume_idx) << std::endl; std::cout << "descr " << neighborhood.rows() << "x" << neighborhood.cols() << ", val = " << neighborhood << std::endl; std::cout << "descr index = " << descr_idx << std::endl; */ // get the TSDF value and store as descriptor entry if (weights_->at (volume_idx) != 0) neighborhood (descr_idx) = volume_->at (volume_idx); else neighborhood (descr_idx) = -1.0; // if never visited we assume inside of object (outside captured and thus filled with positive values) } } } return true; }
bool CostMapCalculation::computeWindowIndices(ros::Time time,double update_radius) { int update_radius_map; Eigen::Vector2f index_world, index_map; // window update region // only update cells within the max a curtain radius of current robot position tf::StampedTransform transform; // get local map transform try { listener.waitForTransform(map_frame_id, local_transform_frame_id, time, ros::Duration(5)); listener.lookupTransform(map_frame_id, local_transform_frame_id, time, transform); } catch (tf::TransformException ex) { ROS_ERROR("%s",ex.what()); return false; } index_world(0) = transform.getOrigin().x(); index_world(1) = transform.getOrigin().y(); index_map = world_map_transform.getC2Coords(index_world); update_radius_map = floor(((double)update_radius/(double)cost_map.info.resolution)); // compute window min/max index if(index_map(1) < update_radius_map) min_index(1) = 0; else min_index(1) = index_map(1) - update_radius_map; if(index_map(1) + update_radius_map > cost_map.info.height) max_index(1) = cost_map.info.height; else max_index(1) = index_map(1) + update_radius_map; if(index_map(0) < update_radius_map) min_index(0) = 0; else min_index(0) = index_map(0) - update_radius_map; if(index_map(0) + update_radius_map > cost_map.info.width) max_index(0) = cost_map.info.width; else max_index(0) = index_map(0) + update_radius_map; return true; }
template <typename VoxelT, typename WeightT> bool pcl::TSDFVolume<VoxelT, WeightT>::addNeighborhood (const Eigen::Vector3i &voxel_coord, int neighborhood_size, const VoxelTVec &neighborhood, WeightT voxel_weight) { // point_index is at the center of a cube of scale_ x scale_ x scale_ voxels int shift = (neighborhood_size - 1) / 2; Eigen::Vector3i min_index = voxel_coord.array() - shift; Eigen::Vector3i max_index = voxel_coord.array() + shift; // check that index is within range if (getLinearVoxelIndex(min_index) < 0 || getLinearVoxelIndex(max_index) >= (int)size()) { pcl::console::print_info ("[addNeighborhood] Skipping voxel with coord (%d, %d, %d).\n", voxel_coord(0), voxel_coord(1), voxel_coord(2)); return false; } // static const int descriptor_size = neighborhood_size*neighborhood_size*neighborhood_size; const Eigen::RowVector3i offset_vector (1, neighborhood_size, neighborhood_size*neighborhood_size); Eigen::Vector3i index = min_index; // loop over all voxels in 3D neighborhood #pragma omp parallel for for (int z = min_index(2); z <= max_index(2); ++z) { for (int y = min_index(1); y <= max_index(1); ++y) { for (int x = min_index(0); x <= max_index(0); ++x) { // linear voxel index in volume and index in descriptor vector Eigen::Vector3i point (x,y,z); int volume_idx = getLinearVoxelIndex (point); int descr_idx = offset_vector * (point - min_index); // add the descriptor entry to the volume VoxelT &voxel = volume_->at (volume_idx); WeightT &weight = weights_->at (volume_idx); // TODO check that this simple lock works correctly!! #pragma omp atomic voxel += neighborhood (descr_idx); #pragma omp atomic weight += voxel_weight; } } } return true; }
int main() { int array[MAX_SIZE], size, index,i; scanf("%d", &size); get_array(array, size); index = min_index(array, size); for(i=0;i<size;i++) printf("==%d,%d==\n",array[i],i); printf("minimum number is %d, whose position is %d.", array[index], index); return 0; }
function_algebra_rotation3D(const coord_type& ax, double a) : axis(ap3d::normalization(ax)), e1(3, coord_type::no_init), e2(3, coord_type::no_init), alpha(a), cosa(cos(a)), sina(sin(a)) { d_Def = 3;d_Im = 3; is_affine = true; int i = min_index(axis); int j=(i==1 ? 2 : 1); e2[i] = 0; e2[j] = -axis[6-i-j]; e2[6-i-j] = axis[j]; // now e2*axis = 0 e1 = ap3d::vectorproduct(e2,axis); if(ap3d::det3(e1,e2,axis) < 0) e1 *= -1; }
int main(int argc, char *argv[]) { int idx[3] = {285,165,143}; long val[3]; pfunc_t func[3] = {func_t, func_p, func_h}; int i; /* initialize T(2),P(2),H(2) */ for (i = 0; i < 3; i++) val[i] = func[i](idx[i]); while (1) { i = min_index(val, NARRAY(val)); idx[i]++; val[i] = func[i](idx[i]); if (equal(val, NARRAY(val))) break; } printf("%ld\n", val[0]); return 0; }
void DateSet::remove(int b, int e) { data_.remove_at(b - min_index(), e - min_index()); }
CostMapCalculation::CostMapCalculation() : nHandle(), pnHandle("~") { int width, height; double grid_res_xy; pnHandle.param("elevation_resolution", grid_res_z, 0.01); //[m] pnHandle.param("max_grid_size_x", width, 1024); //[cell] cost_map.info.width = width; pnHandle.param("max_grid_size_y", height, 1024); //[cell] cost_map.info.height = height; pnHandle.param("map_resolution", grid_res_xy, 0.05); //[m] cost_map.info.resolution = grid_res_xy; pnHandle.param("origin_x",cost_map.info.origin.position.x, -(double)cost_map.info.width*(double)cost_map.info.resolution/2.0); //[m] pnHandle.param("origin_y",cost_map.info.origin.position.y, -(double)cost_map.info.height*(double)cost_map.info.resolution/2.0); //[m] pnHandle.param("origin_z",cost_map.info.origin.position.z, 0.0); //[m] pnHandle.param("orientation_x",cost_map.info.origin.orientation.x, 0.0); //[] pnHandle.param("orientation_y",cost_map.info.origin.orientation.y, 0.0); //[] pnHandle.param("orientation_z",cost_map.info.origin.orientation.z, 0.0); //[] pnHandle.param("orientation_w",cost_map.info.origin.orientation.w, 1.0); //[] pnHandle.param("initial_free_cells_radius", initial_free_cells_radius, 0.30); //[m] pnHandle.param("update_radius", update_radius_world, 4.0); //[m] pnHandle.param("max_delta_elevation", max_delta_elevation, 0.08); //[m] pnHandle.param("map_frame_id", map_frame_id,std::string("map")); pnHandle.param("local_transform_frame_id", local_transform_frame_id,std::string("base_footprint")); pnHandle.param("cost_map_topic", cost_map_topic, std::string("cost_map")); pnHandle.param("elevation_map_topic", elevation_map_topic, std::string("elevation_map_local")); pnHandle.param("grid_map_topic", grid_map_topic, std::string("scanmatcher_map")); pnHandle.param("use_elevation_map", use_elevation_map, true); pnHandle.param("use_grid_map", use_grid_map, true); pnHandle.param("use_cloud_map", use_cloud_map, false); pnHandle.param("allow_elevation_map_to_clear_occupied_cells", allow_elevation_map_to_clear_occupied_cells, true); pnHandle.param("max_clear_size", max_clear_size, 2); pnHandle.param("point_cloud_topic", point_cloud_topic,std::string("openni/depth/points")); pnHandle.param("slize_min_height", slize_min_height, 0.30); //[m] pnHandle.param("slize_max_height", slize_min_height, 0.35); //[m] pnHandle.param("costmap_pub_freq", costmap_pub_freq, 4.0); //[Hz] pnHandle.param("sys_msg_topic", sys_msg_topic, std::string("syscommand")); cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL); elevation_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL); cloud_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL); world_map_transform.setTransforms(cost_map.info); // loop through starting area min_index(0) = cost_map.info.width/2-floor(initial_free_cells_radius/cost_map.info.resolution); min_index(1) = cost_map.info.height/2-floor(initial_free_cells_radius/cost_map.info.resolution); max_index(0) = cost_map.info.width/2+floor(initial_free_cells_radius/cost_map.info.resolution); max_index(1) = cost_map.info.height/2+floor(initial_free_cells_radius/cost_map.info.resolution); for (int v = min_index(1); v < max_index(1); ++v) for (int u = min_index(0); u < max_index(0); ++u) cost_map.data[MAP_IDX(cost_map.info.width, u, v)] = FREE_CELL; pub_cost_map = nHandle.advertise<nav_msgs::OccupancyGrid>(cost_map_topic, 1, true); received_elevation_map = false; received_grid_map = false; received_point_cloud = false; sliced_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>); if(use_elevation_map) sub_elevation_map = nHandle.subscribe(elevation_map_topic,10,&CostMapCalculation::callbackElevationMap,this); if(use_grid_map) sub_grid_map = nHandle.subscribe(grid_map_topic,10,&CostMapCalculation::callbackGridMap,this); if(use_cloud_map) sub_point_cloud = nHandle.subscribe(point_cloud_topic,10,&CostMapCalculation::callbackPointCloud,this); sub_map_info = nHandle.subscribe("map_metadata",1,&CostMapCalculation::updateMapParamsCallback,this); sub_sysMessage = nHandle.subscribe(sys_msg_topic, 10, &CostMapCalculation::sysMessageCallback, this); dyn_rec_server_.setCallback(boost::bind(&CostMapCalculation::dynRecParamCallback, this, _1, _2)); timer = pnHandle.createTimer(ros::Duration(1.0/costmap_pub_freq), &CostMapCalculation::timerCallback,this); ROS_INFO("HectorCM: is up and running."); pub_cloud_slice = pnHandle.advertise<sensor_msgs::PointCloud2>("cloud_slice",1,true); ros::spin(); }
GDate DateSet::data(int i) const { return data_[i - min_index()]; }
void DateSet::remove(int i) { data_.remove_at(i - min_index()); }
void DateSet::add_at(int i, GDate dt) { data_.add_at(i - min_index(), dt); }
void DateSet::set(int i, GDate dt) { data_[i - min_index()] = dt; }
mat vqtrain(Array<vec> &DB, int SIZE, int NOITER, double STARTSTEP, bool VERBOSE) { int DIM = DB(0).length(); vec x; vec codebook(DIM*SIZE); int n, MinIndex, i, j; double MinS, S, D, step, *xp, *cp; for (i = 0;i < SIZE;i++) { codebook.replace_mid(i*DIM, DB(randi(0, DB.length() - 1))); } if (VERBOSE) std::cout << "Training VQ..." << std::endl ; res: D = 0; for (n = 0;n < NOITER;n++) { step = STARTSTEP * (1.0 - double(n) / NOITER); if (step < 0) step = 0; x = DB(randi(0, DB.length() - 1)); // seems unnecessary! Check it up. xp = x._data(); MinS = 1E20; MinIndex = 0; for (i = 0;i < SIZE;i++) { cp = &codebook(i * DIM); S = sqr(xp[0] - cp[0]); for (j = 1;j < DIM;j++) { S += sqr(xp[j] - cp[j]); if (S >= MinS) goto sune; } MinS = S; MinIndex = i; sune: i = i; } D += MinS; cp = &codebook(MinIndex * DIM); for (j = 0;j < DIM;j++) { cp[j] += step * (xp[j] - cp[j]); } if ((n % 20000 == 0) && (n > 1)) { if (VERBOSE) std::cout << n << ": " << D / 20000 << " "; D = 0; } } // checking training result vec dist(SIZE), num(SIZE); dist.clear(); num.clear(); for (n = 0;n < DB.length();n++) { x = DB(n); xp = x._data(); MinS = 1E20; MinIndex = 0; for (i = 0;i < SIZE;i++) { cp = &codebook(i * DIM); S = sqr(xp[0] - cp[0]); for (j = 1;j < DIM;j++) { S += sqr(xp[j] - cp[j]); if (S >= MinS) goto sune2; } MinS = S; MinIndex = i; sune2: i = i; } dist(MinIndex) += MinS; num(MinIndex) += 1; } dist = 10 * log10(dist * dist.length() / sum(dist)); if (VERBOSE) std::cout << std::endl << "Distortion contribution: " << dist << std::endl ; if (VERBOSE) std::cout << "Num spread: " << num / DB.length()*100 << " %" << std::endl << std::endl ; if (min(dist) < -30) { std::cout << "Points without entries! Retraining" << std::endl ; j = min_index(dist); i = max_index(num); codebook.replace_mid(j*DIM, codebook.mid(i*DIM, DIM)); goto res; } mat cb(DIM, SIZE); for (i = 0;i < SIZE;i++) { cb.set_col(i, codebook.mid(i*DIM, DIM)); } return cb; }
bool CostMapCalculation::calculateCostMap(char map_level) { if (!map_level) { ROS_WARN("Invalid map selection was given to cost map calculation!"); return false; } if (map_level & GRID_MAP) ROS_DEBUG("HectorCM: compute costmap based on grid map"); if (map_level & ELEVATION_MAP) ROS_DEBUG("HectorCM: compute costmap based on elevation map"); if (map_level & CLOUD_MAP) ROS_DEBUG("HectorCM: compute costmap based on cloud map"); // loop through each element for (int v = min_index(1); v < max_index(1); ++v) { for (int u = min_index(0); u < max_index(0); ++u) { unsigned int index = MAP_IDX(cost_map.info.width, u, v); int checksum_grid_map = 0; cost_map.data[index] = UNKNOWN_CELL; // grid map if (map_level & GRID_MAP) { switch (grid_map_.data[index]) { case OCCUPIED_CELL: if (map_level & ELEVATION_MAP && allow_elevation_map_to_clear_occupied_cells) { checksum_grid_map += grid_map_.at<int8_t>(v-1, u ); checksum_grid_map += grid_map_.at<int8_t>(v+1, u ); checksum_grid_map += grid_map_.at<int8_t>(v, u-1); checksum_grid_map += grid_map_.at<int8_t>(v, u+1); checksum_grid_map += grid_map_.at<int8_t>(v+1, u+1); checksum_grid_map += grid_map_.at<int8_t>(v-1, u-1); checksum_grid_map += grid_map_.at<int8_t>(v+1, u-1); checksum_grid_map += grid_map_.at<int8_t>(v-1, u+1); } cost_map.data[index] = OCCUPIED_CELL; break; case FREE_CELL: cost_map.data[index] = FREE_CELL; break; } } // point cloud if (map_level & CLOUD_MAP) { if (cost_map.data[index] != OCCUPIED_CELL) { switch (cloud_cost_map.data[index]) { case OCCUPIED_CELL: cost_map.data[index] = OCCUPIED_CELL; break; case FREE_CELL: cost_map.data[index] = FREE_CELL; break; } } } // elevation map if (map_level & ELEVATION_MAP) { if (cost_map.data[index] != OCCUPIED_CELL || (allow_elevation_map_to_clear_occupied_cells && checksum_grid_map <= max_clear_size*OCCUPIED_CELL)) { switch (elevation_cost_map.data[index]) { case OCCUPIED_CELL: cost_map.data[index] = OCCUPIED_CELL; break; case FREE_CELL: cost_map.data[index] = FREE_CELL; break; } } } } } ROS_DEBUG("HectorCM: computed a new costmap"); return true; }
bool CostMapCalculation::calculateCostMap_old(char map_level) { switch(map_level) { case USE_GRID_MAP_ONLY: { // cost map based on grid map only ROS_DEBUG("HectorCM: compute costmap based on grid map"); // loop through each element for (int v = min_index(1); v < max_index(1); ++v) { for (int u = min_index(0); u < max_index(0); ++u) { unsigned int index = MAP_IDX(cost_map.info.width, u, v); // check if cell is known if((char)grid_map_.data[index] != UNKNOWN_CELL) { if(grid_map_.data[index] == OCCUPIED_CELL) { // cell is occupied cost_map.data[index] = OCCUPIED_CELL; } else { // cell is not occupied cost_map.data[index] = FREE_CELL; } } } } break; } case USE_ELEVATION_MAP_ONLY: { // cost map based on elevation map only ROS_DEBUG("HectorCM: compute costmap based on elevation map"); // loop through each element for (int v = min_index(1); v < max_index(1); ++v) { for (int u = min_index(0); u < max_index(0); ++u) { unsigned int index = MAP_IDX(cost_map.info.width, u, v); // check if cell is known if(elevation_cost_map.data[index] != UNKNOWN_CELL) { if(elevation_cost_map.data[index] == OCCUPIED_CELL) { // cell is occupied cost_map.data[index] = OCCUPIED_CELL; } else { // cell is not occupied cost_map.data[index] = FREE_CELL; } } } } break; } case USE_GRID_AND_ELEVATION_MAP: { // cost map based on elevation and grid map ROS_DEBUG("HectorCM: compute costmap based on grid and elevation map"); int checksum_grid_map; // loop through each element for (int v = min_index(1); v < max_index(1); ++v) { for (int u = min_index(0); u < max_index(0); ++u) { unsigned int index = MAP_IDX(cost_map.info.width, u, v); // check if cell is known if(grid_map_.at<int8_t>(v,u) != UNKNOWN_CELL) { if(grid_map_.at<int8_t>(v,u) == OCCUPIED_CELL || elevation_cost_map.data[index] == OCCUPIED_CELL) { checksum_grid_map = 0; checksum_grid_map += grid_map_.at<int8_t>(v-1, u); checksum_grid_map += grid_map_.at<int8_t>(v+1, u); checksum_grid_map += grid_map_.at<int8_t>(v, u-1); checksum_grid_map += grid_map_.at<int8_t>(v, u+1); checksum_grid_map += grid_map_.at<int8_t>(v+1, u+1); checksum_grid_map += grid_map_.at<int8_t>(v-1, u-1); checksum_grid_map += grid_map_.at<int8_t>(v+1, u-1); checksum_grid_map += grid_map_.at<int8_t>(v-1, u+1); if(elevation_cost_map.data[index] == FREE_CELL && allow_elevation_map_to_clear_occupied_cells) { if(checksum_grid_map <= max_clear_size*OCCUPIED_CELL) { // cell is free cost_map.data[index] = FREE_CELL; } else { // cell is occupied cost_map.data[index] = OCCUPIED_CELL; } } else { // cell is occupied cost_map.data[index] = OCCUPIED_CELL; } } else { cost_map.data[index] = FREE_CELL; } } } } break; } case USE_GRID_AND_CLOUD_MAP: { // cost map based on cloud map and grid map ROS_DEBUG("HectorCM: compute costmap based on grid and cloud map"); // loop through each element for (int v = min_index(1); v < max_index(1); ++v) { for (int u = min_index(0); u < max_index(0); ++u) { unsigned int index = MAP_IDX(cost_map.info.width, u, v); // check if cell is known if(grid_map_.at<int8_t>(v,u) != UNKNOWN_CELL) { if(grid_map_.at<int8_t>(v,u) == OCCUPIED_CELL || cloud_cost_map.data[index] == OCCUPIED_CELL) { // cell is occupied cost_map.data[index] = OCCUPIED_CELL; } else { cost_map.data[index] = FREE_CELL; } } } } break; } case USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP: { // cost map based on elevation, cloud and grid map ROS_DEBUG("HectorCM: compute costmap based on grid, cloud and elevation map"); int checksum_grid_map; // loop through each element for (int v = min_index(1); v < max_index(1); ++v) { for (int u = min_index(0); u < max_index(0); ++u) { unsigned int index = MAP_IDX(cost_map.info.width, u, v); // check if cell is known if(grid_map_.at<int8_t>(v,u) != UNKNOWN_CELL) { if(grid_map_.at<int8_t>(v,u) == OCCUPIED_CELL || elevation_cost_map.data[index] == OCCUPIED_CELL) { checksum_grid_map = 0; checksum_grid_map += grid_map_.at<int8_t>(v-1, u); checksum_grid_map += grid_map_.at<int8_t>(v+1, u); checksum_grid_map += grid_map_.at<int8_t>(v, u-1); checksum_grid_map += grid_map_.at<int8_t>(v, u+1); checksum_grid_map += grid_map_.at<int8_t>(v+1, u+1); checksum_grid_map += grid_map_.at<int8_t>(v-1, u-1); checksum_grid_map += grid_map_.at<int8_t>(v+1, u-1); checksum_grid_map += grid_map_.at<int8_t>(v-1, u+1); if(elevation_cost_map.data[index] == FREE_CELL && allow_elevation_map_to_clear_occupied_cells) { if(checksum_grid_map <= max_clear_size*OCCUPIED_CELL) { // cell is free cost_map.data[index] = FREE_CELL; } else { // cell is occupied cost_map.data[index] = OCCUPIED_CELL; } } else { // cell is occupied cost_map.data[index] = OCCUPIED_CELL; } } else { if(cloud_cost_map.data[index] == OCCUPIED_CELL) { // cell is occupied cost_map.data[index] = OCCUPIED_CELL; } else { // cell is free cost_map.data[index] = FREE_CELL; } } } } } break; } } ROS_DEBUG("HectorCM: computed a new costmap"); return true; }
void CostMapCalculation::callbackElevationMap(const hector_elevation_msgs::ElevationGridConstPtr& elevation_map_msg) { ROS_DEBUG("HectorCM: received new elevation map"); // check header if((int)(1000*elevation_map_msg->info.resolution_xy) != (int)(1000*cost_map.info.resolution) && // Magic number 1000 -> min grid size should not be less than 1mm elevation_map_msg->info.height != cost_map.info.height && elevation_map_msg->info.width != cost_map.info.width) { ROS_ERROR("HectorCM: elevation map resolution and or size incorrect!"); return; } // store elevation_map_msg in local variable elevation_map_ = cv::Mat(elevation_map_msg->info.height, elevation_map_msg->info.width, CV_16S, const_cast<int16_t*>(&elevation_map_msg->data[0]), 2*(size_t)elevation_map_msg->info.width); // store elevation map zero level elevation_zero_level = elevation_map_msg->info.zero_elevation; // compute region of intereset if(!computeWindowIndices(elevation_map_msg->header.stamp, update_radius_world)) return; // loop through each element int filtered_cell, filtered_cell_x, filtered_cell_y; for (int v = min_index(1); v < max_index(1); ++v) { for (int u = min_index(0); u < max_index(0); ++u) { // compute cost_map_index unsigned int cost_map_index = MAP_IDX(cost_map.info.width, u, v); // check if neighbouring cells are known if(elevation_map_.at<int16_t>(v+1,u) == (-elevation_zero_level) || elevation_map_.at<int16_t>(v-1,u) == (-elevation_zero_level) || elevation_map_.at<int16_t>(v,u+1) == (-elevation_zero_level) || elevation_map_.at<int16_t>(v,u-1) == (-elevation_zero_level)) continue; // edge filter filtered_cell_y = abs(elevation_map_.at<int16_t>(v,u-1) - elevation_map_.at<int16_t>(v,u+1)); filtered_cell_x = abs(elevation_map_.at<int16_t>(v-1,u) - elevation_map_.at<int16_t>(v+1,u)); if(filtered_cell_x > filtered_cell_y) filtered_cell = filtered_cell_x; else filtered_cell = filtered_cell_y; // check if cell is traversable if(filtered_cell > max_delta_elevation/grid_res_z) { // cell is not traversable -> mark it as occupied elevation_cost_map.data[cost_map_index] = OCCUPIED_CELL; } else { // cell is traversable -> mark it as free elevation_cost_map.data[cost_map_index] = FREE_CELL; } } } // set elevation map received flag received_elevation_map = true; // calculate cost map if(received_grid_map) { if(received_point_cloud) { calculateCostMap(USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP); } else { calculateCostMap(USE_GRID_AND_ELEVATION_MAP); } } else { calculateCostMap(USE_ELEVATION_MAP_ONLY); } }
static int am_p_min_fm(double *p,double *cnr,int *b,int n,int rate,double gamma,int half,double *p_used) { int i,tone,b_total=0,slice; int tone_full[n]; // set to 1 if tone is at MAXBITSPERTONE //double gamma = 9.8; double margin = MARGIN; double c_g = C_G; double gamma_hat = pow(10,(gamma+margin-c_g)/10); double delta_p[n]; double p_tot = 0.0,p_budget; // 0.1 W = 100mW char str[15]; for (i=0;i<n;i++) { b[i] = 0; // zero all bits before loading tone_full[i] = 0; // all tones are not full p[i] = 0; // zero all powers before loading } if (half == TOP_TONES) { strcpy(str,"TOP TONES"); p_budget=tot_p_budget-*p_used; } else if (half == BOTTOM_TONES) { strcpy(str,"BOTTOM TONES"); p_budget=tot_p_budget; } for (i=0;i<n;i++) { // just calculate full delta_p array once then update one member at a time inside loop if (!tone_full[i]) { delta_p[i] = (pow(2,(b[i] + 1))-1) * gamma_hat/cnr[i] - (pow(2,b[i])-1) * gamma_hat/cnr[i]; } else { delta_p[i] = 100; } } while (1) { if ((tone = min_index(delta_p,n)) == -1) { printf("All tones are full!\n"); printf("rate = %d\n",b_total); break; } b[tone]++; b_total++; p_tot += delta_p[tone]; p[tone] += delta_p[tone]; if ((half == BOTTOM_TONES) && (b_total == rate)) { break; } //printf("Added a bit on tone %d\n",tone); if (b[tone] == MAXBITSPERTONE) tone_full[tone] = 1; if (p_tot > p_budget) { b[tone]--; b_total--; p_tot -= delta_p[tone]; p[tone] -= delta_p[tone]; break; } if (!tone_full[tone]) { delta_p[tone] = (pow(2,(b[tone] + 1))-1) * gamma_hat/cnr[tone] - (pow(2,b[tone])-1) * gamma_hat/cnr[tone]; } else { delta_p[tone] = 100; } } if (half == TOP_TONES) { slice = n; if (b_total != rate) { //printf("could not reach target data rate for p_budget = %lf\nb_total = %d\trate = %d\tslice = %d\n",p_budget,b_total,rate,slice); b_total=-1; // return negative if could not reach target rate } } else if (half == BOTTOM_TONES) { slice = DMTCHANNELS - n; *p_used=p_tot; } return b_total; }