示例#1
0
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;
}
示例#2
0
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 "";
}
示例#3
0
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);
}