Merge pull request #40 from shahurik/adas

ADAS: multiple fixes, fcw_train and fcw_detect applications

modules/adas/tools/CMakeLists.txt

@@ -1,33 +1,2 @@
-set(name fcw_train)
+add_subdirectory(fcw_train)
-set(the_target opencv_${name})
+add_subdirectory(fcw_detect)
-set(the_module opencv_adas)
-
-ocv_check_dependencies(${OPENCV_MODULE_${the_module}_DEPS})
-
-if(NOT OCV_DEPENDENCIES_FOUND)
-  return()
-endif()
-
-project(${the_target})
-
-ocv_include_directories("${OpenCV_SOURCE_DIR}/include/opencv")
-ocv_include_modules(${OPENCV_MODULE_${the_module}_DEPS})
-
-file(GLOB ${the_target}_SOURCES ${CMAKE_CURRENT_LIST_DIR}/*.cpp)
-
-add_executable(${the_target} ${${the_target}_SOURCES})
-
-ocv_target_link_libraries(${the_target} ${OPENCV_MODULE_${the_module}_DEPS})
-
-set_target_properties(${the_target} PROPERTIES
-                      DEBUG_POSTFIX "${OPENCV_DEBUG_POSTFIX}"
-                      ARCHIVE_OUTPUT_DIRECTORY ${LIBRARY_OUTPUT_PATH}
-                      RUNTIME_OUTPUT_DIRECTORY ${EXECUTABLE_OUTPUT_PATH}
-                      INSTALL_NAME_DIR lib
-                      OUTPUT_NAME ${the_target})
-
-if(ENABLE_SOLUTION_FOLDERS)
-  set_target_properties(${the_target} PROPERTIES FOLDER "applications")
-endif()
-
-install(TARGETS ${the_target} RUNTIME DESTINATION bin COMPONENT main)

modules/adas/tools/fcw_detect/CMakeLists.txt

@@ -0,0 +1,35 @@
+set(name fcw_detect)
+set(the_target opencv_${name})
+
+set(OPENCV_${the_target}_DEPS opencv_core opencv_imgcodecs opencv_videoio
+    opencv_highgui opencv_xobjdetect)
+
+ocv_check_dependencies(${OPENCV_${the_target}_DEPS})
+
+if(NOT OCV_DEPENDENCIES_FOUND)
+  return()
+endif()
+
+project(${the_target})
+
+ocv_include_directories("${OpenCV_SOURCE_DIR}/include/opencv")
+ocv_include_modules(${OPENCV_${the_target}_DEPS})
+
+file(GLOB ${the_target}_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/*.cpp)
+
+add_executable(${the_target} ${${the_target}_SOURCES})
+
+target_link_libraries(${the_target} ${OPENCV_${the_target}_DEPS})
+
+set_target_properties(${the_target} PROPERTIES
+                      DEBUG_POSTFIX "${OPENCV_DEBUG_POSTFIX}"
+                      ARCHIVE_OUTPUT_DIRECTORY ${LIBRARY_OUTPUT_PATH}
+                      RUNTIME_OUTPUT_DIRECTORY ${EXECUTABLE_OUTPUT_PATH}
+                      INSTALL_NAME_DIR lib
+                      OUTPUT_NAME ${the_target})
+
+if(ENABLE_SOLUTION_FOLDERS)
+  set_target_properties(${the_target} PROPERTIES FOLDER "applications")
+endif()
+
+install(TARGETS ${the_target} RUNTIME DESTINATION bin COMPONENT main)

modules/adas/tools/fcw_detect/fcw_detect.cpp

@@ -0,0 +1,92 @@
+#include <string>
+using std::string;
+
+#include <vector>
+using std::vector;
+
+#include <iostream>
+using std::cerr;
+using std::endl;
+
+#include <opencv2/core.hpp>
+using cv::Rect;
+using cv::Size;
+using cv::Mat;
+using cv::Mat_;
+using cv::Vec3b;
+
+#include <opencv2/highgui.hpp>
+using cv::imread;
+using cv::imwrite;
+
+#include <opencv2/core/utility.hpp>
+using cv::CommandLineParser;
+using cv::FileStorage;
+
+#include <opencv2/xobjdetect.hpp>
+using cv::xobjdetect::ICFDetector;
+
+static Mat visualize(const Mat &image, const vector<Rect> &objects)
+{
+    CV_Assert(image.type() == CV_8UC3);
+    Mat_<Vec3b> img = image.clone();
+    for( size_t j = 0; j < objects.size(); ++j )
+    {
+        Rect obj = objects[j];
+        int x = obj.x;
+        int y = obj.y;
+        int width = obj.width;
+        int height = obj.height;
+        for( int i = y; i <= y + height; ++i ) {
+            img(i, x) = Vec3b(255, 0, 0);
+            img(i, x + width) = Vec3b(255, 0, 0);
+        }
+        for( int i = x; i <= x + width; ++i) {
+            img(y, i) = Vec3b(255, 0, 0);
+            img(y + height, i) = Vec3b(255, 0, 0);
+        }
+    }
+    return img;
+}
+
+int main(int argc, char *argv[])
+{
+    const string keys =
+        "{help           |           | print this message}"
+        "{model_filename | model.xml | filename for reading model}"
+        "{image_path     |  test.png | path to image for detection}"
+        "{out_image_path |   out.png | path to image for output}"
+        "{threshold      |       0.0 | threshold for cascade}"
+        ;
+
+    CommandLineParser parser(argc, argv, keys);
+    parser.about("FCW detection");
+
+    if( parser.has("help") || argc == 1)
+    {
+        parser.printMessage();
+        return 0;
+    }
+
+    string model_filename = parser.get<string>("model_filename");
+    string image_path = parser.get<string>("image_path");
+    string out_image_path = parser.get<string>("out_image_path");
+    float threshold = parser.get<float>("threshold");
+
+
+    if( !parser.check() )
+    {
+        parser.printErrors();
+        return 1;
+    }
+
+    ICFDetector detector;
+    FileStorage fs(model_filename, FileStorage::READ);
+    detector.read(fs["icfdetector"]);
+    fs.release();
+    vector<Rect> objects;
+    Mat img = imread(image_path);
+    detector.detect(img, objects, 1.1f, Size(40, 40),
+        Size(300, 300), threshold);
+    imwrite(out_image_path, visualize(img, objects));
+}

modules/adas/tools/fcw_train.cpp

@@ -1,195 +0,0 @@
-#include <cstdio>
-#include <cstring>
-
-#include <string>
-using std::string;
-
-#include <vector>
-using std::vector;
-
-#include <fstream>
-using std::ifstream;
-using std::getline;
-
-#include <sstream>
-using std::stringstream;
-
-#include <iostream>
-using std::cerr;
-using std::endl;
-
-#include <opencv2/core.hpp>
-using cv::Rect;
-
-#include <opencv2/xobjdetect.hpp>
-
-using cv::xobjdetect::ICFDetectorParams;
-using cv::xobjdetect::ICFDetector;
-using cv::xobjdetect::WaldBoost;
-using cv::xobjdetect::WaldBoostParams;
-using cv::Mat;
-
-static bool read_pos_int(const char *str, int *n)
-{
-    int pos = 0;
-    if( sscanf(str, "%d%n", n, &pos) != 1 || str[pos] != '\0' || *n <= 0 )
-    {
-        return false;
-    }
-    return true;
-}
-
-static bool read_model_size(char *str, int *rows, int *cols)
-{
-    int pos = 0;
-    if( sscanf(str, "%dx%d%n", rows, cols, &pos) != 2 || str[pos] != '\0' ||
-        *rows <= 0 || *cols <= 0)
-    {
-        return false;
-    }
-    return true;
-}
-
-static bool read_overlap(const char *str, double *overlap)
-{
-    int pos = 0;
-    if( sscanf(str, "%lf%n", overlap, &pos) != 1 || str[pos] != '\0' ||
-        *overlap < 0 || *overlap > 1)
-    {
-        return false;
-    }
-    return true;
-}
-
-static bool read_labels(const string& path,
-    vector<string>& filenames, vector< vector<Rect> >& labels)
-{
-    string labels_path = path + "/gt.txt";
-    string filename, line;
-    int x1, y1, x2, y2;
-    char delim;
-    ifstream ifs(labels_path.c_str());
-    if( !ifs.good() )
-        return false;
-
-    while( getline(ifs, line) )
-    {
-        stringstream stream(line);
-        stream >> filename;
-        filenames.push_back(path + "/" + filename);
-        vector<Rect> filename_labels;
-        while( stream >> x1 >> y1 >> x2 >> y2 >> delim )
-        {
-            filename_labels.push_back(Rect(x1, y1, x2, y2));
-        }
-        labels.push_back(filename_labels);
-        filename_labels.clear();
-    }
-    return true;
-}
-
-
-int main(int argc, char *argv[])
-{
-    if( argc == 1 )
-    {
-        printf("Usage: %s OPTIONS, where OPTIONS are:\n"
-               "\n"
-               "--path <path> - path to dir with data and labels\n"
-               "    (labels should have name gt.txt)\n"
-               "\n"
-               "--feature_count <count> - number of features to generate\n"
-               "\n"
-               "--weak_count <count> - number of weak classifiers in cascade\n"
-               "\n"
-               "--model_size <rowsxcols> - model size in pixels\n"
-               "\n"
-               "--overlap <measure> - number from [0, 1], means maximum\n"
-               "    overlap with objects while sampling background\n"
-               "\n"
-               "--model_filename <path> - filename for saving model\n",
-               argv[0]);
-        return 0;
-    }
-
-    string path, model_path;
-    ICFDetectorParams params;
-    for( int i = 1; i < argc; ++i )
-    {
-        if( !strcmp("--path", argv[i]) )
-        {
-            i += 1;
-            path = argv[i];
-        }
-        else if( !strcmp("--feature_count", argv[i]) )
-        {
-            i += 1;
-            if( !read_pos_int(argv[i], &params.feature_count) )
-            {
-                fprintf(stderr, "Error reading feature count from `%s`\n",
-                        argv[i]);
-                return 1;
-            }
-        }
-        else if( !strcmp("--weak_count", argv[i]) )
-        {
-            i += 1;
-            if( !read_pos_int(argv[i], &params.weak_count) )
-            {
-                fprintf(stderr, "Error reading weak count from `%s`\n",
-                        argv[i]);
-                return 1;
-            }
-        }
-        else if( !strcmp("--model_size", argv[i]) )
-        {
-            i += 1;
-            if( !read_model_size(argv[i], &params.model_n_rows,
-                                  &params.model_n_cols) )
-            {
-                fprintf(stderr, "Error reading model size from `%s`\n",
-                        argv[i]);
-                return 1;
-            }
-        }
-        else if( !strcmp("--overlap", argv[i]) )
-        {
-            i += 1;
-            if( !read_overlap(argv[i], &params.overlap) )
-            {
-                fprintf(stderr, "Error reading overlap from `%s`\n",
-                        argv[i]);
-                return 1;
-            }
-        }
-        else if( !strcmp("--model_filename", argv[i]) )
-        {
-            i += 1;
-            model_path = argv[i];
-        }
-        else
-        {
-            fprintf(stderr, "Error: unknown argument `%s`\n", argv[i]);
-            return 1;
-        }
-
-    }
-
-    try
-    {
-        ICFDetector detector;
-        vector<string> filenames;
-        vector< vector<Rect> > labels;
-        read_labels(path, filenames, labels);
-
-        detector.train(filenames, labels, params);
-    }
-    catch( const char *err )
-    {
-        cerr << err << endl;
-    }
-    catch( ... )
-    {
-        cerr << "Unknown error\n" << endl;
-    }
-}

modules/adas/tools/fcw_train/CMakeLists.txt

@@ -0,0 +1,35 @@
+set(name fcw_train)
+set(the_target opencv_${name})
+
+set(OPENCV_${the_target}_DEPS opencv_core opencv_imgcodecs opencv_videoio
+    opencv_highgui opencv_xobjdetect)
+
+ocv_check_dependencies(${OPENCV_${the_target}_DEPS})
+
+if(NOT OCV_DEPENDENCIES_FOUND)
+  return()
+endif()
+
+project(${the_target})
+
+ocv_include_directories("${OpenCV_SOURCE_DIR}/include/opencv")
+ocv_include_modules(${OPENCV_${the_target}_DEPS})
+
+file(GLOB ${the_target}_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/*.cpp)
+
+add_executable(${the_target} ${${the_target}_SOURCES})
+
+target_link_libraries(${the_target} ${OPENCV_${the_target}_DEPS})
+
+set_target_properties(${the_target} PROPERTIES
+                      DEBUG_POSTFIX "${OPENCV_DEBUG_POSTFIX}"
+                      ARCHIVE_OUTPUT_DIRECTORY ${LIBRARY_OUTPUT_PATH}
+                      RUNTIME_OUTPUT_DIRECTORY ${EXECUTABLE_OUTPUT_PATH}
+                      INSTALL_NAME_DIR lib
+                      OUTPUT_NAME ${the_target})
+
+if(ENABLE_SOLUTION_FOLDERS)
+  set_target_properties(${the_target} PROPERTIES FOLDER "applications")
+endif()
+
+install(TARGETS ${the_target} RUNTIME DESTINATION bin COMPONENT main)

modules/adas/tools/fcw_train/fcw_train.cpp

@@ -0,0 +1,118 @@
+#include <cstdio>
+#include <cstring>
+
+#include <string>
+using std::string;
+
+#include <vector>
+using std::vector;
+
+#include <fstream>
+using std::ifstream;
+using std::getline;
+
+#include <sstream>
+using std::stringstream;
+
+#include <iostream>
+using std::cerr;
+using std::endl;
+
+#include <opencv2/core.hpp>
+using cv::Rect;
+using cv::Size;
+#include <opencv2/highgui.hpp>
+using cv::imread;
+#include <opencv2/core/utility.hpp>
+using cv::CommandLineParser;
+using cv::FileStorage;
+
+#include <opencv2/xobjdetect.hpp>
+
+using cv::xobjdetect::ICFDetectorParams;
+using cv::xobjdetect::ICFDetector;
+using cv::xobjdetect::WaldBoost;
+using cv::xobjdetect::WaldBoostParams;
+using cv::Mat;
+
+static bool read_model_size(const char *str, int *rows, int *cols)
+{
+    int pos = 0;
+    if( sscanf(str, "%dx%d%n", rows, cols, &pos) != 2 || str[pos] != '\0' ||
+        *rows <= 0 || *cols <= 0)
+    {
+        return false;
+    }
+    return true;
+}
+
+int main(int argc, char *argv[])
+{
+    const string keys =
+        "{help           |           | print this message}"
+        "{pos_path       |       pos | path to training object samples}"
+        "{bg_path        |        bg | path to background images}"
+        "{bg_per_image   |         5 | number of windows to sample per bg image}"
+        "{feature_count  |     10000 | number of features to generate}"
+        "{weak_count     |       100 | number of weak classifiers in cascade}"
+        "{model_size     |     40x40 | model size in pixels}"
+        "{model_filename | model.xml | filename for saving model}"
+        ;
+
+    CommandLineParser parser(argc, argv, keys);
+    parser.about("FCW trainer");
+
+    if( parser.has("help") || argc == 1)
+    {
+        parser.printMessage();
+        return 0;
+    }
+
+    string pos_path = parser.get<string>("pos_path");
+    string bg_path = parser.get<string>("bg_path");
+    string model_filename = parser.get<string>("model_filename");
+
+    ICFDetectorParams params;
+    params.feature_count = parser.get<int>("feature_count");
+    params.weak_count = parser.get<int>("weak_count");
+    params.bg_per_image = parser.get<int>("bg_per_image");
+
+    string model_size = parser.get<string>("model_size");
+    if( !read_model_size(model_size.c_str(), &params.model_n_rows,
+        &params.model_n_cols) )
+    {
+        cerr << "Error reading model size from `" << model_size << "`" << endl;
+        return 1;
+    }
+
+    if( params.feature_count <= 0 )
+    {
+        cerr << "feature_count must be positive number" << endl;
+        return 1;
+    }
+
+    if( params.weak_count <= 0 )
+    {
+        cerr << "weak_count must be positive number" << endl;
+        return 1;
+    }
+
+    if( params.bg_per_image <= 0 )
+    {
+        cerr << "bg_per_image must be positive number" << endl;
+        return 1;
+    }
+
+    if( !parser.check() )
+    {
+        parser.printErrors();
+        return 1;
+    }
+
+    ICFDetector detector;
+    detector.train(pos_path, bg_path, params);
+    FileStorage fs(model_filename, FileStorage::WRITE);
+    fs << "icfdetector";
+    detector.write(fs);
+    fs.release();
+}

modules/xobjdetect/include/opencv2/xobjdetect.hpp

@@ -58,9 +58,9 @@ namespace xobjdetect
     channels — output array for computed channels
 
 */
-void computeChannels(InputArray image, OutputArrayOfArrays channels);
+CV_EXPORTS void computeChannels(InputArray image, std::vector<Mat>& channels);
 
-class CV_EXPORTS ACFFeatureEvaluator : public Algorithm
+class CV_EXPORTS FeatureEvaluator : public Algorithm
 {
 public:
     /* Set channels for feature evaluation */
@@ -79,23 +79,28 @@ public:
     */
     virtual void evaluateAll(OutputArray feature_values) const = 0;
 
+    virtual void assertChannels() = 0;
 };
 
-/* Construct evaluator, set features to evaluate */
+/* Construct feature evaluator, set features to evaluate
-CV_EXPORTS Ptr<ACFFeatureEvaluator>
+   type can "icf" or "acf" */
-createACFFeatureEvaluator(const std::vector<Point3i>& features);
+CV_EXPORTS Ptr<FeatureEvaluator>
+createFeatureEvaluator(const std::vector<std::vector<int> >& features,
+                       const std::string& type);
 
 /* Generate acf features
 
     window_size — size of window in which features should be evaluated
 
+    type — type of features, can be "icf" or "acf"
     count — number of features to generate.
     Max number of features is min(count, # possible distinct features)
 
 Returns vector of distinct acf features
 */
-std::vector<Point3i>
+std::vector<std::vector<int> >
-generateFeatures(Size window_size, int count = INT_MAX);
+generateFeatures(Size window_size, const std::string& type,
+                 int count = INT_MAX, int channel_count = 10);
 
 
 struct CV_EXPORTS WaldBoostParams
@@ -103,7 +108,7 @@ struct CV_EXPORTS WaldBoostParams
     int weak_count;
     float alpha;
 
-    WaldBoostParams(): weak_count(100), alpha(0.01f)
+    WaldBoostParams(): weak_count(100), alpha(0.02f)
     {}
 };
 
