void ODUFinderModule::configure(tue::Configuration config) {

    if (!config.value("database_path", database_path_, tue::OPTIONAL))
        std::cout << "[" << module_name_ << "] " << "Parameter 'database_path' not found. Using default: " << database_path_ << std::endl;

    if (!config.value("debug_mode", debug_mode_, tue::OPTIONAL))
        std::cout << "[" << module_name_ << "] " << "Parameter 'debug_mode' not found. Using default: " << debug_mode_ << std::endl;

    if (!config.value("score_factor", score_factor_, tue::OPTIONAL))
        std::cout << "[" << module_name_ << "] " << "Parameter 'score_factor' not found. Using default: " << score_factor_ << std::endl;

    if (!config.value("debug_folder", debug_folder_, tue::OPTIONAL))
        std::cout << "[" << module_name_ << "] " << "Parameter 'debug_folder' not found. Using default: " << debug_folder_ << std::endl;

    database_path_ = module_path_ + database_path_;

    if (debug_mode_){
        std::cout << "[" << module_name_ << "] " << "Debug mode enabled. Debug folder: " << debug_folder_ << std::endl;
        ed::perception::cleanDebugFolder(debug_folder_);
    }

    // creat odu finder instance
    odu_finder_ = new odu_finder::ODUFinder(database_path_, debug_mode_);

    if (odu_finder_->get_n_models_loaded() == 0)
         std::cout << "[" << module_name_ << "] " << "No models were loaded!" << std::endl;
    else
        std::cout << "[" << module_name_ << "] " << "Loaded information for " << odu_finder_->get_n_models_loaded() << " models" << std::endl;

    initialized_ = true;

    std::cout << "[" << module_name_ << "] " << "Ready!" << std::endl;
}
예제 #2
0
void TFPublisherPlugin::configure(tue::Configuration config)
{
    config.value("root_frame_id", root_frame_id_);

    config.value("exclude", exclude_, tue::config::OPTIONAL);

    // Remove possible beginning slash
    if (!exclude_.empty() && exclude_[0] == '/')
        exclude_ = exclude_.substr(1);
}
예제 #3
0
bool Associator::configure(tue::Configuration config)
{
    if ( config.readGroup("association") )
    {
        if ( config.readArray("association_modules",tue::REQUIRED) )
        {
            while ( config.nextArrayItem() )
            {
                std::string module_type;
                config.value("type", module_type );
                if ( module_type == "nearest_neighbor" )
                {
                    boost::shared_ptr<NearestNeighborCC> costCalculator(new NearestNeighborCC);
                    costCalculators_.push_back(costCalculator);

                    double max_no_std_devs;
                    if ( !config.value("max_no_std_devs", max_no_std_devs) )
                        std::cout << "[ASSOCIATOR] Configure: Warning! No max_no_std_devs defined for " << module_type << " cost calculator module" << std::endl;

                    max_assoc_dists_.push_back(max_no_std_devs);
                }
                else if ( module_type == "edge_tension" )
                {
                    boost::shared_ptr<EdgeTensionCC> costCalculator(new EdgeTensionCC);
                    costCalculators_.push_back(costCalculator);

                    double max_no_std_devs;
                    if ( !config.value("max_no_std_devs", max_no_std_devs) )
                        std::cout << "[ASSOCIATOR] Configure: Warning! No max_no_std_devs defined for " << module_type << " cost calculator module" << std::endl;

                    max_assoc_dists_.push_back(max_no_std_devs);
                }
                else
                {
                    std::cout << "[ASSOCIATOR] Configure: Warning! Unknown association cost module type given in config" << std::endl;
                }
            }
            config.endArray();

            if ( !costCalculators_.size() )
            {
                std::cout << "\033[31m" << "[ASSOCIATOR] Configure: No cost calculator modules available!" << "\033[0m" << std::endl;
                return false;
            }

        }
        config.endGroup();
    }
    else
    {
        std::cout << "\033[31m" << "[ASSOCIATOR] Configure: No configuration for association found!" << "\033[0m" << std::endl;
        return false;
    }

    return true;
}
void LocalizationPlugin::configure(tue::Configuration config)
{
    std::string laser_topic;
    config.value("laser_topic", laser_topic);

    if (config.hasError())
        return;

    ros::NodeHandle nh;

    // Subscribe to laser topic
    ros::SubscribeOptions sub_options =
            ros::SubscribeOptions::create<sensor_msgs::LaserScan>(
                laser_topic, 1, boost::bind(&LocalizationPlugin::laserCallback, this, _1), ros::VoidPtr(), &cb_queue_);

    sub_laser_ = nh.subscribe(sub_options);

    a_current_ = 0;
    laser_pose_ = geo::Pose3D::identity();
    laser_pose_.t.z = 0.3;
}
std::shared_ptr<SupervisedController> ControllerFactory::createController(tue::Configuration& config, double dt) const
{
    std::shared_ptr<SupervisedController> supervised_controller;

    // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
    // Get controller name and type

    std::string name, type;
    if (!config.value("name", name) | !config.value("type", type))
        return supervised_controller;

    config.setShortErrorContext(name);

    // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
    // Create controller core from given type

    std::map<std::string, t_controller_creator>::const_iterator it = controller_types_.find(type);
    if (it == controller_types_.end())
    {
        config.addError("Unknown controller type: '" + type + "'");
        return supervised_controller;
    }

    std::shared_ptr<Controller> c = it->second();

    // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
    // Configure controller core

    c->configure(config, dt);
    c->setName(name);

    // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
    // Wrap controller in supervised controller, and configure it

    supervised_controller.reset(new SupervisedController);
    supervised_controller->setController(c);
    supervised_controller->configure(config, dt);

    return supervised_controller;
}
예제 #6
0
void FaceDetector::configureClassification(tue::Configuration config)
{
    // default values in case configure(...) is not called!
    debug_mode_ = false;
    classifier_front_scale_factor_ = 1.2;
    classifier_front_min_neighbours_ = 3;
    classif_front_min_size_ = cv::Size(20,20);
    classifier_profile_scale_factor_= 1.2;
    classifier_profile_min_neighbours_ = 3;
    classif_profile_min_size_ = cv::Size(20,20);
    debug_folder_ = "/tmp/face_detector/";

    // load training files for frontal classifier
    std::string cascade_front_files_path_;
    if (config.value("cascade_front_files_path", cascade_front_files_path_))
    {
        if (!classifier_front_.load(cascade_front_files_path_))
            config.addError("Unable to load front haar cascade files (" + cascade_front_files_path_ + ")");
    }

    // load training files for profile classifier
    std::string cascade_profile_files_path_;
    if (config.value("cascade_profile_front_path", cascade_profile_files_path_))
    {
        if (!classifier_profile_.load(cascade_profile_files_path_))
            config.addError("Unable to load profile haar cascade files (" + cascade_profile_files_path_ + ")");
    }

    if (config.hasError())
        return;

    if (!config.value("debug_mode", debug_mode_, tue::OPTIONAL))
        ed::log::info() << "Parameter 'debug_mode' not found. Using default: " << debug_mode_ << std::endl;

    if (!config.value("debug_folder", debug_folder_, tue::OPTIONAL))
        ed::log::info() << "Parameter 'debug_folder' not found. Using default: " << debug_folder_ << std::endl;

    if (!config.value("classifier_front_scale_factor", classifier_front_scale_factor_, tue::OPTIONAL))
        ed::log::info() << "Parameter 'classifier_front_scale_factor' not found. Using default: " << classifier_front_scale_factor_ << std::endl;

    if (!config.value("classifier_front_min_neighbours", classifier_front_min_neighbours_, tue::OPTIONAL))
        ed::log::info() << "Parameter 'classifier_front_min_neighbours' not found. Using default: " << classifier_front_min_neighbours_ << std::endl;

    if (!config.value("classifier_profile_scale_factor", classifier_profile_scale_factor_, tue::OPTIONAL))
        ed::log::info() << "Parameter 'classifier_profile_scale_factor' not found. Using default: " << classifier_profile_scale_factor_ << std::endl;

    if (!config.value("classifier_profile_min_neighbours", classifier_profile_min_neighbours_, tue::OPTIONAL))
        ed::log::info() << "Parameter 'classifier_profile_min_neighbours' not found. Using default: " << classifier_profile_min_neighbours_ << std::endl;

    if (debug_mode_)
    {
        // clean the debug folder if debugging is active
        try {
            boost::filesystem::path dir(debug_folder_);
            boost::filesystem::remove_all(dir);
            boost::filesystem::create_directories(dir);
        }
        catch(const boost::filesystem::filesystem_error& e)
        {
            if(e.code() == boost::system::errc::permission_denied)
                ed::log::error() << "boost::filesystem permission denied" << std::endl;
            else
                ed::log::error() << "boost::filesystem failed with error: " << e.code().message() << std::endl;
        }

        // create debug window
        cv::namedWindow("Face Detector Output", CV_WINDOW_AUTOSIZE);
    }
}
예제 #7
0
void FaceDetector::writeFaceDetectionResult(const ed::Measurement& msr, const cv::Rect& rgb_roi,
                                            const std::vector<cv::Rect>& rgb_face_rois,
                                            int& face_counter, tue::Configuration& result) const
{
    // get color image
    const cv::Mat& color_image = msr.image()->getRGBImage();

    // get color image
    const cv::Mat& depth_image = msr.image()->getDepthImage();

    // Calculate size factor between depth and rgb images
    double f_depth_rgb = (double)depth_image.cols / color_image.cols;

    // Create depth view
    rgbd::View depth_view(*msr.image(), depth_image.cols);

    for (uint j = 0; j < rgb_face_rois.size(); j++)
    {
        cv::Rect rgb_face_roi = rgb_face_rois[j];
        rgb_face_roi.x += rgb_roi.x;
        rgb_face_roi.y += rgb_roi.y;

        if (debug_mode_)
            ed::perception::saveDebugImage("face_detector-rgb", color_image(rgb_face_roi));

        result.addArrayItem();

        result.setValue("index", face_counter);

        // add 2D location of the face
        result.setValue("x", rgb_face_roi.x);
        result.setValue("y", rgb_face_roi.y);
        result.setValue("width", rgb_face_roi.width);
        result.setValue("height", rgb_face_roi.height);

        // Compute face roi for depth image
        cv::Rect depth_face_roi(f_depth_rgb * rgb_face_roi.x, f_depth_rgb * rgb_face_roi.y,
                                f_depth_rgb * rgb_face_roi.width, f_depth_rgb * rgb_face_roi.height);

        if (debug_mode_)
            ed::perception::saveDebugImage("face_detector-depth", depth_image(depth_face_roi));

        cv::Mat face_area = depth_image(depth_face_roi);
        float avg_depth = ed::perception::getMedianDepth(face_area);

        if (avg_depth > 0)
        {
            // calculate the center point of the face
            cv::Point2i p_2d(depth_face_roi.x + depth_face_roi.width/2,
                             depth_face_roi.y + depth_face_roi.height/2);

            geo::Vector3 projection = depth_view.getRasterizer().project2Dto3D(p_2d.x, p_2d.y) * avg_depth;
            geo::Vector3 point_map = msr.sensorPose() * projection;

            // add 3D location of the face
            result.setValue("map_x", point_map.x);
            result.setValue("map_y", point_map.y);
            result.setValue("map_z", point_map.z);

        }
        else
        {
            std::cout << "[ED FACE DETECTOR] Could not calculate face's average depth. Map coordinates might be incorrect!" << std::endl;
        }

        result.endArrayItem();
        face_counter++;
    }
}
예제 #8
0
void GUIPlugin::configure(tue::Configuration config)
{
    std::string srv_get_measurements, srv_set_label, srv_raise_event, srv_get_command, map_image_topic;

    config.value("srv_get_measurements", srv_get_measurements);
    config.value("srv_set_label", srv_set_label);
    config.value("srv_raise_event", srv_raise_event);
    config.value("srv_get_command", srv_get_command);
    config.value("map_image_topic", map_image_topic);

    // TODO: make configurable
    trigger_map_publish_ = ed::EventClock(10);

    int image_width, image_height;
    config.value("map_image_width", image_width);
    config.value("map_image_height", image_height);

    double world_width, world_height;
    config.value("world_width", world_width);
    config.value("world_height", world_height);

    double cam_x, cam_y, cam_z;
    config.value("cam_x", cam_x);
    config.value("cam_y", cam_y);
    config.value("cam_z", cam_z);

    if (config.hasError())
        return;

    ros::NodeHandle nh;

    if (srv_get_measurements_.getService() != srv_get_measurements)
    {
        ros::AdvertiseServiceOptions opt_get_measurements =
                ros::AdvertiseServiceOptions::create<ed_msgs::GetMeasurements>(
                    srv_get_measurements, boost::bind(&GUIPlugin::srvGetMeasurements, this, _1, _2), ros::VoidPtr(), &cb_queue_);
        srv_get_measurements_ = nh.advertiseService(opt_get_measurements);
    }

    if (srv_set_label_.getService() != srv_set_label)
    {
        ros::AdvertiseServiceOptions opt_set_label =
                ros::AdvertiseServiceOptions::create<ed_msgs::SetLabel>(
                    srv_set_label, boost::bind(&GUIPlugin::srvSetLabel, this, _1, _2), ros::VoidPtr(), &cb_queue_);
        srv_set_label_ = nh.advertiseService(opt_set_label);
    }

    if (srv_raise_event_.getService() != srv_raise_event)
    {
        ros::AdvertiseServiceOptions opt_raise_event =
                ros::AdvertiseServiceOptions::create<ed_msgs::RaiseEvent>(
                    srv_raise_event, boost::bind(&GUIPlugin::srvRaiseEvent, this, _1, _2), ros::VoidPtr(), &cb_queue_);
        srv_raise_event_ = nh.advertiseService(opt_raise_event);
    }

    if (srv_get_command_.getService() != srv_get_command)
    {
        ros::AdvertiseServiceOptions opt_get_command =
                ros::AdvertiseServiceOptions::create<ed_msgs::GetGUICommand>(
                    srv_get_command, boost::bind(&GUIPlugin::srvGetCommand, this, _1, _2), ros::VoidPtr(), &cb_queue_);
        srv_get_command_ = nh.advertiseService(opt_get_command);
    }

    if (pub_image_map_.getTopic() != map_image_topic)
        pub_image_map_ = nh.advertise<tue_serialization::Binary>("/ed/gui/map_image", 1);

    map_image_ = cv::Mat(image_width, image_height, CV_8UC3, cv::Scalar(50, 50, 50));

    projector_pose_.setOrigin(geo::Vector3(cam_x, cam_y, cam_z));
    projector_pose_.setBasis(geo::Matrix3::identity());

    projector_.setFocalLengths(image_width / (world_width / cam_z),
                               image_height / (world_height / cam_z));
    projector_.setOpticalCenter(image_width / 2, image_height / 2);
    projector_.setOpticalTranslation(0, 0);
}
예제 #9
0
void HumanContourMatcher::process(ed::EntityConstPtr e, tue::Configuration& result) const
{
    // if initialization failed, return
    if (!init_success_)
        return;

    // Get the best measurement from the entity
    ed::MeasurementConstPtr msr = e->lastMeasurement();
    if (!msr)
        return;

    float depth_sum = 0;
    float avg_depht;
    float classification_error = 0;
    float classification_deviation = 0;
    std::string classification_stance;
    uint point_counter = 0;
    bool is_human = false;

    // Get the depth image from the measurement
    const cv::Mat& depth_image = msr->image()->getDepthImage();
    const cv::Mat& color_image = msr->image()->getRGBImage();

    cv::Mat mask_cv = cv::Mat::zeros(depth_image.rows, depth_image.cols, CV_8UC1);

    // Iterate over all points in the mask. You must specify the width of the image on which you
    // want to apply the mask (see the begin(...) method).
    for(ed::ImageMask::const_iterator it = msr->imageMask().begin(depth_image.cols); it != msr->imageMask().end(); ++it)
    {
        // mask's (x, y) coordinate in the depth image
        const cv::Point2i& p_2d = *it;

        // TODO dont creat a Mat mask, just give human_classifier_.Classify a vector of 2D points!
        // paint a mask
        mask_cv.at<unsigned char>(p_2d) = 255;

        // calculate measurement average depth
        depth_sum += depth_image.at<float>(p_2d);
        point_counter++;
    }

    avg_depht = depth_sum/(float)point_counter;

    is_human = human_classifier_.Classify(depth_image, color_image, mask_cv, avg_depht, classification_error, classification_deviation, classification_stance);

    // ----------------------- assert results -----------------------

    // create group if it doesnt exist
    if (!result.readGroup("perception_result", tue::OPTIONAL))
    {
        result.writeGroup("perception_result");
    }

    // assert the results to the entity
    result.writeGroup(kModuleName);
    result.setValue("label", "Human Shape");

    if (classification_error > 0){
        result.setValue("stance", classification_stance);
        result.setValue("error", classification_error);
        result.setValue("deviation", classification_deviation);
    }

    if(is_human)
    {
        result.setValue("score", 1.0);
    }else{
        result.setValue("score", 0.0);
    }

    result.endGroup();  // close human_contour_matcher group
    result.endGroup();  // close perception_result group
}
예제 #10
0
void TFPublisherPlugin::configure(tue::Configuration config)
{
    config.value("root_frame_id", root_frame_id_);
}
예제 #11
0
void ColorMatcher::configureClassification(tue::Configuration config)
{
    config.value("color_margin", color_margin_);
    initialize();
}