void Read(
std::istream& fin, double minsep, double binsize,
std::vector<CellData>& celldata,
std::vector<std::vector<CellData> >& expcelldata, double& vare)
{
double sumw=0.;
vare=0.;
int j;
double x,y,e1,e2,w;
while(fin >> j>>x>>y>>e1>>e2>>w) {
// Note: e1,e2 are really gamma_1, gamma_2
// _NOT_ observed ellipticities
Position2D pos(x,y);
std::complex<double> e(e1,e2);
sumw += w;
vare += w*w*norm(e);
celldata.push_back(CellData(pos,e,w));
while(j >= int(expcelldata.size()))
expcelldata.push_back(std::vector<CellData>());
expcelldata[j].push_back(CellData(pos,e,w));
}
int ngal = celldata.size();
vare /= 2*sumw*sumw/ngal; // 2 because want var per component
dbg<<"ngal = "<<ngal<<", totw = "<<sumw<<std::endl;
}
Mod::Mod(const Position pos)
{
Position northWestPos = Position(pos.x - 1, pos.y + 1);
northWest = CellData(northWestPos, World::getInstance().getTerrain(northWestPos));
Position northPos = Position(pos.x, pos.y + 1);
north = CellData(northPos, World::getInstance().getTerrain(northPos));
Position northEastPos = Position(pos.x + 1, pos.y + 1);
northEast = CellData(northEastPos, World::getInstance().getTerrain(northEastPos));
Position westPos = Position(pos.x - 1, pos.y);
west = CellData(westPos, World::getInstance().getTerrain(westPos));
Position eastPos = Position(pos.x + 1, pos.y);
east = CellData(eastPos, World::getInstance().getTerrain(eastPos));
Position southWestPos = Position(pos.x - 1, pos.y - 1);
southWest = CellData(southWestPos, World::getInstance().getTerrain(southWestPos));
Position southPos = Position(pos.x, pos.y - 1);
south = CellData(southPos, World::getInstance().getTerrain(southPos));
Position southEastPos = Position(pos.x + 1, pos.y - 1);
southEast = CellData(southEastPos, World::getInstance().getTerrain(southEastPos));
destroyWorld();
}
std::pair<TRasterP, TCacheResource::CellData *> TCacheResource::touch(const PointLess &cellIndex)
{
std::string cellId(getCellCacheId(cellIndex.x, cellIndex.y));
std::map<PointLess, CellData>::iterator it = m_cellDatas.find(cellIndex);
if (it != m_cellDatas.end()) {
//Retrieve the raster from image cache
TImageP img(TImageCache::instance()->get(cellId, true));
if (img)
return std::make_pair(getRaster(img), &it->second);
}
it = m_cellDatas.insert(std::make_pair(cellIndex, CellData())).first;
//Then, attempt retrieval from back resource
TRasterP ras(load(cellIndex));
if (ras) {
TImageCache::instance()->add(cellId, TRasterImageP(ras));
return std::make_pair(ras, &it->second);
}
//Else, create it
return std::make_pair(
createCellRaster(m_tileType, cellId), //increases m_cellsCount too
&it->second);
}
void _notifier_update_cells(VisibilityNotifier2D *p_notifier,const Rect2& p_rect,bool p_add) {
Point2i begin = p_rect.pos;
begin/=cell_size;
Point2i end = p_rect.pos+p_rect.size;
end/=cell_size;
for(int i=begin.x;i<=end.x;i++) {
for(int j=begin.y;j<=end.y;j++) {
CellKey ck;
ck.x=i;
ck.y=j;
Map<CellKey,CellData>::Element *E=cells.find(ck);
if (p_add) {
if (!E)
E=cells.insert(ck,CellData());
E->get().notifiers[p_notifier].inc();
} else {
ERR_CONTINUE(!E);
if (E->get().notifiers[p_notifier].dec()==0) {
E->get().notifiers.erase(p_notifier);
if (E->get().notifiers.empty()) {
cells.erase(E);
}
}
}
}
}
}
void renderLine(const geo::Vector3& p1, const geo::Vector3& p2)
{
geo::Vector3 v_diff = p2 - p1;
double n_diff = v_diff.length();
geo::Vector3 vd = v_diff / n_diff * res_;
int n = n_diff / res_ + 1;
geo::Vector3 p = p1;
for(int i = 0; i < n; ++i)
{
int x = -p.y / res_ + grid_size_ / 2;
int y = -p.x / res_ + grid_size_ / 2;
if (x >= 0 && y >= 0 && x < grid_size_ && y <grid_size_) {
int index = y * grid_size_ + x;
Q_.push(CellData(index, 0, 0, 0));
}
p += vd;
}
p_min.x = std::min(p_min.x, std::min(p1.x, p2.x));
p_max.x = std::max(p_max.x, std::max(p1.x, p2.x));
p_min.y = std::min(p_min.y, std::min(p1.y, p2.y));
p_max.y = std::max(p_max.y, std::max(p1.y, p2.y));
}
/**
* @brief Given an index of a cell in the costmap, place it into a list pending for obstacle inflation
* @param grid The costmap
* @param index The index of the cell
* @param mx The x coordinate of the cell (can be computed from the index, but saves time to store it)
* @param my The y coordinate of the cell (can be computed from the index, but saves time to store it)
* @param src_x The x index of the obstacle point inflation started at
* @param src_y The y index of the obstacle point inflation started at
*/
inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
unsigned int src_x, unsigned int src_y)
{
if (!seen_[index])
{
// we compute our distance table one cell further than the inflation radius dictates so we can make the check below
double distance = distanceLookup(mx, my, src_x, src_y);
// we only want to put the cell in the list if it is within the inflation radius of the obstacle point
if (distance > cell_inflation_radius_)
return;
// push the cell data onto the inflation list and mark
inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
}
}
void LocalizationPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& req)
{
last_laser_msg_.reset();
cb_queue_.callAvailable();
if (!last_laser_msg_)
return;
tue::Timer timer;
timer.start();
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// - Update sensor model
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if (lrf_.getNumBeams() != last_laser_msg_->ranges.size())
{
lrf_.setNumBeams(last_laser_msg_->ranges.size());
lrf_.setRangeLimits(last_laser_msg_->range_min, last_laser_msg_->range_max);
lrf_.setAngleLimits(last_laser_msg_->angle_min, last_laser_msg_->angle_max);
}
std::vector<double> sensor_ranges(last_laser_msg_->ranges.size());
for (unsigned int i = 0; i < last_laser_msg_->ranges.size(); ++i)
sensor_ranges[i] = last_laser_msg_->ranges[i];
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// - Create world model cross section
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
std::vector<ed::EntityConstPtr> entities;
for(ed::WorldModel::const_iterator it = world.begin(); it != world.end(); ++it)
{
// if (it->second->id() == "pico_case")
if (it->second->shape())
entities.push_back(it->second);
}
geo::Pose3D laser_pose;
laser_pose.t = laser_pose_.t + geo::Vector3(0, 0, 0);
laser_pose.R.setRPY(0, 0, 0);
double grid_resolution = 0.025;
int grid_size = 800;
std::queue<CellData> Q;
LineRenderResult render_result(Q, grid_size, grid_resolution);
for(std::vector<ed::EntityConstPtr>::const_iterator it = entities.begin(); it != entities.end(); ++it)
{
const ed::EntityConstPtr& e = *it;
geo::LaserRangeFinder::RenderOptions options;
geo::Transform t_inv = laser_pose.inverse() * e->pose();
options.setMesh(e->shape()->getMesh(), t_inv);
lrf_.render(options, render_result);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// - Create distance map
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
double max_dist = 0.3;
cv::Mat distance_map(grid_size, grid_size, CV_32FC1, (max_dist / grid_resolution) * (max_dist / grid_resolution));
for(int i = 0; i < grid_size; ++i)
{
distance_map.at<float>(i, 0) = 0;
distance_map.at<float>(i, grid_size - 1) = 0;
distance_map.at<float>(0, i) = 0;
distance_map.at<float>(grid_size - 1, i) = 0;
}
std::vector<KernelCell> kernel;
kernel.push_back(KernelCell(-distance_map.cols, 0, -1));
kernel.push_back(KernelCell(distance_map.cols, 0, 1));
kernel.push_back(KernelCell(-1, -1, 0));
kernel.push_back(KernelCell( 1, 1, 0));
while(!Q.empty())
{
const CellData& c = Q.front();
double current_distance = distance_map.at<float>(c.index);
if (c.distance < current_distance)
{
distance_map.at<float>(c.index) = c.distance;
for(unsigned int i = 0; i < kernel.size(); ++i)
{
const KernelCell& kc = kernel[i];
int new_index = c.index + kc.d_index;
int dx_new = c.dx + kc.dx;
int dy_new = c.dy + kc.dy;
int new_distance = (dx_new * dx_new) + (dy_new * dy_new);
Q.push(CellData(new_index, dx_new, dy_new, new_distance));
}
}
Q.pop();
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// - Create samples
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
std::vector<geo::Pose3D> poses;
if (!pose_initialized_)
{
// Uniform sampling over world
for(double x = render_result.p_min.x; x < render_result.p_max.x; x += 0.2)
{
for(double y = render_result.p_min.y; y < render_result.p_max.y; y += 0.2)
{
for(double a = 0; a < 6.28; a += 0.1)
{
geo::Pose3D laser_pose;
laser_pose.t = geo::Vector3(x, y, 0);
laser_pose.R.setRPY(0, 0, a);
poses.push_back(laser_pose);
}
}
}
}
else
{
poses.push_back(best_laser_pose_);
for(double dx = -0.3; dx < 0.3; dx += 0.1)
{
for(double dy = -0.3; dy < 0.3; dy += 0.1)
{
for(double da = -1; da < 1; da += 0.1)
{
geo::Pose3D dT;
dT.t = geo::Vector3(dx, dy, 0);
dT.R.setRPY(0, 0, da);
poses.push_back(best_laser_pose_ * dT);
}
}
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// - Test samples and find sample with lowest error
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
std::vector<geo::Vector3> sensor_points;
lrf_.rangesToPoints(sensor_ranges, sensor_points);
int i_step = 1;
if (poses.size() > 10000)
i_step = sensor_points.size() / 100; // limit to 100 beams
std::cout << "i_step = " << i_step << std::endl;
double min_sum_sq_error = 1e10;
for(std::vector<geo::Pose3D>::const_iterator it = poses.begin(); it != poses.end(); ++it)
{
const geo::Pose3D& laser_pose = *it;
double z1 = laser_pose.R.xx / grid_resolution;
double z2 = laser_pose.R.xy / grid_resolution;
double z3 = laser_pose.R.yx / grid_resolution;
double z4 = laser_pose.R.yy / grid_resolution;
double t1 = laser_pose.t.x / grid_resolution - distance_map.cols / 2;
double t2 = laser_pose.t.y / grid_resolution - distance_map.cols / 2;
double sum_sq_error = 0;
for(unsigned int i = 0; i < sensor_points.size(); i += i_step)
{
const geo::Vector3& v = sensor_points[i];
double py = z3 * v.x + z4 * v.y + t2;
int mx = -py;
if (mx > 0 && mx < grid_size)
{
double px = z1 * v.x + z2 * v.y + t1;
int my = -px;
if (my > 0 && my < grid_size)
{
sum_sq_error += distance_map.at<float>(my, mx);
}
}
}
if (sum_sq_error < min_sum_sq_error)
{
min_sum_sq_error = sum_sq_error;
best_laser_pose_ = laser_pose;
pose_initialized_ = true;
}
}
std::cout << min_sum_sq_error << std::endl;
std::cout << best_laser_pose_ << std::endl;
std::cout << timer.getElapsedTimeInMilliSec() << " ms" << std::endl;
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// - Visualization
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
bool visualize = true;
if (visualize)
{
cv::Mat rgb_image(distance_map.rows, distance_map.cols, CV_8UC3, cv::Scalar(0, 0, 0));
for(int y = 0; y < rgb_image.rows; ++y)
{
for(int x = 0; x < rgb_image.cols; ++x)
{
int c = distance_map.at<float>(y, x) / ((max_dist / grid_resolution) * (max_dist / grid_resolution)) * 255;
rgb_image.at<cv::Vec3b>(y, x) = cv::Vec3b(c, c, c);
}
}
for(unsigned int i = 0; i < sensor_points.size(); ++i)
{
const geo::Vector3& p = best_laser_pose_ * sensor_points[i];
int mx = -p.y / grid_resolution + grid_size / 2;
int my = -p.x / grid_resolution + grid_size / 2;
if (mx >= 0 && my >= 0 && mx < grid_size && my <grid_size)
{
rgb_image.at<cv::Vec3b>(my, mx) = cv::Vec3b(0, 255, 0);
}
}
// Visualize sensor
int lmx = -best_laser_pose_.t.y / grid_resolution + grid_size / 2;
int lmy = -best_laser_pose_.t.x / grid_resolution + grid_size / 2;
cv::circle(rgb_image, cv::Point(lmx,lmy), 0.3 / grid_resolution, cv::Scalar(0, 0, 255), 1);
geo::Vector3 d = best_laser_pose_.R * geo::Vector3(0.3, 0, 0);
int dmx = -d.y / grid_resolution;
int dmy = -d.x / grid_resolution;
cv::line(rgb_image, cv::Point(lmx, lmy), cv::Point(lmx + dmx, lmy + dmy), cv::Scalar(0, 0, 255), 1);
cv::imshow("distance_map", rgb_image);
cv::waitKey(1);
}
}
void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
if (!enabled_ || (cell_inflation_radius_ == 0))
return;
// make sure the inflation list is empty at the beginning of the cycle (should always be true)
ROS_ASSERT_MSG(inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation");
unsigned char* master_array = master_grid.getCharMap();
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
if (seen_ == NULL) {
ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL");
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
}
else if (seen_size_ != size_x * size_y)
{
ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong");
delete[] seen_;
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
}
memset(seen_, false, size_x * size_y * sizeof(bool));
// We need to include in the inflation cells outside the bounding
// box min_i...max_j, by the amount cell_inflation_radius_. Cells
// up to that distance outside the box can still influence the costs
// stored in cells inside the box.
min_i -= cell_inflation_radius_;
min_j -= cell_inflation_radius_;
max_i += cell_inflation_radius_;
max_j += cell_inflation_radius_;
min_i = std::max(0, min_i);
min_j = std::max(0, min_j);
max_i = std::min(int(size_x), max_i);
max_j = std::min(int(size_y), max_j);
// Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle
// We use a map<distance, list> to emulate the priority queue used before, with a notable performance boost
// Start with lethal obstacles: by definition distance is 0.0
std::vector<CellData>& obs_bin = inflation_cells_[0.0];
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
int index = master_grid.getIndex(i, j);
unsigned char cost = master_array[index];
if (cost == LETHAL_OBSTACLE)
{
obs_bin.push_back(CellData(index, i, j, i, j));
}
}
}
// Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
// can overtake previously inserted but farther away cells
std::map<double, std::vector<CellData> >::iterator bin;
for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
{
for (int i = 0; i < bin->second.size(); ++i)
{
// process all cells at distance dist_bin.first
const CellData& cell = bin->second[i];
unsigned int index = cell.index_;
// ignore if already visited
if (seen_[index])
{
continue;
}
seen_[index] = true;
unsigned int mx = cell.x_;
unsigned int my = cell.y_;
unsigned int sx = cell.src_x_;
unsigned int sy = cell.src_y_;
// assign the cost associated with the distance from an obstacle to the cell
unsigned char cost = costLookup(mx, my, sx, sy);
unsigned char old_cost = master_array[index];
if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
master_array[index] = cost;
else
master_array[index] = std::max(old_cost, cost);
// attempt to put the neighbors of the current cell onto the inflation list
if (mx > 0)
enqueue(index - 1, mx - 1, my, sx, sy);
if (my > 0)
enqueue(index - size_x, mx, my - 1, sx, sy);
if (mx < size_x - 1)
enqueue(index + 1, mx + 1, my, sx, sy);
if (my < size_y - 1)
enqueue(index + size_x, mx, my + 1, sx, sy);
}
}
inflation_cells_.clear();
}