@@ -122,8 +127,8 @@ public:
     Returns feature indices chosen for cascade.
     Feature enumeration starts from 0
     */
-    virtual std::vector<int> train(const Mat& data,
+    virtual std::vector<int> train(const Mat& /*data*/,
-                                   const Mat& labels) = 0;
+                                   const Mat& /*labels*/) = 0;
 
     /* Predict object class given object that can compute object features
 
@@ -133,8 +138,13 @@ public:
     is from class +1
     */
     virtual float predict(
-        const Ptr<ACFFeatureEvaluator>& feature_evaluator) const = 0;
+        const Ptr<FeatureEvaluator>& /*feature_evaluator*/) const = 0;
 
+    /* Write WaldBoost to FileStorage */
+    virtual void write(FileStorage& /*fs*/) const = 0;
+
+    /* Read WaldBoost */
+    virtual void read(const FileNode& /*node*/) = 0;
 };
 
 CV_EXPORTS Ptr<WaldBoost>
@@ -146,32 +156,64 @@ struct CV_EXPORTS ICFDetectorParams
     int weak_count;
     int model_n_rows;
     int model_n_cols;
-    double overlap;
+    int bg_per_image;
 
     ICFDetectorParams(): feature_count(UINT_MAX), weak_count(100),
-        model_n_rows(40), model_n_cols(40), overlap(0.0)
+        model_n_rows(56), model_n_cols(56), bg_per_image(5)
     {}
 };
 
 class CV_EXPORTS ICFDetector
 {
 public:
+
+    ICFDetector(): waldboost_(), features_() {}
+
     /* Train detector
 
-        image_filenames — filenames of images for training
+        pos_path — path to folder with images of objects
 
-        labelling — vector of object bounding boxes per every image
+        bg_path — path to folder with background images
 
         params — parameters for detector training
     */
-    void train(const std::vector<std::string>& image_filenames,
+    void train(const String& pos_path,
-               const std::vector<std::vector<cv::Rect> >& labelling,
+               const String& bg_path,
                ICFDetectorParams params = ICFDetectorParams());
 
-    /* Save detector in file, return true on success, false otherwise */
+    /* Detect object on image
-    bool save(const std::string& filename);
+
+        image — image for detection
+
+        object — output array of bounding boxes
+
+        scaleFactor — scale between layers in detection pyramid
+
+        minSize — min size of objects in pixels
+
+        maxSize — max size of objects in pixels
+    */
+    void detect(const Mat& image, std::vector<Rect>& objects,
+        float scaleFactor, Size minSize, Size maxSize, float threshold);
+
+    /* Write detector to FileStorage */
+    void write(FileStorage &fs) const;
+
+    /* Read detector */
+    void read(const FileNode &node);
+
+private:
+    Ptr<WaldBoost> waldboost_;
+    std::vector<std::vector<int> > features_;
+    int model_n_rows_;
+    int model_n_cols_;
 };
 
+CV_EXPORTS void write(FileStorage& fs, String&, const ICFDetector& detector);
+
+CV_EXPORTS void read(const FileNode& node, ICFDetector& d,
+    const ICFDetector& default_value = ICFDetector());
+
 } /* namespace xobjdetect */
 } /* namespace cv */
 

