Пример #1
0
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);
    }
}
Пример #2
0
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);
    }
}