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;
}