modules/xobjdetect/include/opencv2/xobjdetect/private.hpp

@@ -47,6 +47,26 @@ public:
     */
     float predict(int value) const;
 
+    /* Write stump in FileStorage */
+    void write(FileStorage& fs) const
+    {
+        fs << "{"
+            << "threshold" << threshold_
+            << "polarity" << polarity_
+            << "pos_value" << pos_value_
+            << "neg_value" << neg_value_
+            << "}";
+    }
+
+    /* Read stump */
+    void read(const FileNode& node)
+    {
+        threshold_ = (int)node["threshold"];
+        polarity_ = (int)node["polarity"];
+        pos_value_ = (float)node["pos_value"];
+        neg_value_ = (float)node["neg_value"];
+    }
+
 private:
     /* Stump decision threshold */
     int threshold_;
@@ -56,6 +76,10 @@ private:
     float pos_value_, neg_value_;
 };
 
+void read(const FileNode& node, Stump& s, const Stump& default_value=Stump());
+
+void write(FileStorage& fs, String&, const Stump& s);
+
 } /* namespace xobjdetect */
 } /* namespace cv */
 

modules/xobjdetect/src/acffeature.cpp

@@ -54,39 +54,131 @@ namespace cv
 namespace xobjdetect
 {
 
-class ACFFeatureEvaluatorImpl : public ACFFeatureEvaluator
+static bool isNull(const Mat_<int> &m)
+{
+    bool null_data = true;
+    for( int row = 0; row < m.rows; ++row )
+    {
+        for( int col = 0; col < m.cols; ++col )
+            if( m.at<int>(row, col) )
+                null_data = false;
+    }
+    return null_data;
+}
+
+class FeatureEvaluatorImpl : public FeatureEvaluator
 {
 public:
-    ACFFeatureEvaluatorImpl(const vector<Point3i>& features):
+    FeatureEvaluatorImpl(const vector<vector<int> >& features):
         features_(features), channels_(), position_()
     {
         CV_Assert(features.size() > 0);
     }
 
-    virtual void setChannels(InputArrayOfArrays channels);
+    virtual void assertChannels()
-    virtual void setPosition(Size position);
+    {
-    virtual int evaluate(size_t feature_ind) const;
+        bool null_data = true;
-    virtual void evaluateAll(OutputArray feature_values) const;
+        for( size_t i = 0; i < channels_.size(); ++i )
+            null_data &= isNull(channels_[i]);
+        CV_Assert(!null_data);
+    }
 
-private:
+    virtual void evaluateAll(OutputArray feature_values) const
+    {
+        Mat_<int> feature_vals(1, (int)features_.size());
+        for( int i = 0; i < (int)features_.size(); ++i )
+        {
+            feature_vals(0, i) = evaluate(i);
+        }
+        feature_values.assign(feature_vals);
+    }
+
+protected:
     /* Features to evaluate */
-    std::vector<Point3i> features_;
+    vector<vector<int> > features_;
     /* Channels for feature evaluation */
     std::vector<Mat> channels_;
     /* Channels window position */
     Size position_;
 };
 
-void ACFFeatureEvaluatorImpl::setChannels(cv::InputArrayOfArrays channels)
+class ICFFeatureEvaluatorImpl : public FeatureEvaluatorImpl
+{
+public:
+    ICFFeatureEvaluatorImpl(const vector<vector<int> >& features):
+        FeatureEvaluatorImpl(features)
+    {
+    }
+
+    virtual void setChannels(InputArrayOfArrays channels);
+    virtual void setPosition(Size position);
+    virtual int evaluate(size_t feature_ind) const;
+};
+
+void ICFFeatureEvaluatorImpl::setChannels(InputArrayOfArrays channels)
 {
     channels_.clear();
     vector<Mat> ch;
     channels.getMatVector(ch);
     CV_Assert(ch.size() == 10);
+
     for( size_t i = 0; i < ch.size(); ++i )
     {
         const Mat &channel = ch[i];
-        Mat_<int> acf_channel(channel.rows / 4, channel.cols / 4);
+        Mat integral_channel;
+        integral(channel, integral_channel, CV_32F);
+        Mat_<int> chan(integral_channel.rows, integral_channel.cols);
+        for( int row = 0; row < integral_channel.rows; ++row )
+            for( int col = 0; col < integral_channel.cols; ++col )
+                chan(row, col) = (int)integral_channel.at<float>(row, col);
+        channels_.push_back(chan.clone());
+    }
+}
+
+void ICFFeatureEvaluatorImpl::setPosition(Size position)
+{
+    position_ = position;
+}
+
+int ICFFeatureEvaluatorImpl::evaluate(size_t feature_ind) const
+{
+    CV_Assert(channels_.size() == 10);
+    CV_Assert(feature_ind < features_.size());
+
+    const vector<int>& feature = features_[feature_ind];
+    int x = feature[0] + position_.height;
+    int y = feature[1] + position_.width;
+    int x_to = feature[2] + position_.height;
+    int y_to = feature[3] + position_.width;
+    int n = feature[4];
+    const Mat_<int>& ch = channels_[n];
+    return ch(y_to + 1, x_to + 1) - ch(y, x_to + 1) - ch(y_to + 1, x) + ch(y, x);
+}
+
+class ACFFeatureEvaluatorImpl : public FeatureEvaluatorImpl
+{
+public:
+    ACFFeatureEvaluatorImpl(const vector<vector<int> >& features):
+        FeatureEvaluatorImpl(features)
+    {
+    }
+
+    virtual void setChannels(InputArrayOfArrays channels);
+    virtual void setPosition(Size position);
+    virtual int evaluate(size_t feature_ind) const;
+};
+
+void ACFFeatureEvaluatorImpl::setChannels(InputArrayOfArrays channels)
+{
+    channels_.clear();
+    vector<Mat> ch;
+    channels.getMatVector(ch);
+    CV_Assert(ch.size() == 10);
+
+    for( size_t i = 0; i < ch.size(); ++i )
+    {
+        const Mat &channel = ch[i];
+        Mat_<int> acf_channel = Mat_<int>::zeros(channel.rows / 4, channel.cols / 4);
         for( int row = 0; row < channel.rows; row += 4 )
         {
             for( int col = 0; col < channel.cols; col += 4 )
@@ -99,13 +191,14 @@ void ACFFeatureEvaluatorImpl::setChannels(cv::InputArrayOfArrays channels)
                 acf_channel(row / 4, col / 4) = sum;
             }
         }
-        channels_.push_back(acf_channel);
+
+        channels_.push_back(acf_channel.clone());
     }
 }
 
 void ACFFeatureEvaluatorImpl::setPosition(Size position)
 {
-    position_ = position;
+    position_ = Size(position.width / 4, position.height / 4);
 }
 
 int ACFFeatureEvaluatorImpl::evaluate(size_t feature_ind) const
