const QVector<int> Q3PlotAxis::tickPositions() const
{
QVector<int> tickPositions(ticks_.count());
for (int i = 0; i < ticks_.count(); ++i) {
tickPositions[i] = coordinateToPixel(ticks_[i]);
}
return tickPositions;
}
ed::UUID GUIPlugin::getEntityFromClick(const cv::Point2i& p) const
{
for(ed::WorldModel::const_iterator it_entity = world_model_->begin(); it_entity != world_model_->end(); ++it_entity)
{
const ed::EntityConstPtr& e = *it_entity;
if (!e->shape())
{
const pcl::PointCloud<pcl::PointXYZ>& chull_points = e->convexHull().chull;
if (!chull_points.empty())
{
std::vector<cv::Point2i> image_chull(chull_points.size());
for(unsigned int i = 0; i < chull_points.size(); ++i)
image_chull[i] = coordinateToPixel(chull_points[i]);
if (inPolygon(image_chull, p))
return e->id();
}
}
}
return "";
}
void GUIPlugin::publishMapImage()
{
// clear image
map_image_ = cv::Mat(map_image_.rows, map_image_.cols, CV_8UC3, cv::Scalar(10, 10, 10));
// Draw world model shapes
cv::Mat z_buffer(map_image_.rows, map_image_.cols, CV_32FC1, 0.0);
for(ed::WorldModel::const_iterator it_entity = world_model_->begin(); it_entity != world_model_->end(); ++it_entity)
{
const ed::EntityConstPtr& e = *it_entity;
if (e->shape() && e->id() != "floor")
{
cv::Scalar color = idToColor(e->id());
cv::Vec3b color_vec(0.5 * color[0], 0.5 * color[1], 0.5 * color[2]);
geo::Pose3D pose = projector_pose_.inverse() * e->pose();
geo::RenderOptions opt;
opt.setMesh(e->shape()->getMesh(), pose);
ColorRenderResult res(map_image_, z_buffer, color_vec, projector_pose_.t.z - 3, projector_pose_.t.z + 1);
// Render
projector_.render(opt, res);
}
}
for(ed::WorldModel::const_iterator it_entity = world_model_->begin(); it_entity != world_model_->end(); ++it_entity)
{
const ed::EntityConstPtr& e = *it_entity;
const pcl::PointCloud<pcl::PointXYZ>& chull_points = e->convexHull().chull;
cv::Scalar color = idToColor(e->id());
int thickness = 1;
if (selected_id_ == e->id())
thickness = 3;
// draw the polygon
if (!chull_points.empty())
{
// Lower polygon
for(unsigned int i = 0; i < chull_points.size(); ++i)
{
int j = (i + 1) % chull_points.size();
const pcl::PointXYZ& p1 = chull_points[i];
const pcl::PointXYZ& p2 = chull_points[j];
cv::line(map_image_,
coordinateToPixel(p1.x, p1.y, e->convexHull().min_z),
coordinateToPixel(p2.x, p2.y, e->convexHull().min_z),
0.3 * color, thickness);
}
// Edges in between
for(unsigned int i = 0; i < chull_points.size(); ++i)
{
const pcl::PointXYZ p = chull_points[i];
cv::line(map_image_,
coordinateToPixel(p.x, p.y, e->convexHull().min_z),
coordinateToPixel(p.x, p.y, e->convexHull().max_z),
0.5 * color, thickness);
}
// Upper polygon
for(unsigned int i = 0; i < chull_points.size(); ++i)
{
int j = (i + 1) % chull_points.size();
const pcl::PointXYZ& p1 = chull_points[i];
const pcl::PointXYZ& p2 = chull_points[j];
cv::line(map_image_,
coordinateToPixel(p1.x, p1.y, e->convexHull().max_z),
coordinateToPixel(p2.x, p2.y, e->convexHull().max_z),
color, thickness);
}
if (e->type() == "person")
{
cv::circle(map_image_, coordinateToPixel(e->convexHull().center_point), 15, cv::Scalar(255, 0, 0), 10);
}
}
}
// Find the global most recent measurement
ed::MeasurementConstPtr last_measurement;
for(ed::WorldModel::const_iterator it_entity = world_model_->begin(); it_entity != world_model_->end(); ++it_entity)
{
const ed::EntityConstPtr& e = *it_entity;
if (e->lastMeasurement() && (!last_measurement || e->lastMeasurement()->timestamp() > e->lastMeasurement()->timestamp()))
last_measurement = e->lastMeasurement();
}
if (last_measurement)
{
const geo::Pose3D& sensor_pose = last_measurement->sensorPose();
// Draw sensor origin
const geo::Vector3& p = sensor_pose.getOrigin();
cv::Point2i p_2d = coordinateToPixel(p);
cv::circle(map_image_, p_2d, 10, cv::Scalar(255, 255, 255));
rgbd::View view(*last_measurement->image(), 100); // width doesnt matter; we'll go back to world coordinates anyway
geo::Vector3 p1 = view.getRasterizer().project2Dto3D(0, 0) * 3;
geo::Vector3 p2 = view.getRasterizer().project2Dto3D(view.getWidth() - 1, 0) * 3;
geo::Vector3 p3 = view.getRasterizer().project2Dto3D(0, view.getHeight() - 1) * 3;
geo::Vector3 p4 = view.getRasterizer().project2Dto3D(view.getWidth() - 1, view.getHeight() - 1) * 3;
cv::Point2i p1_2d = coordinateToPixel(sensor_pose * p1);
cv::Point2i p2_2d = coordinateToPixel(sensor_pose * p2);
cv::Point2i p3_2d = coordinateToPixel(sensor_pose * p3);
cv::Point2i p4_2d = coordinateToPixel(sensor_pose * p4);
cv::Scalar fustrum_color(100, 100, 100);
cv::line(map_image_, p_2d, p1_2d, fustrum_color);
cv::line(map_image_, p_2d, p2_2d, fustrum_color);
cv::line(map_image_, p_2d, p3_2d, fustrum_color);
cv::line(map_image_, p_2d, p4_2d, fustrum_color);
cv::line(map_image_, p1_2d, p2_2d, fustrum_color);
cv::line(map_image_, p1_2d, p3_2d, fustrum_color);
cv::line(map_image_, p2_2d, p4_2d, fustrum_color);
cv::line(map_image_, p3_2d, p4_2d, fustrum_color);
}
if (t_last_click_ > ros::Time(0) && t_last_click_ > ros::Time::now() - ros::Duration(1))
{
cv::circle(map_image_, p_click_, 20, cv::Scalar(0, 0, 255), 5);
}
// Convert to binary
tue_serialization::Binary msg;
if (imageToBinary(map_image_, msg.data, IMAGE_COMPRESSION_PNG))
{
pub_image_map_.publish(msg);
}
// cv::imshow("gui", map_image_);
// cv::waitKey(3);
}
