void TFPublisherPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& req)
{
for(ed::WorldModel::const_iterator it = world.begin(); it != world.end(); ++it)
{
const ed::EntityConstPtr& e = *it;
if (!e->has_pose())
continue;
std::string id = e->id().str();
if (!id.empty() && id[0] == '/')
id = id.substr(1);
// If exclude is set, do not add entities whose id starts with exclude
if (!exclude_.empty() && id.size() >= exclude_.size() && id.substr(0, exclude_.size()) == exclude_)
continue;
geo::Pose3D pose_MAP;
pose_MAP = e->pose();
tf::StampedTransform t;
geo::convert(pose_MAP, t);
t.frame_id_ = root_frame_id_;
t.child_frame_id_ = e->id().str();
t.stamp_ = ros::Time::now();
tf_broadcaster_->sendTransform(t);
}
}
void TFPublisherPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& req)
{
for(ed::WorldModel::const_iterator it = world.begin(); it != world.end(); ++it)
{
const ed::EntityConstPtr& e = it->second;
geo::Pose3D pose_MAP;
pose_MAP = e->pose();
if (e->bestMeasurement())
{
pose_MAP.t = e->convexHull().center_point;
}
tf::StampedTransform t;
geo::convert(pose_MAP, t);
t.frame_id_ = root_frame_id_;
t.child_frame_id_ = e->id();
t.stamp_ = ros::Time::now();
tf_broadcaster_->sendTransform(t);
}
}
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);
}
}