@@ -113,60 +206,78 @@ int ACFFeatureEvaluatorImpl::evaluate(size_t feature_ind) const
     CV_Assert(channels_.size() == 10);
     CV_Assert(feature_ind < features_.size());
 
-    Point3i feature = features_.at(feature_ind);
+    const vector<int>& feature = features_[feature_ind];
-    int x = feature.x;
+    int x = feature[0];
-    int y = feature.y;
+    int y = feature[1];
-    int n = feature.z;
+    int n = feature[2];
-    return channels_[n].at<int>(y, x);
+    return channels_[n].at<int>(y + position_.width, x + position_.height);
 }
 
-void ACFFeatureEvaluatorImpl::evaluateAll(OutputArray feature_values) const
+Ptr<FeatureEvaluator> createFeatureEvaluator(
+    const vector<vector<int> >& features, const std::string& type)
 {
-    Mat_<int> feature_vals(1, (int)features_.size());
+    FeatureEvaluator *evaluator = NULL;
-    for( int i = 0; i < (int)features_.size(); ++i )
+    if( type == "acf" )
-    {
+        evaluator = new ACFFeatureEvaluatorImpl(features);
-        feature_vals(0, i) = evaluate(i);
+    else if( type == "icf" )
-    }
+        evaluator = new ICFFeatureEvaluatorImpl(features);
-    feature_values.setTo(feature_vals);
+    else
+        CV_Error(CV_StsBadArg, "type value is either acf or icf");
+
+    return Ptr<FeatureEvaluator>(evaluator);
 }
 
-Ptr<ACFFeatureEvaluator>
+vector<vector<int> > generateFeatures(Size window_size, const std::string& type,
-createACFFeatureEvaluator(const vector<Point3i>& features)
+                                      int count, int channel_count)
-{
-    return Ptr<ACFFeatureEvaluator>(new ACFFeatureEvaluatorImpl(features));
-}
-
-vector<Point3i> generateFeatures(Size window_size, int count)
 {
     CV_Assert(count > 0);
-    int cur_count = 0;
+    vector<vector<int> > features;
-    int max_count = window_size.width * window_size.height / 16;
+    if( type == "acf" )
-    count = min(count, max_count);
+    {
-    vector<Point3i> features;
+        int cur_count = 0;
-    for( int x = 0; x < window_size.width / 4; ++x )
+        int max_count = window_size.width * window_size.height / 16;
+        count = min(count, max_count);
+        for( int x = 0; x < window_size.width / 4; ++x )
+            for( int y = 0; y < window_size.height / 4; ++y )
+                for( int n = 0; n < channel_count; ++n )
+                {
+                    int f[] = {x, y, n};
+                    vector<int> feature(f, f + sizeof(f) / sizeof(*f));
+                    features.push_back(feature);
+                    if( (cur_count += 1) == count )
+                        break;
+                }
+    }
+    else if( type == "icf" )
     {
-        for( int y = 0; y < window_size.height / 4; ++y )
+        RNG rng;
+        for( int i = 0; i < count; ++i )
         {
-            /* Assume there are 10 channel types */
+            int x = rng.uniform(0, window_size.width - 1);
-            for( int n = 0; n < 10; ++n )
+            int y = rng.uniform(0, window_size.height - 1);
-            {
+            int x_to = rng.uniform(x, window_size.width - 1);
-                features.push_back(Point3i(x, y, n));
+            int y_to = rng.uniform(y, window_size.height - 1);
-                if( (cur_count += 1) == count )
+            int n = rng.uniform(0, channel_count - 1);
-                    break;
+            int f[] = {x, y, x_to, y_to, n};
-            }
+            vector<int> feature(f, f + sizeof(f) / sizeof(*f));
+            features.push_back(feature);
         }
     }
+    else
+        CV_Error(CV_StsBadArg, "type value is either acf or icf");
+
     return features;
 }
 
-void computeChannels(cv::InputArray image, cv::OutputArrayOfArrays channels_)
+void computeChannels(InputArray image, vector<Mat>& channels)
 {
     Mat src(image.getMat().rows, image.getMat().cols, CV_32FC3);
     image.getMat().convertTo(src, CV_32FC3, 1./255);
 
     Mat_<float> grad;
-    Mat gray;
+    Mat luv, gray;
     cvtColor(src, gray, CV_RGB2GRAY);
+    cvtColor(src, luv, CV_RGB2Luv);
 
     Mat_<float> row_der, col_der;
     Sobel(gray, row_der, CV_32F, 0, 1);
@@ -174,7 +285,7 @@ void computeChannels(cv::InputArray image, cv::OutputArrayOfArrays channels_)
 
     magnitude(row_der, col_der, grad);
 
-    Mat_<Vec6f> hist(grad.rows, grad.cols);
+    Mat_<Vec6f> hist = Mat_<Vec6f>::zeros(grad.rows, grad.cols);
     const float to_deg = 180 / 3.1415926f;
     for (int row = 0; row < grad.rows; ++row) {
         for (int col = 0; col < grad.cols; ++col) {
@@ -182,12 +293,22 @@ void computeChannels(cv::InputArray image, cv::OutputArrayOfArrays channels_)
             if (angle < 0)
                 angle += 180;
             int ind = (int)(angle / 30);
-            hist(row, col)[ind] = grad(row, col);
+
+            // If angle == 180, prevent index overflow
+            if (ind == 6)
+                ind = 5;
+
+            hist(row, col)[ind] = grad(row, col) * 255;
         }
     }
 
-    vector<Mat> channels;
+    channels.clear();
-    channels.push_back(gray);
+
+    Mat luv_channels[3];
+    split(luv, luv_channels);
+    for( int i = 0; i < 3; ++i )
+        channels.push_back(luv_channels[i]);
+
     channels.push_back(grad);
 
     vector<Mat> hist_channels;
@@ -195,8 +316,6 @@ void computeChannels(cv::InputArray image, cv::OutputArrayOfArrays channels_)
 
     for( size_t i = 0; i < hist_channels.size(); ++i )
         channels.push_back(hist_channels[i]);
-
-    channels_.setTo(channels);
 }
 
 } /* namespace xobjdetect */

modules/xobjdetect/src/icfdetector.cpp

@@ -39,6 +39,17 @@ the use of this software, even if advised of the possibility of such damage.
 
 */
 
+#include <sstream>
+using std::ostringstream;
+
+#include <iomanip>
+using std::setfill;
+using std::setw;
+
+#include <iostream>
+using std::cout;
+using std::endl;
+
 #include "precomp.hpp"
 
 using std::vector;
@@ -52,47 +63,41 @@ namespace cv
 namespace xobjdetect
 {
 
-static bool overlap(const Rect& r, const vector<Rect>& gt)
+void ICFDetector::train(const String& pos_path,
-{
+                        const String& bg_path,
-    for( size_t i = 0; i < gt.size(); ++i )
-        if( (r & gt[i]).area() )
-            return true;
-    return false;
-}
-
-void ICFDetector::train(const vector<string>& image_filenames,
-                        const vector< vector<Rect> >& labelling,
                         ICFDetectorParams params)
 {
+    vector<String> pos_filenames;
+    glob(pos_path + "/*.png", pos_filenames);
+
+    vector<String> bg_filenames;
+    glob(bg_path + "/*.jpg", bg_filenames);
+
+    model_n_rows_ = params.model_n_rows;
+    model_n_cols_ = params.model_n_cols;
+
     Size model_size(params.model_n_cols, params.model_n_rows);
 
     vector<Mat> samples; /* positive samples + negative samples */
     Mat sample, resized_sample;
     int pos_count = 0;
-    for( size_t i = 0; i < image_filenames.size(); ++i, ++pos_count )
-    {
-        Mat img = imread(String(image_filenames[i].c_str()));
-        for( size_t j = 0; j < labelling[i].size(); ++j )
-        {
-            Rect r = labelling[i][j];
-            if( r.x > img.cols || r.y > img.rows )
-                continue;
 
-            sample = img.colRange(max(r.x, 0), min(r.width, img.cols))
+    for( size_t i = 0; i < pos_filenames.size(); ++i, ++pos_count )
-                        .rowRange(max(r.y, 0), min(r.height, img.rows));
+    {
-
+        cout << setw(6) << (i + 1) << "/" << pos_filenames.size() << "\r";
-            resize(sample, resized_sample, model_size);
+        Mat img = imread(pos_filenames[i]);
-
+        resize(img, resized_sample, model_size);
-            samples.push_back(resized_sample);
+        samples.push_back(resized_sample.clone());
-        }
     }
+    cout << "\n";
 
     int neg_count = 0;
     RNG rng;
-    for( size_t i = 0; i < image_filenames.size(); ++i )
+    for( size_t i = 0; i < bg_filenames.size(); ++i )
     {
-        Mat img = imread(String(image_filenames[i].c_str()));
+        cout << setw(6) << (i + 1) << "/" << bg_filenames.size() << "\r";
-        for( int j = 0; j < (int)(pos_count / image_filenames.size() + 1); )
+        Mat img = imread(bg_filenames[i]);
+        for( int j = 0; j < params.bg_per_image; ++j, ++neg_count)
         {
             Rect r;
             r.x = rng.uniform(0, img.cols);
@@ -100,16 +105,12 @@ void ICFDetector::train(const vector<string>& image_filenames,
             r.y = rng.uniform(0, img.rows);
             r.height = rng.uniform(r.y + 1, img.rows);
 
-            if( !overlap(r, labelling[i]) )
+            sample = img.colRange(r.x, r.width).rowRange(r.y, r.height);
-            {
+            resize(sample, resized_sample, model_size);
-                sample = img.colRange(r.x, r.width).rowRange(r.y, r.height);
+            samples.push_back(resized_sample.clone());
-                //resize(sample, resized_sample);
-                samples.push_back(resized_sample);
-                ++neg_count;
-                ++j;
-            }
         }
     }
+    cout << "\n";
 
     Mat_<int> labels(1, pos_count + neg_count);
     for( int i = 0; i < pos_count; ++i)
@@ -117,33 +118,136 @@ void ICFDetector::train(const vector<string>& image_filenames,
     for( int i = pos_count; i < pos_count + neg_count; ++i )
         labels(0, i) = -1;
 
-    vector<Point3i> features = generateFeatures(model_size);
+    vector<vector<int> > features = generateFeatures(model_size, "icf",
-    Ptr<ACFFeatureEvaluator> feature_evaluator = createACFFeatureEvaluator(features);
+        params.feature_count);
+    Ptr<FeatureEvaluator> evaluator = createFeatureEvaluator(features, "icf");
 
-    Mat_<int> data((int)features.size(), (int)samples.size());
+    Mat_<int> data = Mat_<int>::zeros((int)features.size(), (int)samples.size());
-    Mat_<int> feature_col;
+    Mat_<int> feature_col(1, (int)samples.size());
 
     vector<Mat> channels;
     for( int i = 0; i < (int)samples.size(); ++i )
     {
+        cout << setw(6) << i << "/" << samples.size() << "\r";
         computeChannels(samples[i], channels);
-        feature_evaluator->setChannels(channels);
+        evaluator->setChannels(channels);
-        feature_evaluator->evaluateAll(feature_col);
+        //evaluator->assertChannels();
-        for( int j = 0; j < feature_col.rows; ++j )
+        evaluator->evaluateAll(feature_col);
-            data(i, j) = feature_col(0, j);
+
+        CV_Assert(feature_col.cols == (int)features.size());
+
+        for( int j = 0; j < feature_col.cols; ++j )
+            data(j, i) = feature_col(0, j);
     }
+    cout << "\n";
+    samples.clear();
 
     WaldBoostParams wparams;
     wparams.weak_count = params.weak_count;
-    wparams.alpha = 0.001f;
+    wparams.alpha = 0.02f;
+
+    waldboost_ = createWaldBoost(wparams);
+    vector<int> indices = waldboost_->train(data, labels);
+    cout << "indices: ";
+    for( size_t i = 0; i < indices.size(); ++i )
+        cout << indices[i] << " ";
+    cout << endl;
+
+    features_.clear();
+    for( size_t i = 0; i < indices.size(); ++i )
+        features_.push_back(features[indices[i]]);
+}
 
-    Ptr<WaldBoost> waldboost = createWaldBoost(wparams);
+void ICFDetector::write(FileStorage& fs) const
-    waldboost->train(data, labels);
+{
+    fs << "{";
+    fs << "model_n_rows" << model_n_rows_;
+    fs << "model_n_cols" << model_n_cols_;
+    fs << "waldboost";
+    waldboost_->write(fs);
+    fs << "features" << "[";
+    for( size_t i = 0; i < features_.size(); ++i )
+    {
+        fs << features_[i];
+    }
+    fs << "]";
+    fs << "}";
+}
+
+void ICFDetector::read(const FileNode& node)
+{
+    waldboost_ = Ptr<WaldBoost>(createWaldBoost(WaldBoostParams()));
+    node["model_n_rows"] >> model_n_rows_;
+    node["model_n_cols"] >> model_n_cols_;
+    waldboost_->read(node["waldboost"]);
+    FileNode features = node["features"];
+    features_.clear();
+    vector<int> p;
+    for( FileNodeIterator n = features.begin(); n != features.end(); ++n )
+    {
+        (*n) >> p;
+        features_.push_back(p);
+    }
+}
+
+void ICFDetector::detect(const Mat& img, vector<Rect>& objects,
+    float scaleFactor, Size minSize, Size maxSize, float threshold)
+{
+    float scale_from = min(model_n_cols_ / (float)maxSize.width,
+                           model_n_rows_ / (float)maxSize.height);
+    float scale_to = max(model_n_cols_ / (float)minSize.width,
+                         model_n_rows_ / (float)minSize.height);
+    objects.clear();
+    Ptr<FeatureEvaluator> evaluator = createFeatureEvaluator(features_, "icf");
+    Mat rescaled_image;
+    int step = 8;
+    vector<Mat> channels;
+    for( float scale = scale_from; scale < scale_to + 0.001; scale *= scaleFactor )
+    {
+        cout << "scale " << scale << endl;
+        int new_width = int(img.cols * scale);
+        new_width -= new_width % 4;
+        int new_height = int(img.rows * scale);
+        new_height -= new_height % 4;
+
+        resize(img, rescaled_image, Size(new_width, new_height));
+        computeChannels(rescaled_image, channels);
+        evaluator->setChannels(channels);
+        for( int row = 0; row <= rescaled_image.rows - model_n_rows_; row += step)
+        {
+            for( int col = 0; col <= rescaled_image.cols - model_n_cols_;
+                col += step )
+            {
+                evaluator->setPosition(Size(row, col));
+                float value = waldboost_->predict(evaluator);
+                if( value > threshold )
+                {
+                    int x = (int)(col / scale);
+                    int y = (int)(row / scale);
+                    int width = (int)(model_n_cols_ / scale);
+                    int height = (int)(model_n_rows_ / scale);
+                    cout << value << " " << x << " " << y << " " << width << " "
+                         << height << endl;
+                    objects.push_back(Rect(x, y, width, height));
+                }
+            }
+        }
+
+    }
+}
+
+void write(FileStorage& fs, String&, const ICFDetector& detector)
+{
+    detector.write(fs);
 }
 
-bool ICFDetector::save(const string&)
+void read(const FileNode& node, ICFDetector& d,
+    const ICFDetector& default_value)
 {
-    return true;
+    if( node.empty() )
+        d = default_value;
+    else
+        d.read(node);
 }
 
 } /* namespace xobjdetect */

modules/xobjdetect/src/precomp.hpp

@@ -45,6 +45,8 @@ the use of this software, even if advised of the possibility of such damage.
 #include <opencv2/xobjdetect.hpp>
 #include <opencv2/xobjdetect/private.hpp>
 
+#include <opencv2/core/utility.hpp>
+
 #include <opencv2/imgproc.hpp>
 #include <opencv2/imgproc/types_c.h>
 

modules/xobjdetect/src/stump.cpp

@@ -65,131 +65,119 @@ int Stump::train(const Mat& data, const Mat& labels, const Mat& weights)
 {
     CV_Assert(labels.rows == 1 && labels.cols == data.cols);
     CV_Assert(weights.rows == 1 && weights.cols == data.cols);
+    CV_Assert(data.cols > 1);
     /* Assert that data and labels have int type */
     /* Assert that weights have float type */
 
+    Mat_<int> d = Mat_<int>::zeros(1, data.cols);
+    const Mat_<int>& l = labels;
+    const Mat_<float>& w = weights;
 
-    /* Prepare labels for each feature rearranged according to sorted order */
+    Mat_<int> indices(1, l.cols);
-    Mat sorted_labels(data.rows, data.cols, labels.type());
+
-    Mat sorted_weights(data.rows, data.cols, weights.type());
+    Mat_<int> sorted_d(1, data.cols);
-    Mat indices;
+    Mat_<int> sorted_l(1, l.cols);
-    sortIdx(data, indices, cv::SORT_EVERY_ROW | cv::SORT_ASCENDING);
+    Mat_<float> sorted_w(1, w.cols);
-    for( int row = 0; row < indices.rows; ++row )
-    {
-        for( int col = 0; col < indices.cols; ++col )
-        {
-            sorted_labels.at<int>(row, col) =
-                labels.at<int>(0, indices.at<int>(row, col));
-            sorted_weights.at<float>(row, col) =
-                weights.at<float>(0, indices.at<int>(row, col));
-        }
-    }
 
-    /* Sort feature values */
-    Mat sorted_data(data.rows, data.cols, data.type());
-    sort(data, sorted_data, cv::SORT_EVERY_ROW | cv::SORT_ASCENDING);
 
-    /* Split positive and negative weights */
+    Mat_<float> pos_c_w = Mat_<float>::zeros(1, w.cols);
-    Mat pos_weights = Mat::zeros(sorted_weights.rows, sorted_weights.cols,
+    Mat_<float> neg_c_w = Mat_<float>::zeros(1, w.cols);
-        sorted_weights.type());
+
-    Mat neg_weights = Mat::zeros(sorted_weights.rows, sorted_weights.cols,
+
-        sorted_weights.type());
+    float min_err = FLT_MAX;
+    int min_row = -1;
+    int min_thr = -1;
+    int min_pol = -1;
+    float min_pos = 1;
+    float min_neg = -1;
+    float eps = 1.0f / (4 * l.cols);
+
+    /* For every feature */
     for( int row = 0; row < data.rows; ++row )
     {
         for( int col = 0; col < data.cols; ++col )
-        {
+            d(0, col) = data.at<int>(row, col);
-            if( sorted_labels.at<int>(row, col) == +1 )
-            {
-                pos_weights.at<float>(row, col) =
-                    sorted_weights.at<float>(row, col);
-            }
-            else
-            {
-                neg_weights.at<float>(row, col) =
-                    sorted_weights.at<float>(row, col);
-            }
-        }
-    }
 
-    /* Compute cumulative sums for fast stump error computation */
+        sortIdx(d, indices, cv::SORT_EVERY_ROW | cv::SORT_ASCENDING);
-    Mat pos_cum_weights = Mat::zeros(sorted_weights.rows, sorted_weights.cols,
-        sorted_weights.type());
-    Mat neg_cum_weights = Mat::zeros(sorted_weights.rows, sorted_weights.cols,
-        sorted_weights.type());
-    cumsum(pos_weights, pos_cum_weights);
-    cumsum(neg_weights, neg_cum_weights);
 
-    /* Compute total weights of positive and negative samples */
+        for( int col = 0; col < indices.cols; ++col )
-    float pos_total_weight = pos_cum_weights.at<float>(0, weights.cols - 1);
+        {
-    float neg_total_weight = neg_cum_weights.at<float>(0, weights.cols - 1);
+            int ind = indices(0, col);
+            sorted_d(0, col) = d(0, ind);
+            sorted_l(0, col) = l(0, ind);
+            sorted_w(0, col) = w(0, ind);
+        }
 
+        Mat_<float> pos_w = Mat_<float>::zeros(1, w.cols);
+        Mat_<float> neg_w = Mat_<float>::zeros(1, w.cols);
+        for( int col = 0; col < d.cols; ++col )
+        {
+            float weight = sorted_w(0, col);
+            if( sorted_l(0, col) == +1)
+                pos_w(0, col) = weight;
+            else
+                neg_w(0, col) = weight;
+        }
 
-    float eps = 1.0f / (4 * labels.cols);
+        cumsum(pos_w, pos_c_w);
+        cumsum(neg_w, neg_c_w);
 
-    /* Compute minimal error */
+        float pos_total_w = pos_c_w(0, w.cols - 1);
-    float min_err = FLT_MAX;
+        float neg_total_w = neg_c_w(0, w.cols - 1);
-    int min_row = -1;
-    int min_col = -1;
-    int min_polarity = 0;
-    float min_pos_value = 1, min_neg_value = -1;
 
-    for( int row = 0; row < sorted_weights.rows; ++row )
+        for( int col = 0; col < w.cols - 1; ++col )
-    {
-        for( int col = 0; col < sorted_weights.cols - 1; ++col )
         {
             float err, h_pos, h_neg;
+            float pos_wrong, pos_right;
+            float neg_wrong, neg_right;
 
-            // Direct polarity
+            /* Direct polarity */
-
-            float pos_wrong = pos_cum_weights.at<float>(row, col);
-            float pos_right = pos_total_weight - pos_wrong;
 
-            float neg_right = neg_cum_weights.at<float>(row, col);
+            pos_wrong = pos_c_w(0, col);
-            float neg_wrong = neg_total_weight - neg_right;
+            pos_right = pos_total_w - pos_wrong;
 
-            h_pos = (float)(.5 * log((pos_right + eps) / (pos_wrong + eps)));
+            neg_right = neg_c_w(0, col);
-            h_neg = (float)(.5 * log((neg_wrong + eps) / (neg_right + eps)));
+            neg_wrong = neg_total_w - neg_right;
 
             err = sqrt(pos_right * neg_wrong) + sqrt(pos_wrong * neg_right);
 
+            h_pos = .5f * log((pos_right + eps) / (pos_wrong + eps));
+            h_neg = .5f * log((neg_wrong + eps) / (neg_right + eps));
+
             if( err < min_err )
             {
                 min_err = err;
                 min_row = row;
-                min_col = col;
+                min_thr = (sorted_d(0, col) + sorted_d(0, col + 1)) / 2;
-                min_polarity = +1;
+                min_pol = +1;
-                min_pos_value = h_pos;
+                min_pos = h_pos;
-                min_neg_value = h_neg;
+                min_neg = h_neg;
             }
 
-            // Opposite polarity
+            /* Opposite polarity */
+
             swap(pos_right, pos_wrong);
             swap(neg_right, neg_wrong);
 
-            h_pos = -h_pos;
-            h_neg = -h_neg;
-
             err = sqrt(pos_right * neg_wrong) + sqrt(pos_wrong * neg_right);
 
-
             if( err < min_err )
             {
                 min_err = err;
                 min_row = row;
-                min_col = col;
+                min_thr = (sorted_d(0, col) + sorted_d(0, col + 1)) / 2;
-                min_polarity = -1;
+                min_pol = -1;
-                min_pos_value = h_pos;
+                min_pos = -h_pos;
-                min_neg_value = h_neg;
+                min_neg = -h_neg;
             }
+
         }
     }
 
-    /* Compute threshold, store found values in fields */
+    threshold_ = min_thr;
-    threshold_ = ( sorted_data.at<int>(min_row, min_col) +
+    polarity_ = min_pol;
-                   sorted_data.at<int>(min_row, min_col + 1) ) / 2;
+    pos_value_ = min_pos;
-    polarity_ = min_polarity;
+    neg_value_ = min_neg;
-    pos_value_ = min_pos_value;
-    neg_value_ = min_neg_value;
 
     return min_row;
 }
@@ -199,5 +187,19 @@ float Stump::predict(int value) const
     return polarity_ * (value - threshold_) > 0 ? pos_value_ : neg_value_;
 }
 
+
+void read(const FileNode& node, Stump& s, const Stump& default_value)
+{
+    if( node.empty() )
+        s = default_value;
+    else
+        s.read(node);
+}
+
+void write(FileStorage& fs, String&, const Stump& s)
+{
+    s.write(fs);
+}
+
 } /* namespace xobjdetect */
 } /* namespace cv */

modules/xobjdetect/src/waldboost.cpp

@@ -45,6 +45,11 @@ using std::swap;
 
 using std::vector;
 
+#include <iostream>
+using std::cout;
+using std::endl;
+
+
 namespace cv
 {
 namespace xobjdetect
@@ -62,7 +67,11 @@ public:
                                    const Mat& labels);
 
     virtual float predict(
-        const Ptr<ACFFeatureEvaluator>& feature_evaluator) const;
+        const Ptr<FeatureEvaluator>& feature_evaluator) const;
+
+    virtual void write(FileStorage& fs) const;
+
+    virtual void read(const FileNode& node);
 
 private:
     /* Parameters for cascade training */
@@ -73,18 +82,82 @@ private:
     std::vector<float> thresholds_;
 };
 
-vector<int> WaldBoostImpl::train(const Mat& data, const Mat& labels)
+static int count(const Mat_<int> &m, int elem)
 {
-    CV_Assert(labels.rows == 1 && labels.cols == data.cols);
+    int res = 0;
+    for( int row = 0; row < m.rows; ++row )
+        for( int col = 0; col < m.cols; ++col )
+            if( m(row, col) == elem)
+                res += 1;
+    return res;
+}
 
-    int pos_count = 0, neg_count = 0;
+void WaldBoostImpl::read(const FileNode& node)
-    for( int col = 0; col < labels.cols; ++col )
+{
+    FileNode params = node["waldboost_params"];
+    params_.weak_count = (int)(params["weak_count"]);
+    params_.alpha = (float)(params["alpha"]);
+
+    FileNode stumps = node["waldboost_stumps"];
+    stumps_.clear();
+    for( FileNodeIterator n = stumps.begin(); n != stumps.end(); ++n )
     {
-        if( labels.at<int>(0, col) == +1 )
+        Stump s;
-            pos_count += 1;
+        *n >> s;
-        else
+        stumps_.push_back(s);
-            neg_count += 1;
+    }
+
+    FileNode thresholds = node["waldboost_thresholds"];
+    thresholds_.clear();
+    for( FileNodeIterator n = thresholds.begin(); n != thresholds.end(); ++n )
+    {
+        float t;
+        *n >> t;
+        thresholds_.push_back(t);
     }
+}
+
+void WaldBoostImpl::write(FileStorage& fs) const
+{
+    fs << "{";
+    fs << "waldboost_params" << "{"
+        << "weak_count" << params_.weak_count
+        << "alpha" << params_.alpha
+        << "}";
+
+    fs << "waldboost_stumps" << "[";
+    for( size_t i = 0; i < stumps_.size(); ++i )
+        fs << stumps_[i];
+    fs << "]";
+
+    fs << "waldboost_thresholds" << "[";
+    for( size_t i = 0; i < thresholds_.size(); ++i )
+        fs << thresholds_[i];
+    fs << "]";
+    fs << "}";
+
+}
+
+vector<int> WaldBoostImpl::train(const Mat& data_, const Mat& labels_)
+{
+    CV_Assert(labels_.rows == 1 && labels_.cols == data_.cols);
+    CV_Assert(data_.rows >= params_.weak_count);
+
+    Mat labels, data;
+    data_.copyTo(data);
+    labels_.copyTo(labels);
+
+    bool null_data = true;
+    for( int row = 0; row < data.rows; ++row )
+    {
+        for( int col = 0; col < data.cols; ++col )
+            if( data.at<int>(row, col) )
+                null_data = false;
+    }
+    CV_Assert(!null_data);
+
+    int pos_count = count(labels, +1);
+    int neg_count = count(labels, -1);
 
     Mat_<float> weights(labels.rows, labels.cols);
     float pos_weight = 1.0f / (2 * pos_count);
@@ -97,6 +170,9 @@ vector<int> WaldBoostImpl::train(const Mat& data, const Mat& labels)
             weights.at<float>(0, col) = neg_weight;
     }
 
+    vector<int> feature_indices_pool;
+    for( int ind = 0; ind < data.rows; ++ind )
+        feature_indices_pool.push_back(ind);
 
     vector<int> feature_indices;
     Mat_<float> trace = Mat_<float>::zeros(labels.rows, labels.cols);
@@ -104,10 +180,14 @@ vector<int> WaldBoostImpl::train(const Mat& data, const Mat& labels)
     thresholds_.clear();
     for( int i = 0; i < params_.weak_count; ++i)
     {
+        cout << "stage " << i << endl;
         Stump s;
         int feature_ind = s.train(data, labels, weights);
+        cout << "feature_ind " << feature_ind << endl;
         stumps_.push_back(s);
-        feature_indices.push_back(feature_ind);
+        int ind = feature_indices_pool[feature_ind];
+        feature_indices_pool.erase(feature_indices_pool.begin() + feature_ind);
+        feature_indices.push_back(ind);
 
         // Recompute weights
         for( int col = 0; col < weights.cols; ++col )
@@ -118,6 +198,14 @@ vector<int> WaldBoostImpl::train(const Mat& data, const Mat& labels)
             weights.at<float>(0, col) *= exp(-label * h);
         }
 
+        // Erase row for feature in data
+        Mat fixed_data;
+        fixed_data.push_back(data.rowRange(0, feature_ind));
+        fixed_data.push_back(data.rowRange(feature_ind + 1, data.rows));
+
+        data = fixed_data;
+
+
         // Normalize weights
         float z = (float)sum(weights)[0];
         for( int col = 0; col < weights.cols; ++col)
@@ -125,12 +213,12 @@ vector<int> WaldBoostImpl::train(const Mat& data, const Mat& labels)
             weights.at<float>(0, col) /= z;
         }
 
-
         // Sort trace
         Mat indices;
         sortIdx(trace, indices, cv::SORT_EVERY_ROW | cv::SORT_ASCENDING);
         Mat new_weights = Mat_<float>::zeros(weights.rows, weights.cols);
         Mat new_labels = Mat_<int>::zeros(labels.rows, labels.cols);
+        Mat new_data = Mat_<int>::zeros(data.rows, data.cols);
         Mat new_trace;
         for( int col = 0; col < new_weights.cols; ++col )
         {
@@ -138,34 +226,69 @@ vector<int> WaldBoostImpl::train(const Mat& data, const Mat& labels)
                 weights.at<float>(0, indices.at<int>(0, col));
             new_labels.at<int>(0, col) =
                 labels.at<int>(0, indices.at<int>(0, col));
+            for( int row = 0; row < new_data.rows; ++row )
+            {
+                new_data.at<int>(row, col) =
+                    data.at<int>(row, indices.at<int>(0, col));
+            }
         }
         sort(trace, new_trace, cv::SORT_EVERY_ROW | cv::SORT_ASCENDING);
 
 
         // Compute threshold for trace
+        /*
         int col = 0;
         for( int pos_i = 0;
-             pos_i < pos_count * params_.alpha && col < weights.cols;
+             pos_i < pos_count * params_.alpha && col < new_labels.cols;
              ++col )
         {
-            if( labels.at<int>(0, col) == +1 )
+            if( new_labels.at<int>(0, col) == +1 )
                 ++pos_i;
         }
+        */
+
+        int cur_pos = 0, cur_neg = 0;
+        int max_col = 0;
+        for( int col = 0; col < new_labels.cols - 1; ++col ) {
+            if (new_labels.at<int>(0, col) == +1 )
+                ++cur_pos;
+            else
+                ++cur_neg;
+
+            float p_neg = cur_neg / (float)neg_count;
+            float p_pos = cur_pos / (float)pos_count;
+            if( params_.alpha * p_neg > p_pos )
+                max_col = col;
+        }
 
-        thresholds_.push_back(new_trace.at<float>(0, col));
+        thresholds_.push_back(new_trace.at<float>(0, max_col));
+        cout << "threshold " << *(thresholds_.end() - 1) << endl;
+
+        cout << "col " << max_col << " size " << data.cols << endl;
 
         // Drop samples below threshold
-        new_trace.colRange(col, new_trace.cols).copyTo(trace);
+        new_data.colRange(max_col, new_data.cols).copyTo(data);
-        new_weights.colRange(col, new_weights.cols).copyTo(weights);
+        new_trace.colRange(max_col, new_trace.cols).copyTo(trace);
-        new_labels.colRange(col, new_labels.cols).copyTo(labels);
+        new_weights.colRange(max_col, new_weights.cols).copyTo(weights);
+        new_labels.colRange(max_col, new_labels.cols).copyTo(labels);
+
+        pos_count = count(labels, +1);
+        neg_count = count(labels, -1);
+        cout << "pos_count " << pos_count << "; neg_count " << neg_count << endl;
+
+        if( data.cols < 2 || neg_count == 0)
+        {
+            break;
+        }
     }
     return feature_indices;
 }
 
 float WaldBoostImpl::predict(
-    const Ptr<ACFFeatureEvaluator>& feature_evaluator) const
+    const Ptr<FeatureEvaluator>& feature_evaluator) const
 {
     float trace = 0;
+    CV_Assert(stumps_.size() == thresholds_.size());
     for( size_t i = 0; i < stumps_.size(); ++i )
     {
         int value = feature_evaluator->evaluate(i);