├── doc └── track_robot.jpg ├── Makefile ├── src ├── ProbMatrix.cpp ├── Crutchfield.cpp ├── FileImageSource.cpp ├── Histogram.cpp └── PositionParticleFilter.cpp ├── inc ├── DistanceSource.h ├── ImageSource.h ├── Config.h ├── Print.hpp ├── File.hpp ├── strstr.h ├── Crutchfield.h ├── chunkbuffer.hpp ├── FileImageSource.h ├── Histogram.h ├── Autoregression.hpp ├── imgbuffer.hpp ├── ProbMatrix.h ├── PositionParticleFilter.h ├── IpcamImageSource.h ├── ParticleFilter.hpp ├── ConfigFile.hpp ├── alphanum.hpp └── Container.hpp ├── test ├── testHistogram.h ├── testRegression.h ├── testFilter.h ├── testConvolution.h ├── createImages.h ├── testImageHistogram.h ├── createTrackImage.h ├── testDistance.h └── main.cpp ├── CMakeLists.txt └── README.md /doc/track_robot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mrquincle/particlefilter/HEAD/doc/track_robot.jpg -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | #!/bin/make 2 | 3 | # Author: Anne C. van Rossum 4 | # Date: Apr. 16, 2012 5 | 6 | all: 7 | @mkdir -p build 8 | cd build && cmake $(CMAKE_FLAGS) .. $(FLAGS) 9 | cd build && make 10 | 11 | clean: 12 | cd build && make clean 13 | 14 | distclean: clean 15 | 16 | .PHONY: all clean distclean 17 | -------------------------------------------------------------------------------- /src/ProbMatrix.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file ProbMatrix.cpp 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 6, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | #include 25 | #include 26 | 27 | /* ************************************************************************************** 28 | * Implementation of ProbMatrix 29 | * **************************************************************************************/ 30 | 31 | ProbMatrix::ProbMatrix(int bins, int width, int height): bins(bins), 32 | p_height(height), 33 | p_width(width), 34 | p_size(height * width), 35 | bins_squared(bins*bins), 36 | freq(NULL), 37 | joint_freq(NULL), 38 | frame_count(0) { 39 | 40 | } 41 | 42 | ProbMatrix::~ProbMatrix() { 43 | 44 | } 45 | -------------------------------------------------------------------------------- /inc/DistanceSource.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file DistanceSource.h 3 | * @brief Abstract class for algorithms that generate distances between sensors 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object against this software used by the military, in the 12 | * bio-industry, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2010 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Feb 23, 2011 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case 22 | */ 23 | 24 | 25 | #ifndef DISTANCESOURCE_H_ 26 | #define DISTANCESOURCE_H_ 27 | 28 | // General files 29 | 30 | /* ************************************************************************************** 31 | * Interface of DistanceSource 32 | * **************************************************************************************/ 33 | 34 | /** 35 | * Abstract class that defines getDistance() for two given sensors. 36 | */ 37 | class DistanceSource { 38 | public: 39 | //! Constructor DistanceSource 40 | DistanceSource() {} 41 | 42 | //! Destructor ~DistanceSource 43 | virtual ~DistanceSource() {} 44 | 45 | //! Get distance 46 | virtual float getDistance(int sensor0, int sensor1) = 0; 47 | 48 | //! Get number of sensors 49 | virtual int getSensorCount() = 0; 50 | protected: 51 | 52 | private: 53 | 54 | }; 55 | 56 | #endif /* DISTANCESOURCE_H_ */ 57 | -------------------------------------------------------------------------------- /test/testHistogram.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file testHistogram.cpp 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 14, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | using namespace std; 30 | 31 | void test_histogram() { 32 | cout << " === start test histogram === " << endl; 33 | 34 | for (int i = 0; i < 2; ++i) { 35 | int size = 10; 36 | int bins = 4; 37 | Histogram histogram(bins, size, 1); 38 | 39 | DataFrames frames; 40 | int nof_frames = 1; 41 | for (int f = 0; f < nof_frames; ++f) { 42 | pDataMatrix data = new DataValue[size]; 43 | for (int i = 0; i < size; ++i) { 44 | data[i] = 40 * ((i % 2) + 1); // set values 40 or 80 45 | } 46 | frames.push_back(data); 47 | } 48 | 49 | histogram.calcProbabilities(frames); 50 | 51 | std::vector result; 52 | histogram.getProbabilities(result); 53 | 54 | cout << "Result: "; 55 | for (int i = 0; i < result.size(); ++i) { 56 | cout << result[i] << ' '; 57 | } 58 | cout << endl; 59 | } 60 | cout << " === end test histogram === " << endl; 61 | } 62 | -------------------------------------------------------------------------------- /test/testRegression.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file testRegression.cpp 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 14, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | using namespace std; 29 | 30 | void test_autoregression() { 31 | cout << " === start test histogram === " << endl; 32 | 33 | cout << "[*] test predict" << endl; 34 | std::vector x; 35 | x.clear(); 36 | std::vector c; 37 | c.clear(); 38 | c.push_back(1); 39 | c.push_back(-2); 40 | cout << "Coefficients: "; 41 | dobots::print(c.begin(), c.end()); 42 | 43 | double result = 0.0; 44 | x.push_back(1); 45 | x.push_back(1.2); 46 | x.push_back(1.4); 47 | 48 | double sum = std::inner_product(x.begin(), x.end(), c.begin(), 0.0); 49 | cout << "Inner product: " << sum << endl; 50 | 51 | for (int i = 0; i < 10; ++i) { 52 | result = dobots::predict(x.begin(), x.end(), c.begin(), 0.0); 53 | dobots::pushpop(x.begin(), x.end(), result); 54 | } 55 | cout << "Predicted values after 10 time steps: "; 56 | dobots::print(x.begin(), x.end()); 57 | 58 | cout << "[*] test print and rotate and pushpop" << endl; 59 | x.clear(); 60 | for (int i = 0; i < 10; ++i) { 61 | x.push_back(i); 62 | } 63 | dobots::print(x.begin(), x.end()); 64 | dobots::pushpop(x.begin(), x.end(), 30); 65 | dobots::pushpop(x.begin(), x.end(), 40); 66 | dobots::print(x.begin(), x.end()); 67 | 68 | cout << " === end test histogram === " << endl; 69 | } 70 | -------------------------------------------------------------------------------- /test/testFilter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file testFilter.h 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 15, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | using namespace dobots; 29 | 30 | /** 31 | * If you use a stream operator to print the particles, you will need to define it for 32 | * your state variable too. 33 | */ 34 | struct TestData { 35 | int fieldA; 36 | int fieldB; 37 | 38 | friend std::ostream& operator<<(std::ostream& os, const TestData & p) { 39 | os << p.fieldA << ',' << p.fieldB; 40 | return os; 41 | } 42 | }; 43 | 44 | class TestParticleFilter: public ParticleFilter { 45 | public: 46 | TestParticleFilter() { 47 | particle_count = 10; 48 | } 49 | 50 | ~TestParticleFilter() {} 51 | 52 | void Init() { 53 | for (int i = 1; i < particle_count+1; ++i) { 54 | Particle *p = new Particle(); 55 | TestData *data = new TestData(); 56 | data->fieldA = i; data->fieldB = i; 57 | p->setState(data); 58 | p->setWeight(i); // unnormalized 59 | getParticles().push_back(p); 60 | } 61 | } 62 | 63 | void Print() { 64 | std::cout << "Particles (in order): "; 65 | print(getParticles().begin(), getParticles().end()); 66 | } 67 | 68 | void Transition() { 69 | assert(false); 70 | } 71 | 72 | void Likelihood() { 73 | assert(false); 74 | } 75 | 76 | private: 77 | int particle_count; 78 | }; 79 | 80 | 81 | void test_filter() { 82 | TestParticleFilter filter; 83 | filter.Init(); 84 | filter.Print(); 85 | filter.Resample(); 86 | filter.Print(); 87 | } 88 | -------------------------------------------------------------------------------- /test/testConvolution.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file testConvolution.h 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 23, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | 25 | #ifndef TESTCONVOLUTION_H_ 26 | #define TESTCONVOLUTION_H_ 27 | 28 | #include 29 | #include 30 | 31 | using namespace dobots; 32 | using namespace std; 33 | 34 | void test_convolution() { 35 | std::vector vec1; 36 | std::vector vec2; 37 | std::vector vec3; 38 | vec1.clear(); 39 | vec2.clear(); 40 | vec3.clear(); 41 | int size = 4; 42 | 43 | for (int i = 0; i < size; ++i) 44 | vec1.push_back(i+1); 45 | 46 | cout << "Vector 1: "; 47 | print(vec1.begin(), vec1.end()); 48 | 49 | for (int i = 0; i < size; ++i) 50 | vec2.push_back(i+1); 51 | 52 | cout << "Vector 2: "; 53 | print(vec2.begin(), vec2.end()); 54 | 55 | vec3.resize(size); 56 | 57 | integral(vec1.begin(), vec1.end(), vec2.begin(), vec3.begin()); 58 | cout << "Integral: "; 59 | print(vec3.begin(), vec3.end()); 60 | 61 | clean(vec3.begin(), vec3.end()); 62 | cauchy_product(vec1.begin(), vec1.end(), vec2.end(), vec3.begin()); 63 | cout << "Cauchy: "; 64 | print(vec3.begin(), vec3.end()); 65 | 66 | // circular convolution, check quickly with octave 67 | // vec3 = ifft(fft(vec1).*fft(vec2)): 26 28 26 20 68 | clean(vec3.begin(), vec3.end()); 69 | circular_convolution(vec1.begin(), vec1.end(), vec2.begin(), vec2.end(), vec3.begin(), 1); 70 | cout << "Circular convolution: "; 71 | print(vec3.begin(), vec3.end()); 72 | 73 | cout << "Vector 2: "; 74 | print(vec2.begin(), vec2.end()); 75 | } 76 | 77 | #endif /* TESTCONVOLUTION_H_ */ 78 | -------------------------------------------------------------------------------- /inc/ImageSource.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file ImageSource.h 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Sep 18, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | 25 | #ifndef IMAGESOURCE_H_ 26 | #define IMAGESOURCE_H_ 27 | 28 | // General files 29 | #include 30 | #include 31 | 32 | /* ************************************************************************************** 33 | * Interface of ImageSource 34 | * **************************************************************************************/ 35 | 36 | template 37 | class ImageSource { 38 | public: 39 | //! Constructor ImageSource 40 | ImageSource(): img_path(""), img_basename("image"), img_extension(".jpeg") {} 41 | 42 | //! Destructor ~ImageSource 43 | virtual ~ImageSource() {} 44 | 45 | //! Perform functionality that is required to get images 46 | virtual bool Update() = 0; 47 | 48 | //! Get an image (the next image if there are multiple). 49 | virtual Image* getImage() = 0; 50 | 51 | //! Get an image but shifted in maximum two directions. 52 | virtual Image* getImageShifted(int shift_x, int shift_y) = 0; 53 | 54 | //! Get the path 55 | void SetPath(std::string path) { img_path = path; } 56 | 57 | //! Get the path 58 | void SetBasename(std::string basename) { img_basename = basename; } 59 | 60 | //! Get extension mask 61 | void SetExtension(std::string extension) { img_extension = extension; } 62 | 63 | protected: 64 | //! Search path for the pictures or path where they will need to be written 65 | std::string img_path; 66 | 67 | //! Basename for the pictures to be loaded or saved 68 | std::string img_basename; 69 | 70 | //! Mask for the extensions of the pictures to be found or stored 71 | std::string img_extension; 72 | 73 | }; 74 | 75 | #endif /* IMAGESOURCE_H_ */ 76 | -------------------------------------------------------------------------------- /inc/Config.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file Config.h 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 15, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | 25 | #ifndef CONFIG_H_ 26 | #define CONFIG_H_ 27 | 28 | /* ************************************************************************************** 29 | * Configuration options 30 | * **************************************************************************************/ 31 | 32 | //! Adds a lot of extra checks, turn it off for performance (by setting it to 0) 33 | #define CAREFUL_USAGE 0 34 | 35 | /* ************************************************************************************** 36 | * Configuration option implementations 37 | * **************************************************************************************/ 38 | 39 | #if CAREFUL_USAGE == 0 40 | #undef CAREFUL_USAGE 41 | #endif 42 | 43 | #ifdef CAREFUL_USAGE 44 | #define QUIT_ON_ERROR { assert(false); } 45 | #define QUIT_ON_ERROR_VAL(RETURN) { assert(false); } 46 | #else 47 | #define QUIT_ON_ERROR { return; } 48 | #define QUIT_ON_ERROR_VAL(RETURN) { return (RETURN); } 49 | #endif 50 | 51 | #define ASSERT_EQUAL(EXPRESSION_A, EXPRESSION_B) \ 52 | if ((EXPRESSION_A) != (EXPRESSION_B)) \ 53 | std::cerr << __func__ << ": Assert error " << EXPRESSION_A << " != " << EXPRESSION_B << std::endl; \ 54 | assert((EXPRESSION_A) == (EXPRESSION_B)); 55 | 56 | #define ASSERT_TRUE(EXPRESSION, MESSAGE) \ 57 | if (!EXPRESSION) \ 58 | std::cerr << __func__ << ": Assert error " << MESSAGE << std::endl; \ 59 | assert(EXPRESSION); 60 | 61 | /* ************************************************************************************** 62 | * Configuration option includes 63 | * **************************************************************************************/ 64 | 65 | #ifdef CAREFUL_USAGE 66 | #include 67 | #include // definition of NULL 68 | #endif 69 | 70 | 71 | #endif /* CONFIG_H_ */ 72 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ########################################################################################## 2 | # Part of PhD thesis of Anne van Rossum, concerning action-perception loops. This makefile 3 | # is part of a cmake build system. 4 | # The code falls under the Lesser General Public License (LGPL GNU v3). 5 | 6 | # Author: Anne C. van Rossum (Almende B.V.) 7 | # Date: Apr. 16, 2012 8 | # 9 | # Copyright © 2012 Anne van Rossum 10 | ########################################################################################## 11 | 12 | IF(WIN32) 13 | cmake_minimum_required(VERSION 2.6) 14 | ELSE(WIN32) 15 | cmake_minimum_required(VERSION 2.4) 16 | ENDIF(WIN32) 17 | 18 | ########################################################################################## 19 | # The name of the final binary 20 | SET(PROJECT_NAME "ParticleFilter") 21 | 22 | # The name of the path from the parent directory 23 | SET(TESTBENCH_PATH "test") 24 | 25 | ########################################################################################## 26 | 27 | # Set the name 28 | PROJECT(${PROJECT_NAME}) 29 | 30 | # Find packages 31 | FIND_PACKAGE(X11 REQUIRED) 32 | FIND_PACKAGE(Threads REQUIRED) 33 | 34 | # Set include directories and libraries to be linked from the results of the find package macros 35 | IF(X11_FOUND) 36 | INCLUDE_DIRECTORIES(${X11_INCLUDE_DIRS}) 37 | SET(LIBS ${LIBS} ${X11_LIBRARIES}) 38 | ENDIF(X11_FOUND) 39 | 40 | IF(THREADS_FOUND) 41 | SET(LIBS ${LIBS} ${CMAKE_THREAD_LIBS_INIT}) 42 | ENDIF(THREADS_FOUND) 43 | 44 | # Search for source code. 45 | FILE(GLOB folder_source src/*.cpp src/*.cc src/*.c ${TESTBENCH_PATH}/*.c ${TESTBENCH_PATH}/*.cpp) 46 | FILE(GLOB folder_header inc/*.h inc/*.hpp ${TESTBENCH_PATH}/*.h) 47 | 48 | # Some debug information 49 | MESSAGE("[*] ${PROJECT_NAME} is using CXX flags: ${CMAKE_CXX_FLAGS}") 50 | MESSAGE("[*] Libraries to be linked: \"${LIBS}\"") 51 | MESSAGE("[*] Include header files: \"${folder_header}\"") 52 | 53 | # only useful for visual basic or so 54 | #SOURCE_GROUP("Source Files" FILES ${folder_source}) 55 | #SOURCE_GROUP("Header Files" FILES ${folder_header}) 56 | 57 | # Automatically add include directories if needed. 58 | FOREACH(header_file ${folder_header}) 59 | GET_FILENAME_COMPONENT(p ${header_file} PATH) 60 | INCLUDE_DIRECTORIES(${p}) 61 | ENDFOREACH(header_file ${folder_header}) 62 | 63 | # Testing 64 | # enable_testing() 65 | #add_subdirectory(test) 66 | 67 | # Set up our main executable. 68 | IF (folder_source) 69 | ADD_EXECUTABLE(${PROJECT_NAME} ${folder_source} ${folder_header}) 70 | TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${LIBS}) 71 | install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION bin) 72 | ELSE (folder_source) 73 | MESSAGE(FATAL_ERROR "No source code files found. Please add something") 74 | ENDIF (folder_source) 75 | 76 | -------------------------------------------------------------------------------- /test/createImages.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file createTrackImage.h 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 15, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | 25 | #ifndef CREATEIMAGES_H_ 26 | #define CREATEIMAGES_H_ 27 | 28 | #include 29 | #include 30 | 31 | #include // getenv 32 | #include // mkdir 33 | 34 | using namespace std; 35 | using namespace cimg_library; 36 | 37 | typedef cimg_library::CImg ImageType; 38 | 39 | void create_images() { 40 | 41 | string home = string(getenv("HOME")); 42 | if (home.empty()) { 43 | cerr << "Error: no $HOME env. variable" << endl; 44 | return; 45 | } 46 | 47 | string datapath = home + "/mydata"; 48 | string path = home + "/mydata/dotty"; 49 | string basename = "image"; 50 | string extension = ".jpg"; 51 | IpcamImageSource source; 52 | source.SetPath(path); 53 | 54 | struct stat st; 55 | int status; 56 | if (stat(datapath.c_str(), &st) != 0) { 57 | status = mkdir(datapath.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH); 58 | if (status != 0) { 59 | cerr << "Could not create directory " << datapath << endl; 60 | exit (EXIT_FAILURE); 61 | } 62 | } 63 | if (stat(path.c_str(), &st) != 0) { 64 | status = mkdir(path.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH); 65 | if (status != 0) { 66 | cerr << "Could not create directory " << path << endl; 67 | exit (EXIT_FAILURE); 68 | } 69 | } 70 | 71 | if (!source.Update()) { 72 | cerr << "Wrong path?" << endl; 73 | exit (EXIT_FAILURE); 74 | } 75 | 76 | int frame_count = 40; 77 | int frame_id = 0; 78 | while (++frame_id < frame_count) { 79 | ImageType &img = *source.getImage(); 80 | 81 | // stringstream ss; ss.clear(); ss.str(""); 82 | // ss << path << '/' << basename << '_' << frame_id << ".jpeg"; 83 | // img._save_jpeg(NULL, ss.str().c_str(), 100); 84 | 85 | cout << "Save picture: " << frame_id << endl; 86 | } 87 | 88 | } 89 | 90 | #endif /* CREATEIMAGES_H_ */ 91 | -------------------------------------------------------------------------------- /inc/Print.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Some functions to make printing easier 3 | * @file Print.hpp 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 14, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | 25 | #ifndef PRINT_HPP_ 26 | #define PRINT_HPP_ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace dobots { 34 | 35 | /** 36 | * Function to print to standard out. It will accept any container for which an iterator is defined. 37 | * The default separator/delimiter is the comma plus a white space. The default empty vector are square 38 | * brackets. And by default there will be printed an end of line. 39 | * @param first First item of the range to be printed 40 | * @param last Last item 41 | * @param delim A string of symbols separating the items (will not be printed at the end) 42 | * @param empty The string denoting an empty container 43 | * @param endl Printing a new line at the end of the output or not. 44 | */ 45 | template 46 | void print(InputIterator first, InputIterator last, std::string delim=", ", std::string empty="{}", bool endl=true) 47 | { 48 | // concept requirements 49 | __glibcxx_function_requires(_InputIteratorConcept); 50 | __glibcxx_requires_valid_range(first, last); 51 | typedef typename std::iterator_traits::value_type ValueType; 52 | 53 | typedef typename std::iterator_traits::difference_type DistanceType; 54 | 55 | DistanceType dist = std::distance(first, last); 56 | std::cout << '[' << dist << "] "; 57 | switch (dist) { 58 | case 0: 59 | std::cout << empty; 60 | break; 61 | case 1: 62 | std::cout << *first; 63 | break; 64 | default: 65 | std::ostringstream ss; 66 | std::copy(first, last - 1, std::ostream_iterator< ValueType >(ss, delim.c_str())); 67 | ss << *(last - 1); 68 | std::cout << ss.str(); 69 | break; 70 | } 71 | if (endl) std::cout << std::endl; 72 | } 73 | 74 | } 75 | 76 | 77 | #endif /* PRINT_HPP_ */ 78 | -------------------------------------------------------------------------------- /src/Crutchfield.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file Crutchfield.cpp 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 6, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | using namespace std; 30 | 31 | /* ************************************************************************************** 32 | * Implementation of Crutchfield 33 | * **************************************************************************************/ 34 | 35 | Crutchfield::Crutchfield(int bins, int width, int height): Histogram(bins, width, height), 36 | DistanceSource(), 37 | dist(NULL) { 38 | 39 | } 40 | 41 | Crutchfield::~Crutchfield() { 42 | Clear(); 43 | } 44 | 45 | void Crutchfield::Clear() { 46 | if (dist != NULL) { 47 | delete [] dist; 48 | } 49 | dist = NULL; 50 | } 51 | 52 | /** 53 | * Calculates all mutual distances between "sensors". This will fill the "dist" matrix. We 54 | * will loop over all possible sensor pairs and calculate their distance (considering a 55 | * series of images) using the calcDistance function. 56 | * 57 | * The only thing you might to take a look at is the default binning procedure. 58 | */ 59 | void Crutchfield::calcDistances() { 60 | CHECK_DIST; 61 | Clear(); 62 | 63 | cout << "Calculate distances" << endl; 64 | dist = new Value[p_size * p_size]; 65 | for (int p0 = 0; p0 < p_size; p0++) { 66 | for (int p1 = 0; p1 < p_size; p1++) { 67 | dist[p0*p_size+p1] = calcDistance(p0, p1); 68 | } 69 | } 70 | } 71 | 72 | 73 | /** 74 | * Get distance between pixel p0 and pixel p1 given a sequence of images. Every pixel is 75 | * considered a separate sensor. The distance between pixels can be used to decide if a 76 | * camera is mounted on the same side of the organism using only visual input. 77 | * 78 | * d(X,Y) = H(X|Y) + H(Y|X) 79 | * 80 | * Where d is the Crutchfield distance and H is conditional entropy of one information source 81 | * given the other. 82 | */ 83 | Value Crutchfield::calcDistance(int p0, int p1) { 84 | Value dist = getConditionalEntropy(p0, p1) + 85 | getConditionalEntropy(p1, p0); 86 | assert (dist >= 0); 87 | return dist; 88 | } 89 | -------------------------------------------------------------------------------- /inc/File.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file File.hpp 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 15, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | 25 | #ifndef FILE_HPP_ 26 | #define FILE_HPP_ 27 | 28 | #include 29 | #include 30 | 31 | //! The fastest way to implement is to use boost::filesystem::recursive_directory, 32 | //! however, that adds another dependency (boost libraries), so we just use a simple path 33 | //! iterator: http://bit.ly/MYDCno 34 | #include 35 | 36 | #ifdef WARNING 37 | #include 38 | #endif 39 | 40 | #ifdef VERBOSE 41 | #include 42 | #endif 43 | 44 | namespace dobots { 45 | 46 | /** 47 | * Separate function to get the filenames in a given path without using third-party libraries, 48 | * such as boost. 49 | * @param names vector with file names that need to be filled (it will not be emptied) 50 | * @param path path where we need to search for the given filenames 51 | * @param substring substring, e.g. file extension that is used as search mask 52 | * @param at_end boolean indicating that the substring needs to be at the end (e.g. file extension) 53 | * @return success false on non-existing path (for example) 54 | */ 55 | bool getFilenames(std::vector &names, const std::string & path, std::string substring, bool at_end=false) { 56 | DIR *dp; 57 | struct dirent *ep; 58 | dp = opendir(path.c_str()); 59 | if (!dp) { 60 | #ifdef WARNING 61 | std::cerr << "Something wrong with the working path: " << path << std::endl; 62 | #endif 63 | return false; 64 | } 65 | while ((ep = readdir (dp)) != 0) { 66 | std::string file = std::string(ep->d_name); 67 | #ifdef VERBOSE 68 | std::cout << "Found file: " << file << std::endl; 69 | #endif 70 | std::size_t pos = file.rfind(substring); 71 | if (pos == std::string::npos) { 72 | #ifdef VERBOSE 73 | std::cerr << "File " << file << " does not contain substring \"" << substring << "\"" << std::endl; 74 | #endif 75 | continue; 76 | } 77 | if (at_end) { 78 | if ((pos + substring.size()) == file.size()) 79 | names.push_back(file); 80 | } else { 81 | names.push_back(file); 82 | } 83 | } 84 | closedir(dp); 85 | return true; 86 | } 87 | 88 | } 89 | 90 | #endif /* FILE_HPP_ */ 91 | -------------------------------------------------------------------------------- /inc/strstr.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file strstr.h 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Sep 21, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | 25 | #ifndef STRSTR_H_ 26 | #define STRSTR_H_ 27 | 28 | #include 29 | 30 | /* 31 | 32 | int *kmp_borders(const char *needle, size_t nlen){ 33 | if (!needle) return NULL; 34 | int i, j, *borders = (int*)malloc((nlen+1)*sizeof(*borders)); 35 | if (!borders) return NULL; 36 | i = 0; 37 | j = -1; 38 | borders[i] = j; 39 | while((size_t)i < nlen){ 40 | while(j >= 0 && needle[i] != needle[j]){ 41 | j = borders[j]; 42 | } 43 | ++i; 44 | ++j; 45 | borders[i] = j; 46 | } 47 | return borders; 48 | } 49 | 50 | char *kmp_search(char *haystack, size_t haylen, const char *needle, size_t nlen, int *borders){ 51 | size_t max_index = haylen-nlen, i = 0, j = 0; 52 | while(i <= max_index){ 53 | while(j < nlen && *haystack && needle[j] == *haystack){ 54 | ++j; 55 | ++haystack; 56 | } 57 | if (j == nlen){ 58 | return haystack-nlen; 59 | } 60 | if (!(*haystack)){ 61 | return NULL; 62 | } 63 | if (j == 0){ 64 | ++haystack; 65 | ++i; 66 | } else { 67 | do{ 68 | i += j - (size_t)borders[j]; 69 | j = borders[j]; 70 | }while(j > 0 && needle[j] != *haystack); 71 | } 72 | } 73 | return NULL; 74 | } 75 | 76 | char *sstrnstr(char *haystack, const char *needle, size_t haylen){ 77 | if (!haystack || !needle){ 78 | return NULL; 79 | } 80 | size_t nlen = strlen(needle); 81 | if (haylen < nlen){ 82 | return NULL; 83 | } 84 | int *borders = kmp_borders(needle, nlen); 85 | if (!borders){ 86 | return NULL; 87 | } 88 | char *match = kmp_search(haystack, haylen, needle, nlen, borders); 89 | free(borders); 90 | return match; 91 | } 92 | */ 93 | 94 | char *sstrnstr(char *haystack, const char *needle, size_t length) 95 | { 96 | size_t needle_length = strlen(needle); 97 | size_t i; 98 | for(i = 0; i < length; i++) 99 | { 100 | if(i + needle_length > length) 101 | { 102 | return NULL; 103 | } 104 | if(strncmp(&haystack[i], needle, needle_length) == 0) 105 | { 106 | return &haystack[i]; 107 | } 108 | } 109 | return NULL; 110 | } 111 | 112 | 113 | 114 | #endif /* STRSTR_H_ */ 115 | -------------------------------------------------------------------------------- /inc/Crutchfield.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file Crutchfield.h 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 6, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | 25 | #ifndef CRUTCHFIELD_H_ 26 | #define CRUTCHFIELD_H_ 27 | 28 | // General files 29 | #include 30 | #include 31 | 32 | #ifndef CAREFUL_USAGE 33 | #define CHECK_DIST 34 | #else 35 | #define CHECK_DIST { assert (dist != NULL); } 36 | #endif 37 | 38 | /* ************************************************************************************** 39 | * Interface of Crutchfield 40 | * **************************************************************************************/ 41 | 42 | /** 43 | * The Crutchfield distance metric is not so sophisticated anymore, once the probabilities 44 | * and conditional probabilities between sensors is available. 45 | * 46 | * Adds "distance" information considering the joint/conditional probabilities between 47 | * sensor sources. 48 | * 49 | * Provides: 50 | * - calcDistances 51 | * - getDistance for each sensor pair 52 | * 53 | */ 54 | class Crutchfield: public Histogram, public DistanceSource { 55 | public: 56 | //! Constructor Crutchfield 57 | Crutchfield(int bins, int width, int height); 58 | 59 | //! Destructor ~Crutchfield 60 | virtual ~Crutchfield(); 61 | 62 | /** 63 | * Deallocate the internally used distance matrix. 64 | */ 65 | void Clear(); 66 | 67 | /** 68 | * Calculate all distances. It is necessary to call first 69 | * "calcProbabilities(DataFrames & frames)" 70 | * from the parent Histogram class. 71 | * 72 | * This sets all the (conditional) probabilities that are required to do the distance 73 | * calculations. The result is a side-effect in the form of filling a data structure. 74 | */ 75 | void calcDistances(); 76 | 77 | /** 78 | * Calculate the Crutchfield distance between two sensors 79 | */ 80 | Value calcDistance(int p0, int p1); 81 | 82 | /** 83 | * Get previously calculated distance (with calcDistance) 84 | */ 85 | Value getDistance(int p0, int p1) { 86 | CHECK_DIST; 87 | return dist[p0*p_size+p1]; 88 | } 89 | 90 | protected: 91 | 92 | //! Distances matrix (these are actual floats) 93 | Value *dist; 94 | 95 | private: 96 | 97 | 98 | }; 99 | 100 | #endif /* CRUTCHFIELD_H_ */ 101 | -------------------------------------------------------------------------------- /src/FileImageSource.cpp: -------------------------------------------------------------------------------- 1 | ///** 2 | // * @brief 3 | // * @file FileImageSource.cpp 4 | // * 5 | // * This file is created at Almende B.V. It is open-source software and part of the Common 6 | // * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | // * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | // * This software is published under the GNU Lesser General Public license (LGPL). 9 | // * 10 | // * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | // * we personally strongly object to this software being used by the military, in factory 12 | // * farming, for animal experimentation, or anything that violates the Universal 13 | // * Declaration of Human Rights. 14 | // * 15 | // * Copyright © 2012 Anne van Rossum 16 | // * 17 | // * @author Anne C. van Rossum 18 | // * @date Aug 15, 2012 19 | // * @project Replicator FP7 20 | // * @company Almende B.V. 21 | // * @case modular robotics / sensor fusion 22 | // */ 23 | // 24 | //#include 25 | //#include 26 | //#include 27 | //#include 28 | //#include 29 | // 30 | //#include 31 | // 32 | //using namespace std; 33 | //using namespace dobots; 34 | ////using namespace cimg_library; 35 | // 36 | ///* ************************************************************************************** 37 | // * Implementation of FileImageSource 38 | // * **************************************************************************************/ 39 | // 40 | ///** 41 | // * Update, for example if a directory contents has changed. 42 | // */ 43 | //template 44 | //bool FileImageSource::Update() { 45 | // // clear history 46 | // filenames.clear(); 47 | // 48 | // // get all *.jpg files 49 | // string extension = ".jpg"; 50 | // bool success = getFilenames(filenames, path, extension, true); 51 | // if (!success) QUIT_ON_ERROR_VAL(false); 52 | // 53 | // if (filenames.empty()) { 54 | // cerr << "No pictures available!" << endl; 55 | // return false; 56 | // } 57 | // 58 | // // sort in such order that the one with the lowest "postfix" comes first (t1.jpg ... t10.jpg) 59 | // sort(filenames.begin(), filenames.end(), doj::alphanum_less()); 60 | // 61 | // // set pointer to first file 62 | // file_ptr = 0; 63 | // 64 | // // make sure looping through the images allows for "continuous" tracking by adding the entire 65 | // // sequence in reverse [0, 1, 2, 3] becomes [0, 1, 2, 3, 2, 1] upon which can be looped 66 | // if (copy_reverse_series) { 67 | // size_t cnt = filenames.size(); 68 | // if (cnt > 2) { 69 | // for (size_t i = cnt-2; i > 0; --i) { 70 | // filenames.push_back(filenames[i]); 71 | // } 72 | // assert (filenames.size() == (cnt-1)*2); 73 | // } 74 | // } 75 | // 76 | // return success; 77 | //} 78 | // 79 | //template 80 | //std::string FileImageSource::nextFile() { 81 | // if (file_ptr < 0) { 82 | // std::cerr << "Call to update() was probably not successful" << std::endl; 83 | // QUIT_ON_ERROR_VAL(""); 84 | // } 85 | // assert (file_ptr < filenames.size()); 86 | // std::string file = filenames[file_ptr]; 87 | // file_ptr = (file_ptr + 1) % filenames.size(); 88 | // std::cout << "Open file " << file << std::endl; 89 | // return file; 90 | //} 91 | -------------------------------------------------------------------------------- /test/testImageHistogram.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file testImageHistogram.h 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 15, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | 25 | #ifndef TESTIMAGEHISTOGRAM_H_ 26 | #define TESTIMAGEHISTOGRAM_H_ 27 | 28 | #include 29 | #include 30 | 31 | using namespace cimg_library; 32 | using namespace std; 33 | 34 | /* ************************************************************************************** 35 | * Interface of testImageHistogram 36 | * **************************************************************************************/ 37 | 38 | void test_image_histogram() { 39 | 40 | PositionParticleFilter pf; 41 | ImageSource source; 42 | 43 | string path = "/home/anne/mydata/active_wheel_camera"; 44 | string extension = ".jpg"; 45 | 46 | source.SetPath(path); 47 | source.SetExtension(extension); 48 | source.Update(); 49 | 50 | CImg & img = *source.getImage(); 51 | 52 | CImgDisplay main_disp(img, "Show image"); 53 | 54 | while (!main_disp.is_closed() && !main_disp.is_keyESC()) { 55 | main_disp.wait(); 56 | if (main_disp.button() && main_disp.mouse_y()>=0) { 57 | const int y = main_disp.mouse_y(); 58 | CImg img_coords = img.get_select(main_disp,2,0); 59 | CImg img_selection = img.get_crop(img_coords(0),img_coords(1),img_coords(3),img_coords(4)); 60 | 61 | cout << "Coordinates: ["; 62 | cout << img_coords(0) << ',' << img_coords(1) << ',' << img_coords(3) << ',' << img_coords(4); 63 | cout << ']' << endl; 64 | 65 | int width = img_coords(3) - img_coords(0); 66 | int height = img_coords(4) - img_coords(1); 67 | int bins = 16; 68 | 69 | cout << "Create histogram with " << bins << " bins" << endl; 70 | Histogram histogram(bins, width, height); 71 | DataFrames frames; 72 | pDataMatrix data = img_selection._data; 73 | frames.push_back(data); 74 | 75 | cout << "Add data for histograms" << endl; 76 | histogram.calcProbabilities(frames); 77 | 78 | NormalizedHistogramValues result; 79 | histogram.getProbabilities(result); 80 | 81 | cout << "Result: "; 82 | Value sum = 0; 83 | for (int i = 0; i < result.size(); ++i) { 84 | sum += result[i]; 85 | cout << result[i] << ' '; 86 | } 87 | cout << " with sum = " << sum << endl; 88 | 89 | // Also create histogram from the image using CImg functionality 90 | // in case [0,2556] are not given as boundaries, these boundaries are dynamically set! 91 | const CImg img_hist = img_selection.histogram(16, 0, 256); 92 | img_hist.display_graph(0,3); 93 | } 94 | } 95 | 96 | delete &img; 97 | } 98 | 99 | #endif /* TESTIMAGEHISTOGRAM_H_ */ 100 | -------------------------------------------------------------------------------- /test/createTrackImage.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file createTrackImage.h 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 15, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | 25 | #ifndef CREATETRACKIMAGE_H_ 26 | #define CREATETRACKIMAGE_H_ 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | #include // getenv 33 | 34 | using namespace std; 35 | using namespace cimg_library; 36 | 37 | void create_track_image() { 38 | 39 | string home = string(getenv("HOME")); 40 | if (home.empty()) { 41 | cerr << "Error: no $HOME env. variable" << endl; 42 | return; 43 | } 44 | 45 | string path = home + "/mydata/dotty"; 46 | string basename = "image1"; 47 | string extension = ".jpg"; 48 | 49 | string file = path + '/' + basename + extension; 50 | 51 | 52 | CImg img(file.c_str()); 53 | 54 | CImgDisplay main_disp(img, "Click a point"); 55 | 56 | srand(time(NULL)); 57 | 58 | while (!main_disp.is_closed() && !main_disp.is_keyESC()) { 59 | main_disp.wait(); 60 | if (main_disp.button() && main_disp.mouse_y()>=0) { 61 | const int y = main_disp.mouse_y(); 62 | // img.draw_rectangle; 63 | // img.draw_rectangle(0,y0,img.width()-1,y0+12,red); 64 | // from http://www.codeproject.com/answers/55965/How-to-select-an-area-from-displayed-image-using-C.aspx#answer2 65 | CImg img_coords = img.get_select(main_disp,2,0); 66 | CImg img_selection = img.get_crop(img_coords(0),img_coords(1),img_coords(3),img_coords(4)); 67 | 68 | cout << "Coordinates: ["; 69 | cout << img_coords(0) << ',' << img_coords(1) << ',' << img_coords(3) << ',' << img_coords(4); 70 | cout << ']' << endl; 71 | 72 | // show the selection 73 | CImgDisplay disp2(img_selection, "Cropped image selection"); 74 | while(!disp2.is_closed() && !disp2.is_keyESC()) { 75 | disp2.wait(); 76 | 77 | if (disp2.is_keyS()) { 78 | int random_id = rand(); 79 | cout << "Save selection with random id: " << random_id << endl; 80 | stringstream ss; ss.clear(); ss.str(""); 81 | // on purpose .jpeg instead of .jpg, makes it easy to distinguish source images from selected target images 82 | ss << path << "/target_" << basename << '_' << random_id << ".jpeg"; 83 | img_selection._save_jpeg(NULL, ss.str().c_str(), 100); 84 | 85 | // also store coordinates! 86 | ss.clear(); ss.str(""); 87 | ss << path << "/target_" << basename << '_' << random_id << ".ini"; 88 | string config_filename = ss.str(); 89 | cout << "Config file: " << config_filename << endl; 90 | std::ofstream out( config_filename.c_str() ); 91 | if (!out) { 92 | cerr << "Cannot create config file" << endl; 93 | } else { 94 | try { 95 | ConfigFile configfile(config_filename); 96 | configfile.add("coord0", img_coords(0)); 97 | configfile.add("coord1", img_coords(1)); 98 | configfile.add("coord3", img_coords(3)); 99 | configfile.add("coord4", img_coords(4)); 100 | out << configfile; 101 | } catch (const ConfigFile::file_not_found &e) { 102 | cerr << "Cannot find file: " << config_filename << endl; 103 | } 104 | out.close(); 105 | } 106 | } 107 | } 108 | } 109 | } 110 | 111 | } 112 | 113 | #endif /* CREATETRACKIMAGE_H_ */ 114 | -------------------------------------------------------------------------------- /test/testDistance.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file testDistance.h 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 21, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | 25 | #ifndef TESTDISTANCE_H_ 26 | #define TESTDISTANCE_H_ 27 | 28 | #include 29 | 30 | #include 31 | 32 | using namespace std; 33 | using namespace dobots; 34 | 35 | #define TESTVALUE double 36 | #define TESTPOINT vector 37 | #define TESTSET set 38 | 39 | #define TESTPOINT_DEF std::TESTPOINT < TESTVALUE > 40 | #define TESTSET_DEF std::TESTSET 41 | 42 | #define TESTPOINT_ITER TESTPOINT_DEF::iterator 43 | #define TESTSET_ITER TESTSET_DEF::iterator 44 | 45 | void test_distance() { 46 | cout << " === start test distance === " << endl; 47 | 48 | TESTSET_DEF set0; 49 | TESTPOINT_DEF *p = new TESTPOINT_DEF(); 50 | 51 | p->push_back(1); 52 | 53 | TESTPOINT_DEF *p0 = new TESTPOINT_DEF(); 54 | TESTPOINT_DEF *p1 = new TESTPOINT_DEF(); 55 | p0->clear(); 56 | p1->clear(); 57 | p0->push_back(3); 58 | p1->push_back(6); 59 | set0.clear(); 60 | set0.insert(p0); 61 | set0.insert(p1); 62 | 63 | SetDistanceMetric set_metric = SDM_INFIMIM; 64 | DistanceMetric point_metric = DM_EUCLIDEAN; 65 | TESTVALUE result; 66 | result = dobots::distance_to_point(set0.begin(), set0.end(), p->begin(), p->end(), set_metric, point_metric); 67 | 68 | cout << "Inf result d(1,[3,6]) = " << result << " and should be 2 (minimum distance is between 1 and 3)" << endl; 69 | assert (result == 2); 70 | 71 | TESTPOINT_DEF *p2 = new TESTPOINT_DEF(); 72 | TESTPOINT_DEF *p3 = new TESTPOINT_DEF(); 73 | TESTPOINT_DEF *p4 = new TESTPOINT_DEF(); 74 | TESTPOINT_DEF *p5 = new TESTPOINT_DEF(); 75 | TESTSET_DEF set1; 76 | set0.clear(); 77 | set1.clear(); 78 | p0->clear(); 79 | p1->clear(); 80 | p2->clear(); 81 | p3->clear(); 82 | p4->clear(); 83 | p5->clear(); 84 | p0->push_back(1); 85 | p1->push_back(3); 86 | p2->push_back(6); 87 | p3->push_back(7); 88 | set0.insert(p0); set0.insert(p1); set0.insert(p2); set0.insert(p3); 89 | p4->push_back(3); 90 | p5->push_back(6); 91 | set1.insert(p4); 92 | set1.insert(p5); 93 | 94 | set_metric = SDM_SUPINF; 95 | result = dobots::distance_to_set(set0.begin(), set0.end(), set1.begin(), set1.end(), set_metric, point_metric); 96 | 97 | cout << "SupInf result d([1,3,6,7], [3,6]) = " << result << " and should be 2 :::: "; 98 | cout << "d(1,[3,6])=2, d(3,[3,6])=0, d(6,[3,6])=0, d(7,[3,6])=1 (maximum value here is 2)" << endl; 99 | assert (result == 2); 100 | 101 | result = dobots::distance_to_set(set1.begin(), set1.end(), set0.begin(), set0.end(), set_metric, point_metric); 102 | 103 | cout << "SupInf result d([3,6], [1,3,6,7]) = " << result << " and should be 0 :::: "; 104 | cout << "d(3,[1,3,6,7])=0, d(6,[1,3,6,7])=0 (maximum value here is 0)" << endl; 105 | assert (result == 0); 106 | 107 | set_metric = SDM_HAUSDORFF; 108 | result = dobots::distance_to_set(set1.begin(), set1.end(), set0.begin(), set0.end(), set_metric, point_metric); 109 | 110 | cout << "Hausdorff result d([3,6], [1,3,6,7]) = " << result << " and should be 2 again (maximum of above)" << endl; 111 | assert (result == 2); 112 | 113 | 114 | delete p; 115 | delete p0; 116 | delete p1; 117 | delete p2; 118 | delete p3; 119 | delete p4; 120 | delete p5; 121 | set0.clear(); 122 | set1.clear(); 123 | 124 | cout << " === end test distance metrics === " << endl; 125 | } 126 | 127 | 128 | #endif /* TESTDISTANCE_H_ */ 129 | -------------------------------------------------------------------------------- /inc/chunkbuffer.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file chunkbuffer.hpp 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Sep 21, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | 25 | #ifndef CHUNKBUFFER_HPP_ 26 | #define CHUNKBUFFER_HPP_ 27 | 28 | // General files 29 | #include 30 | 31 | template 32 | struct chunk { 33 | T *start; 34 | int size; 35 | }; 36 | 37 | /* ************************************************************************************** 38 | * Interface of chunkbuffer 39 | * **************************************************************************************/ 40 | 41 | /** 42 | * In many situations it seems you will need a ringbuffer. However, the situation is such 43 | * that you know more or less how big the items are you will need to retrieve from a 44 | * continuous stream of data. And you have plenty of space available, at least enough to 45 | * store say 20 of these items. The data comes continuous, but in chunks. In that case 46 | * it is convenient to use this chunkbuffer. You can add chunks of data, and items are 47 | * constructed out of these chunks. 48 | */ 49 | template 50 | class chunkbuffer { 51 | public: 52 | //! Constructor chunkbuffer 53 | chunkbuffer(): last_item_begin(buffer), last_chunk_end(buffer) { 54 | } 55 | 56 | //! Destructor ~chunkbuffer 57 | virtual ~chunkbuffer() { 58 | } 59 | 60 | /** 61 | * Add a chunk to the buffer. If there is not enough space, move all chunks that 62 | * belong to the last item to the beginning. 63 | */ 64 | void addchunk(chunk c) { 65 | if (c.size >= remain_to_end()) { 66 | move_to_begin(); 67 | } 68 | memcpy(last_chunk_end, c.start, c.size); 69 | last_chunk_end += c.size; 70 | } 71 | 72 | /** 73 | * The start of the next item is not the same as the "last_chunk_end". It can be 74 | * halfway the last chunk. This is not set in "item_received" because that would 75 | * screw up the "last_item_begin" of the item that has yet to be received. 76 | */ 77 | void next_item(uint32_t skip) { 78 | assert (last_item_begin + skip <= last_chunk_end); 79 | last_item_begin += skip; 80 | } 81 | 82 | //! Check item for errors 83 | virtual bool check_item_errors() = 0; 84 | 85 | //! Get the size of the item (should be total of header_size + content_size) 86 | virtual bool get_item_size(uint32_t & header_size, uint32_t & content_size) = 0; 87 | 88 | //! Returns true if item has been received (will probably call get_item_size) 89 | virtual bool item_received(int & item_size) = 0; 90 | 91 | /** 92 | * Physically copy the chunks belonging to the last item to the beginning of the 93 | * buffer. The rest of the buffer will be overwritten. 94 | */ 95 | void move_to_begin() { 96 | uint32_t already_there = last_chunk_end - last_item_begin; 97 | cout << __func__ << ": move all last chunks to beginning (size=" << already_there << ")" << endl; 98 | memcpy(buffer, last_item_begin, already_there); 99 | last_item_begin = buffer; 100 | last_chunk_end = buffer+already_there; 101 | } 102 | 103 | /** 104 | * Remaining size of the buffer. 105 | */ 106 | inline uint32_t remain_to_end() { 107 | uint32_t r = size - (last_chunk_end - buffer); 108 | cout << __func__ << ": space = " << (int)r << " (should be < " << size << ")" << endl; 109 | return r; 110 | } 111 | 112 | inline void reset() { 113 | last_item_begin = last_chunk_end = buffer; 114 | } 115 | 116 | /** 117 | * Current item size. 118 | */ 119 | inline uint32_t current_item_size() { 120 | uint32_t s = last_chunk_end - last_item_begin; 121 | cout << __func__ << ": size = " << (int)s << endl; 122 | return s; 123 | } 124 | 125 | protected: 126 | 127 | T buffer[size]; 128 | 129 | T* last_item_begin; 130 | 131 | T* last_chunk_end; 132 | 133 | private: 134 | }; 135 | 136 | #endif /* CHUNKBUFFER_HPP_ */ 137 | -------------------------------------------------------------------------------- /inc/FileImageSource.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Image source which gets images from files 3 | * @file FileImageSource.h 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 15, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | 25 | #ifndef FILEIMAGESOURCE_H_ 26 | #define FILEIMAGESOURCE_H_ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | 40 | #include 41 | 42 | /* ************************************************************************************** 43 | * Interface of FileImageSource 44 | * **************************************************************************************/ 45 | 46 | template 47 | class FileImageSource: public ImageSource { 48 | public: 49 | //! Constructor FileImageSource 50 | FileImageSource(): file_ptr(-1), copy_reverse_series(true) { 51 | filenames.clear(); 52 | } 53 | 54 | //! Destructor ~FileImageSource 55 | virtual ~FileImageSource() {} 56 | 57 | //! Perform functionality that is required to get images 58 | bool Update() { 59 | assert(!this->img_path.empty()); 60 | 61 | // clear history 62 | filenames.clear(); 63 | 64 | // get all *.jpg files 65 | string extension = ".jpg"; 66 | bool success = dobots::getFilenames(filenames, this->img_path, this->img_extension, true); 67 | if (!success) QUIT_ON_ERROR_VAL(false); 68 | 69 | if (filenames.empty()) { 70 | cerr << "No pictures available!" << endl; 71 | return false; 72 | } 73 | 74 | // sort in such order that the one with the lowest "postfix" comes first (t1.jpg ... t10.jpg) 75 | sort(filenames.begin(), filenames.end(), doj::alphanum_less()); 76 | 77 | // set pointer to first file 78 | file_ptr = 0; 79 | 80 | // make sure looping through the images allows for "continuous" tracking by adding the entire 81 | // sequence in reverse [0, 1, 2, 3] becomes [0, 1, 2, 3, 2, 1] upon which can be looped 82 | if (copy_reverse_series) { 83 | size_t cnt = filenames.size(); 84 | if (cnt > 2) { 85 | for (size_t i = cnt-2; i > 0; --i) { 86 | filenames.push_back(filenames[i]); 87 | } 88 | assert (filenames.size() == (cnt-1)*2); 89 | } 90 | } 91 | 92 | return success; 93 | } 94 | 95 | /** 96 | * Get an image (the next image if there are multiple). 97 | */ 98 | Image* getImage() { 99 | return getImage(nextFile()); 100 | } 101 | 102 | /** 103 | * Get a specific image with given name, should reside in previously set path. 104 | * Assumes that there is a constructor that accepts a filename and returns an Image object. 105 | */ 106 | Image* getImage(std::string file) { 107 | file = this->img_path + '/' + file; 108 | Image *img = new Image(file.c_str()); 109 | return img; 110 | } 111 | 112 | /** 113 | * Gets shifted image. Assumes that there is a shift operator defined on Image! 114 | */ 115 | Image* getImageShifted(int shift_x, int shift_y) { 116 | assert (!filenames.empty()); 117 | Image *img = getImage(filenames.front()); 118 | img->shift(shift_x, shift_y, 0, 0, 2); 119 | return img; 120 | } 121 | 122 | protected: 123 | //! Return next file from the previously build up vector with image filenames 124 | std::string nextFile() { 125 | if (file_ptr < 0) { 126 | std::cerr << "Call to update() was probably not successful" << std::endl; 127 | QUIT_ON_ERROR_VAL(""); 128 | } 129 | assert (file_ptr < filenames.size()); 130 | std::string file = filenames[file_ptr]; 131 | file_ptr = (file_ptr + 1) % filenames.size(); 132 | std::cout << "Open file " << file << std::endl; 133 | return file; 134 | } 135 | private: 136 | //! All files with pictures (does not contain path) 137 | std::vector filenames; 138 | 139 | //! Pointer to current file 140 | int file_ptr; 141 | 142 | //! Use the entire series in reverse (convenient for tracking) 143 | bool copy_reverse_series; 144 | }; 145 | 146 | #endif /* FILEIMAGESOURCE_H_ */ 147 | -------------------------------------------------------------------------------- /inc/Histogram.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Crutchfield.h 3 | * @brief 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object against this software used by the military, in the 12 | * bio-industry, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 6, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case 22 | */ 23 | #ifndef HISTOGRAM_H_ 24 | #define HISTOGRAM_H_ 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | //! Container for histogram values 31 | typedef std::vector HistogramValues; 32 | 33 | //! Container for normalised histogram values 34 | typedef std::vector NormalizedHistogramValues; 35 | 36 | /* ************************************************************************************** 37 | * Interface of Histogram 38 | * **************************************************************************************/ 39 | 40 | /** 41 | * This class calculates Histogram from data provided in the form of "matrices". It calculates 42 | * frequencies, and not probabilities directly. However, the latter can be easily obtained by 43 | * properly normalising the frequencies. For that the number of frames is used. 44 | * 45 | * Usage: 46 | * calcProbabilities 47 | * Do not call the getX functions before the calcX functions. 48 | */ 49 | class Histogram: public ProbMatrix { 50 | public: 51 | //! Constructor with number of bins and the size of the frames 52 | Histogram(int bins, int width, int height); 53 | 54 | //! Destructor 55 | virtual ~Histogram(); 56 | 57 | /** 58 | * Deallocate structures, used in destructor and on calcProbabilities. 59 | */ 60 | void Clear(); 61 | 62 | /** 63 | * Calculate probabilities of value given a series of frames with (different) values at that 64 | * same position in the frame. This is enough to define a histogram over a data frame. This 65 | * will calculate the probabilities of occurrence of individual values in a certain bin, but 66 | * also the joint probabilities with respect to all other values. So, this will generate 67 | * internally two data structures, one of size of the data (D), and one quadratic with 68 | * respect to that (DxD). 69 | * 70 | * The results are in the form of "side effects" to this function. Internal data structures 71 | * are filled. To get the calculated entities, use for example: 72 | * - getProbability 73 | * - getJointProbability 74 | * - getConditionalEntropy 75 | */ 76 | void calcProbabilities(DataFrames & frames); 77 | 78 | /** 79 | * The conditional entropy can be obtained after calcProbability. It does not actually 80 | * calculate the entropy itself, you will need to call calcProbabilities before! 81 | */ 82 | Value getConditionalEntropy(int p0, int p1); 83 | 84 | /** 85 | * Calculation that calculates the bin index given a specific value. Currently very simple 86 | * just uniformly dividing bins over the entire expected range from [0,255] of values. 87 | */ 88 | inline int value2bin(Value v) { 89 | assert (v >= 0); 90 | int bin = (v * bins) / 256; 91 | assert (bin < bins); 92 | assert (bin < 256); 93 | return bin; 94 | } 95 | 96 | /** 97 | * The other (default) function returns the frequency of data events of an individual sensor over 98 | * time. A value is counted several times to fall into bin X over a set of frames, and then 99 | * counted a few times to fall into bin Y over another set of frames. 100 | * 101 | * Contrary to that, this function does forget about individual sensors and returns probability 102 | * measures over all sensors (and over time). 103 | */ 104 | void getFrequencies(HistogramValues &bin_result); 105 | 106 | //! Get total number of samples that are recorded 107 | int getSamples(); 108 | 109 | /** 110 | * Exactly the same as getFrequencies, but now normalised with the sum of all events. 111 | */ 112 | void getProbabilities(NormalizedHistogramValues &bin_result); 113 | #ifdef DEBUG 114 | //! Print distances 115 | void printDistances(); 116 | 117 | //! Print bin values 118 | void printBins(pDataMatrix data); 119 | 120 | void printFrequencies(int bin); 121 | 122 | void printJointFrequencies(char printmode = 1); 123 | 124 | void printJointFrequencies(int bin0, int bin1); 125 | 126 | pDataMatrix drawDistances(); 127 | protected: 128 | void printJointFrequenciesForPixels(int p0, int p1); 129 | 130 | #endif 131 | }; 132 | 133 | #endif /* HISTOGRAM_H_ */ 134 | -------------------------------------------------------------------------------- /inc/Autoregression.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file Autoregression.hpp 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 8, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | 25 | #ifndef AUTOREGRESSION_HPP_ 26 | #define AUTOREGRESSION_HPP_ 27 | 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | namespace dobots { 34 | 35 | enum RotateDirection { 36 | RD_LEFT, 37 | RD_RIGHT, 38 | RD_COUNT 39 | }; 40 | 41 | /** 42 | * The seed used for autoregression. Know your seeds, it makes it easier to repeat experiments. 43 | */ 44 | static int autoregression_seed = 334340; 45 | 46 | /** 47 | * Predict the next value using auto-regression (AR). 48 | * See also: http://en.wikipedia.org/wiki/Autoregressive_model which describes an autoregressive 49 | * model also as an all-pole infinite impulse response filter with some additional interpretation. 50 | * There are no constraints enforced on the coefficients, so the model is not guaranteed to be 51 | * stationary. For example, for AR(1) the coefficient should be: |\phi| < 1 52 | * The size of the autoregression is derived from the size of the coefficient container. For now 53 | * it is expected that the data container is of the same size. 54 | * @param container input values x[t-1], x[t-2], x[t-3], ... 55 | * @param coefficient parameters of the autoregressive model phi[i], phi[2], phi[3], ... 56 | * @param variance (optional) an AR progress has white noise with variance \sigma^2. 57 | * @param constant (optional) a constant as in the definition 58 | * @return next value x[t] 59 | */ 60 | template 61 | inline T predict(InputIterator1 first1, InputIterator1 last1, 62 | InputIterator2 first2, T constant, T variance = T(1)) { 63 | __glibcxx_function_requires(_InputIteratorConcept); 64 | __glibcxx_function_requires(_InputIteratorConcept); 65 | __glibcxx_requires_valid_range(first1, last1); 66 | 67 | // assert (container.size() == coefficients.size()); 68 | static boost::mt19937 random_number_generator(autoregression_seed); 69 | boost::normal_distribution<> normal_dist(0.0, variance); 70 | boost::variate_generator > epsilon( 71 | random_number_generator, normal_dist); 72 | 73 | T sum = std::inner_product(first1, last1, first2, T(0)); 74 | T prediction = constant + sum + epsilon(); 75 | #ifdef VERBOSE 76 | std::cout << __func__ << ": result is " << prediction << std::endl; 77 | #endif 78 | return prediction; 79 | } 80 | 81 | /** 82 | * A vector is not the best format to implement a circular buffer. However, sometimes a 83 | * vector is required for other purposes and then an "advance" method is interesting, it 84 | * will move all elements in the vector to the front and put the front at the back. 85 | */ 86 | template 87 | void rotate(InputIterator first, InputIterator last, RotateDirection direction = RD_RIGHT) { 88 | __glibcxx_function_requires(_InputIteratorConcept); 89 | __glibcxx_requires_valid_range(first, last); 90 | if (last - first == 1) return; 91 | switch (direction) { 92 | case RD_LEFT: 93 | std::rotate(first, first+1, last); 94 | break; 95 | case RD_RIGHT: default: 96 | std::rotate(first, last-1, last); 97 | break; 98 | } 99 | } 100 | 101 | /** 102 | * Pushes an item upon a container and pops off the oldest value. In case the container is build 103 | * up out of pointers to objects, make sure you delete the item yourself. 104 | * 105 | * @param first iterator to the beginning of the container 106 | * @param last iterator to the end of the container 107 | * @param item the item to be added to the container 108 | * @param direction the direction in which to push and pop (left or right) 109 | */ 110 | template 111 | void pushpop(InputIterator first, InputIterator last, T item, RotateDirection direction = RD_RIGHT) { 112 | __glibcxx_function_requires(_InputIteratorConcept); 113 | __glibcxx_requires_valid_range(first, last); 114 | dobots::rotate(first, last, direction); 115 | // overwrite first item 116 | *first = item; 117 | } 118 | 119 | 120 | 121 | } 122 | 123 | #endif /* AUTOREGRESSION_HPP_ */ 124 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | # ParticleFilter 4 | A particle filter can be used for tracking: 5 | 6 | - to track an object in a sequence of camera images. 7 | - to track the position of a robot given inaccurate position and movement estimates. 8 | 9 | ## What does it do? 10 | This software is actually just one file, [ParticleFilter.hpp](https://github.com/mrquincle/particlefilter/blob/master/inc/ParticleFilter.hpp), which contains only one important function, namely "Resample". Every particle stands for an estimate of the actual world or robot state. Each particle can be better or worse representing the world state and this is indicated by one value, its "weight". The weight is determined by an observation model (which is not part of the particle filter itself, but depends on the application). The "Resample" function does basically three things: 11 | 12 | - normalizing the particle weights so they sum up to one 13 | - sort the particles according to their weights 14 | - make copies of a particle with the number of copies depending on the weight of the particle (favoring obesity ;-) ) 15 | 16 | Now, we have a new set of particles and a so-called transition model moves each of these particles according to a predefined model by the user. Due for example to our limited physical capabilities human movements would follow some autoregressive model. An example is given by the [PositionParticleFilter.h](https://github.com/mrquincle/particlefilter/blob/master/inc/PositionParticleFilter.h) that does implement an observation and a transition model. 17 | 18 | ## Is it good? 19 | Current phase: just out of alpha. And not with respect to the [series](http://en.wikipedia.org/wiki/Alphas) but with respect to the fact that all functionality is there, however everything needs to be debugged. 20 | 21 | ## What are the alternatives? 22 | There are many alternatives for tracking, done here by particle filtering, ranging from Kalman filters to (other types of) Bayes filters. Then there are many forms of particle filters, for example the boosted particle filter, the unscented particle filter, the cascade particle filter, the kernel particle filter, the bootstrap particle filter and the mean-shift embedded particle filter. And last, but not least, there are software alternatives, such as the particle filter implementation in the [Mobile Robot Programming Toolkit](http://www.mrpt.org/Particle_Filters). 23 | 24 | ## An example 25 | An example of the particle filter being able to track the docking element of a Replicator robot will follow soon. The following pictures shows the match of all pixels with the reference histogram. 26 | 27 | ![picture](https://raw.github.com/mrquincle/particlefilter/master/doc/track_robot.jpg) 28 | 29 | ## Interesting 30 | Maybe you find convenient or interesting some of the helper files that have been written for the particle filter. 31 | 32 | 39 | 40 | * [Container.hpp](https://github.com/mrquincle/particlefilter/blob/master/inc/Container.hpp) which contains distance functions (Euclidean, Battacharyya, Hellinger, Manhattan, Chebyshev) for standard C++ containers. For example the Hellinger distance is ![equation](http://latex.codecogs.com/gif.latex?d%28x%2Cy%29%3D1%2F%5Csqrt%7B2%7D*%5Csqrt%7B%5Csum_%7Bi%3D1%7D%5Ek%28%5Csqrt%7Bx_i%7D-%5Csqrt%7By_i%7D%29%5E2%7D). 41 | * [Autoregression.hpp](https://github.com/mrquincle/particlefilter/blob/master/inc/Autoregression.hpp) with three nice utility template functions, one of them does calculate the actual autoregression, the others rotate or perform an automic "push-pop" operation. The latter is convenient if your data container does not happen to be a deque, but for example a vector. 42 | * [Print.hpp](https://github.com/mrquincle/particlefilter/blob/master/inc/Print.hpp) in case you print comma-separated data containers content all the time. 43 | * [File.hpp](https://github.com/mrquincle/particlefilter/blob/master/inc/File.hpp) get files from a directory without any dependencies (such as boost). 44 | 45 | Except for the CImg template library, there have been two files used for demonstrating the particle filter: 46 | 47 | * [ConfigFile.hpp](https://github.com/mrquincle/particlefilter/blob/master/inc/Container.hpp) which is written by Richard Wagner and makes it extremely easy to quickly write back and forth to configuration files. 48 | * [alphanum.hpp](https://github.com/mrquincle/particlefilter/blob/master/inc/alphanum.hpp) written by Dirk Jagdmann. 49 | 50 | ## Where can I read more? 51 | * [Wikipedia](http://en.wikipedia.org/wiki/Particle_Filter) 52 | 53 | ## Copyrights 54 | The copyrights (2012) belong to: 55 | 56 | - License: LGPL v.3 57 | - Author: Anne van Rossum 58 | - Almende B.V., http://www.almende.com and DO bots B.V., http://www.dobots.nl 59 | - Rotterdam, The Netherlands 60 | 61 | Note, ideas on using which algorithms (for example the specific autoregressive model) are inspired by [Rob Hess](http://blogs.oregonstate.edu/hess/code/particles/) for tracking a football player in a sequence of video frames. However, none of Rob's code has been used. This is an independent implementation. 62 | -------------------------------------------------------------------------------- /inc/imgbuffer.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file imgbuffer.hpp 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Sep 21, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | 25 | #ifndef IMGBUFFER_HPP_ 26 | #define IMGBUFFER_HPP_ 27 | 28 | // General files 29 | #include 30 | 31 | #include 32 | 33 | using namespace std; 34 | 35 | #define CHUNKSIZE 1460 36 | 37 | /* ************************************************************************************** 38 | * Interface of imgbuffer 39 | * **************************************************************************************/ 40 | 41 | #define CHUNK_BUFFER_SIZE (1024*1024) 42 | 43 | class imgbuffer: public chunkbuffer { 44 | public: 45 | //! Constructor imgbuffer 46 | imgbuffer(): frame_number(0), debug(5) {} 47 | 48 | //! Destructor ~imgbuffer 49 | virtual ~imgbuffer() {} 50 | 51 | /** 52 | * Check item content for errors 53 | */ 54 | bool check_item_errors() { 55 | // the DCS-900 sends this spelling error in my firmware if it does not understand nof_bytes_read packet. 56 | if( sstrnstr(last_item_begin, "unknwon", current_item_size()) ) { 57 | if (debug) 58 | fprintf(stderr, "DCS-900 DETECTED UNKNOWN DATA, NETWORK PROBLEM?\n"); 59 | return true; 60 | } 61 | return false; 62 | } 63 | 64 | /** 65 | * Check from the start of the block and returns the size of the item. There are two interpretations of the 66 | * size of the item. The "content size" is a lower bound on this and denotes for example the size of an image 67 | * without the HTTP header. The "header size" is the header, so the total size of the thing in the stream 68 | * including all kind of header info is "header_size" + "content_size". 69 | */ 70 | bool get_item_size(uint32_t & header_size, uint32_t & content_size) { 71 | char *ptr; 72 | int content_length; 73 | uint32_t size = current_item_size(); 74 | 75 | //! search the string "image/jpeg" in the buffer 76 | ptr = sstrnstr(last_item_begin, "image", size); 77 | if(!ptr) { 78 | cerr << __func__ << ": could not find \"image/jpeg\" in chunk of size " << size << endl; 79 | if (size > 45000) exit(-1); 80 | return false; 81 | } 82 | if (debug) 83 | cout << __func__ << ": found Content-type: image/jpeg" << endl; 84 | 85 | // jump over string "image/jpeg" 86 | ptr = sstrnstr(last_item_begin, "Content-length: ", size); 87 | if(!ptr) { 88 | fprintf(stderr, "could not find \"Content-length: \", looping for more\n"); 89 | return false; 90 | } 91 | if (debug) 92 | cout << __func__ << ": found Content-length" << endl; 93 | 94 | // read content length 95 | sscanf(ptr + 16, "%d", &content_length); 96 | cout << __func__ << ": content length = " << content_length << endl; 97 | 98 | header_size = 0; 99 | char *p; 100 | for (p = last_item_begin; p < last_item_begin+size; ++p) { 101 | if (((unsigned char)*p == 255) && ((unsigned char)*(p+1) == 216)) { //'0xFF' and '0xD8' 102 | header_size = p - last_item_begin; 103 | cout << __func__ << ": header size is " << header_size << endl; 104 | break; 105 | } 106 | } 107 | if (header_size == 0) return false; 108 | 109 | content_size = content_length; 110 | return true; 111 | } 112 | 113 | /** 114 | * Read bytes from socket and put them in the ringbuffer 115 | */ 116 | int read_from_socket(int socketfd) { 117 | int nof_bytes_read = 0; 118 | nof_bytes_read = read(socketfd, cbuffer, CHUNKSIZE); 119 | if(nof_bytes_read > 0) { 120 | chunk c; 121 | c.start = cbuffer; 122 | c.size = nof_bytes_read; 123 | addchunk(c); 124 | if (debug) 125 | fprintf(stderr, "read() returned %d bytes\n", nof_bytes_read); 126 | return nof_bytes_read; 127 | } 128 | fprintf(stderr, "mcamip: read(): returned EOF (power failure, network, interference?)\n"); 129 | 130 | return nof_bytes_read; 131 | } 132 | 133 | /** 134 | * Returns true if item is received and returns item size too (with respect to last_item_begin). 135 | */ 136 | bool item_received(int & item_size) { 137 | uint32_t content_size; 138 | uint32_t header_size; 139 | bool success; 140 | success = get_item_size(header_size, content_size); 141 | if (!success) return false; 142 | 143 | char *end_ptr = last_item_begin + content_size + header_size; 144 | // char *end_ptr = last_item_begin + header_size; 145 | 146 | int goback = 10; 147 | 148 | cout << __func__ << ": search from " << (end_ptr - goback) - last_item_begin \ 149 | << " to " << last_chunk_end - last_item_begin << endl; 150 | 151 | // check if end of picture is indeed 0xFF 0xD9 152 | //for ( char *p = end_ptr - goback; p < last_chunk_end; ++p) { 153 | for ( char *p = end_ptr - goback; p < end_ptr; ++p) { 154 | if (((unsigned char)*p == 255) && ((unsigned char)*(p+1) == 217)) { //'0xFF' and '0xD9' 155 | item_size = p - last_item_begin + 2; 156 | if (debug) { 157 | cout << __func__ << ": item received with size " << item_size << endl; 158 | cout << __func__ << ": expected size was " << header_size + content_size << endl; 159 | } 160 | return true; 161 | } 162 | } 163 | return false; 164 | } 165 | 166 | /** 167 | * Write image buffer to file. The argument is relative to the beginning of the last item. 168 | */ 169 | void write_image(uint32_t header_size, uint32_t size, std::string filename) { 170 | FILE *pFile = fopen(filename.c_str(), "wb"); 171 | fwrite(header_size + last_item_begin, 1, size, pFile); 172 | fclose(pFile); 173 | } 174 | 175 | inline void update_frame_number() { 176 | ++frame_number; 177 | } 178 | 179 | inline int get_frame_number() { return frame_number; } 180 | 181 | 182 | protected: 183 | 184 | private: 185 | char cbuffer[CHUNKSIZE]; 186 | 187 | int frame_number; 188 | 189 | char debug; 190 | }; 191 | 192 | #endif /* IMGBUFFER_HPP_ */ 193 | -------------------------------------------------------------------------------- /test/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file main.cpp 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object against this software being used by the military, in the 12 | * bio-industry, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Jul 25, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | #define cimg_use_jpeg 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | using namespace cimg_library; 47 | using namespace std; 48 | 49 | const DataValue red[] = { 255,0,0 }, green[] = { 0,255,0 }, blue[] = { 0,0,255 }; 50 | 51 | typedef cimg_library::CImg ImageType; 52 | 53 | /* ************************************************************************************** 54 | * Implementation of main 55 | * **************************************************************************************/ 56 | 57 | template 58 | void getHistogram(CImg &img, NormalizedHistogramValues & result) { 59 | int width = img._width; 60 | int height = img._height; 61 | int bins = 16; 62 | 63 | cout << "Create histogram with " << bins << " bins" << endl; 64 | Histogram histogram(bins, width, height); 65 | DataFrames frames; 66 | pDataMatrix data = img._data; 67 | frames.push_back(data); 68 | 69 | cout << "Add data for histograms" << endl; 70 | histogram.calcProbabilities(frames); 71 | 72 | // get actual histogram 73 | histogram.getProbabilities(result); 74 | } 75 | 76 | static int seed = 239483; 77 | 78 | /** 79 | * Showcase for a particle filter. It - for that reason - uses images, because it is easier 80 | * to tell what is going on using visuals. The user needs to select the thing to be tracked 81 | * and the particle filter will subsequently try to track it. 82 | */ 83 | int main() { 84 | // test_histogram(); 85 | // test_autoregression(); 86 | // test_filter(); 87 | // test_distance(); 88 | // create_track_image(); 89 | // test_convolution(); 90 | create_images(); 91 | return EXIT_SUCCESS; 92 | 93 | PositionParticleFilter filter; 94 | //FileImageSource source; 95 | IpcamImageSource source; 96 | 97 | string home = string(getenv("HOME")); 98 | if (home.empty()) { 99 | cerr << "Error: no $HOME env. variable" << endl; 100 | return EXIT_FAILURE; 101 | } 102 | 103 | string path = home + "/mydata/dotty"; 104 | string extension = ".jpg"; 105 | 106 | srand48(seed); 107 | 108 | source.SetPath(path); 109 | #ifdef FROM_FILE 110 | source.SetExtension(extension); 111 | #endif 112 | if (!source.Update()) { 113 | cerr << "Wrong path?" << endl; 114 | return EXIT_FAILURE; 115 | } 116 | 117 | string fn = "target_t1_1924674796"; 118 | 119 | FileImageSource track; 120 | track.SetPath(path); 121 | track.SetExtension(extension); 122 | string track_fn = fn + ".jpeg"; 123 | ImageType &track_img = *track.getImage(track_fn); 124 | 125 | NormalizedHistogramValues result; 126 | getHistogram(track_img, result); 127 | 128 | string config = path + '/' + fn + ".ini"; 129 | cout << "Load config file: " << config << endl; 130 | ConfigFile configfile(config); 131 | CImg img_coords(6); 132 | configfile.readInto(img_coords(0), "coord0"); 133 | configfile.readInto(img_coords(1), "coord1"); 134 | configfile.readInto(img_coords(3), "coord3"); 135 | configfile.readInto(img_coords(4), "coord4"); 136 | 137 | int subticks = 1; 138 | int particles = 100; 139 | int shift = 4; 140 | filter.Init(result, img_coords, particles); 141 | 142 | vector *> coordinates; 143 | 144 | int frame_count = 40; 145 | int frame_id = 0; 146 | while (++frame_id < frame_count) { 147 | 148 | #ifdef FROM_FILE 149 | ImageType &img = *source.getImageShifted(frame_id*shift, 0); 150 | #else 151 | ImageType &img = *source.getImage(); 152 | // CImgDisplay test_disp(img, "Show image"); 153 | // sleep (30); 154 | // return 1; 155 | #endif 156 | 157 | cout << "Clear coordinates" << endl; 158 | coordinates.erase(coordinates.begin(), coordinates.end()); 159 | coordinates.clear(); 160 | filter.GetParticleCoordinates(coordinates); 161 | 162 | CImg &img_copy = *new CImg(img); 163 | int max = 10; 164 | for (int i = 0; i < coordinates.size(); ++i) { 165 | CImg* coord = coordinates[i]; 166 | //cout << "Draw rectangle at: [" << coord->_data[0] << "," << coord->_data[1] << "," << coord->_data[3] << "," << coord->_data[4] << "]" << endl; 167 | float opacity = 0.05; 168 | img_copy.draw_line(coord->_data[0], coord->_data[1], coord->_data[0], coord->_data[4], red); 169 | img_copy.draw_line(coord->_data[0], coord->_data[1], coord->_data[3], coord->_data[1], red); 170 | img_copy.draw_line(coord->_data[3], coord->_data[1], coord->_data[3], coord->_data[4], red); 171 | img_copy.draw_line(coord->_data[0], coord->_data[4], coord->_data[3], coord->_data[4], red); 172 | if (--max == 0) break; 173 | } 174 | 175 | CImgDisplay main_disp(img_copy, "Show image"); 176 | 177 | // usleep(2000000); 178 | 179 | main_disp.wait(2000); 180 | if (main_disp.is_keyESC()) { // doesn't work, is not captured during wait (is usleep) 181 | delete &img; 182 | cout << "Escape by user, exit" << endl; 183 | break; 184 | } 185 | 186 | cout << "Particle filter tick " << frame_id << endl; 187 | filter.Tick(&img, subticks); 188 | 189 | #ifdef DISPLAY_LIKELIHOOD 190 | cout << "Calculate likelihood for all pixels" << endl; 191 | RegionSize region; 192 | region.width = img_coords(3)-img_coords(0); 193 | region.height = img_coords(4)-img_coords(1); 194 | cout << "Create picture of size " << img._height*img._width << endl; 195 | 196 | CImg &img2 = *new CImg(img._width, img._height, 1, 3); 197 | filter.GetLikelihoods(img2, region); 198 | 199 | CImgDisplay disp(img2, "Show values"); 200 | 201 | usleep(4000000); 202 | delete &img2; 203 | #endif 204 | 205 | delete &img; 206 | } 207 | 208 | delete &track_img; 209 | } 210 | 211 | -------------------------------------------------------------------------------- /inc/ProbMatrix.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file ProbMatrix.h 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 6, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | 25 | #ifndef PROBMATRIX_H_ 26 | #define PROBMATRIX_H_ 27 | 28 | /* ************************************************************************************** 29 | * Configuration options 30 | * **************************************************************************************/ 31 | 32 | #include 33 | 34 | //! Remove joint probability calculations, which saves a lot on memory requirements 35 | #define CALC_JOINTFREQ 0 36 | 37 | /* ************************************************************************************** 38 | * Configuration option consequences 39 | * **************************************************************************************/ 40 | 41 | #if CALC_JOINTFREQ == 0 42 | #undef CALC_JOINTFREQ 43 | #endif 44 | 45 | #ifndef CAREFUL_USAGE 46 | #define CHECK_FREQ 47 | #define CHECK_JOINTFREQ 48 | #define CHECK_FRAMECOUNT 49 | #else 50 | #define CHECK_FREQ { assert (freq != NULL); } 51 | #define CHECK_JOINTFREQ { assert (joint_freq != NULL); } 52 | #define CHECK_FRAMECOUNT { assert (frame_count > 0); } 53 | #endif 54 | 55 | 56 | /* ************************************************************************************** 57 | * Includes 58 | * **************************************************************************************/ 59 | 60 | // General files 61 | #include 62 | 63 | /* ************************************************************************************** 64 | * General data types/formats 65 | * **************************************************************************************/ 66 | 67 | /** 68 | * Probabilities need to represented by floats or doubles. 69 | */ 70 | typedef float Value; 71 | 72 | /** 73 | * It is possible to represent the incoming data by floats, doubles, chars, integers, 74 | * whatever floats your boat. :-) 75 | */ 76 | typedef unsigned char DataValue; 77 | 78 | /** 79 | * For just counting (to create histogram) you will not need floating point values. We will 80 | * use integers for that. 81 | */ 82 | typedef int HistogramValue; 83 | 84 | /** 85 | * The data is just represented as a big char/float/double vector. To remember the fact 86 | * that it is already a pointer to a series of values, it gets the prefix "p". It can just 87 | * be passed like function(pDataMatrix data) and put in a vector as vector, because it 88 | * doesn't actually carry the values "with it". 89 | */ 90 | typedef DataValue* pDataMatrix; 91 | 92 | /** 93 | * A (successive) series of data frames is a vector, with each element referring to an array 94 | * of doubles/floats/integers as defined by "Value". 95 | */ 96 | typedef std::vector DataFrames; 97 | 98 | 99 | /* ************************************************************************************** 100 | * Interface of ProbMatrix 101 | * **************************************************************************************/ 102 | 103 | /** 104 | * This sets up the data structures for calculations with respect to frequencies and probabilities. 105 | * It does not perform these calculations itself. 106 | */ 107 | class ProbMatrix { 108 | public: 109 | //! Constructor ProbMatrix 110 | ProbMatrix(int bins, int width, int height); 111 | 112 | //! Destructor ~ProbMatrix 113 | virtual ~ProbMatrix(); 114 | 115 | //! Get frequency of certain pixel in certain state/bin 116 | inline HistogramValue getFrequency(int p, int bin) { 117 | CHECK_FREQ; 118 | return freq[p*bins+bin]; 119 | } 120 | 121 | //! Get probability for pixel in state bin 122 | //! Try to use getFrequency instead and only divide by t_len later on 123 | inline Value getProbability(int p, int bin) { 124 | CHECK_FREQ; 125 | CHECK_FRAMECOUNT; 126 | return freq[p*bins+bin] / (Value)frame_count; 127 | } 128 | 129 | //! Get number of sensors 130 | inline int getSensorCount() { return p_size; } 131 | 132 | //! Get number of bins 133 | inline int getBins() { return bins; } 134 | 135 | //! Set number of bins 136 | inline void setBins(int bins) { this->bins = bins; bins_squared = bins*bins; } 137 | 138 | //! Get joint probability for pixel p0 and p1 in state bin0 and bin1 139 | //! Try to use getJointFrequency instead and divide by time_len as 140 | //! late as possible 141 | Value getJointProbability(int p0, int bin0, int p1, int bin1) { 142 | CHECK_FRAMECOUNT; 143 | return getJointFrequency(p0, bin0, p1, bin1) / (Value)frame_count; 144 | } 145 | 146 | /** 147 | * Get joint "probability". As you can see it is called joint frequency and not joint probability. 148 | * This means that we do not divide by the total number of possible events (in other words, divide 149 | * by the number of frames. This makes it possible to use integers, and we do not need floats/doubles. 150 | * If you really want to use probabilities, use getJointProbability instead (which of course will use 151 | * the previously set number of frames from the calculations before). 152 | * 153 | * The getJointFrequency function just returns values that are already pre-calculated. Each time 154 | * the input changes, you will have to call calcProbabilities first. 155 | */ 156 | HistogramValue getJointFrequency(int p0, int bin0, int p1, int bin1) { 157 | CHECK_JOINTFREQ; 158 | if (p0 == p1) return 0; 159 | if (p0 < p1) { // swap, only upper triangle of matrix is filled, because it is symmetric 160 | int temp = p0; p0 = p1; p1 = temp; 161 | temp = bin0; bin0 = bin1; bin1 = temp; 162 | } 163 | // get offset for joint freq vector for (p0,p1) 164 | // p_size is needed for row size estimation 165 | int m = p0*bins_squared + p1*bins_squared*p_size; 166 | return joint_freq[m+bin0+bins*bin1]; 167 | } 168 | 169 | protected: 170 | //! Number of bins 171 | int bins; 172 | 173 | //! Width of individual data frame 174 | int p_width; 175 | 176 | //! Height of individual data frame 177 | int p_height; 178 | 179 | //! Number of bins squared (automatically calculated) 180 | int bins_squared; 181 | 182 | //! The total size of one data frame (automatically calculated) 183 | int p_size; 184 | 185 | //! Probability (or actually frequency) matrix 186 | HistogramValue *freq; 187 | 188 | //! Joint probability (or actually frequency) matrix 189 | HistogramValue *joint_freq; 190 | 191 | //! Number of frames 192 | int frame_count; 193 | 194 | private: 195 | 196 | }; 197 | 198 | #endif /* PROBMATRIX_H_ */ 199 | -------------------------------------------------------------------------------- /inc/PositionParticleFilter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Particle Filter for Position Estimates on Images 3 | * @file PositionParticleFilter.h 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 14, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | 25 | #ifndef POSITIONPARTICLEFILTER_H_ 26 | #define POSITIONPARTICLEFILTER_H_ 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | using namespace cimg_library; 39 | using namespace std; 40 | 41 | //! An "unsigned char" is not be enough for resolution larger than 256x256, so we go for an "int" 42 | typedef int CoordValue; 43 | 44 | /** 45 | * Just the extend of a region, not its location. 46 | */ 47 | struct RegionSize { 48 | int width; 49 | int height; 50 | }; 51 | 52 | static int ParticleStateId = 0; 53 | 54 | /** 55 | * The particle's state is just a rectangular region, and is defined over a few time steps. 56 | */ 57 | class ParticleState { 58 | public: 59 | ParticleState() { 60 | id = ++ParticleStateId; 61 | x.clear(); 62 | y.clear(); 63 | scale.clear(); 64 | likelihood = 0; 65 | width = 0; 66 | height = 0; 67 | } 68 | 69 | ParticleState(int id): id(id) { 70 | x.clear(); 71 | y.clear(); 72 | scale.clear(); 73 | likelihood = 0; 74 | width = 0; 75 | height = 0; 76 | } 77 | 78 | ~ParticleState() { 79 | x.clear(); 80 | y.clear(); 81 | scale.clear(); 82 | } 83 | 84 | //! The (default) width of the rectangular region 85 | int width; 86 | //! The (default) height of the rectangular region 87 | int height; 88 | 89 | //! The histogram corresponding to a particle 90 | // int histogram; 91 | Value likelihood; 92 | 93 | //! The x-vector denotes the horizontal centre of a rectangular region and its history 94 | std::vector x; 95 | //! The y-vector denotes the vertical centre of a rectangular region and its history 96 | std::vector y; 97 | //! A floating point value that scales width and height 98 | std::vector scale; 99 | 100 | //! Easy printing 101 | friend std::ostream& operator<<(std::ostream& os, const ParticleState & ps) { 102 | if ((ps.x.size() == 1) && (ps.y.size() == 1)) { 103 | os << ps.id << " [" << ps.x.front() << ',' << ps.y.front() << "] (" << ps.likelihood << ")"; 104 | } else if (ps.x.size() > 1 && ps.y.size() > 1) { 105 | // use history at position [1] 106 | os << ps.id << " [" << ps.x[1] << ',' << ps.y[1] << "] -> [" << ps.x[0] << ',' << ps.y[0] << "] (" << ps.likelihood << ")"; 107 | } else { 108 | os << ps.id << " []"; 109 | } 110 | return os; 111 | } 112 | 113 | /** 114 | * A copy instructor for ParticleState. Somehow we have to build up the vectors from 115 | * scratch and cannot use std::copy. 116 | */ 117 | ParticleState(const ParticleState & other): x(other.x.size()), 118 | y(other.y.size()), scale(other.scale.size()) { 119 | id = other.id; 120 | width = other.width; 121 | height = other.height; 122 | likelihood = other.likelihood; 123 | 124 | x.clear(); y.clear(); scale.clear(); 125 | for (int i = 0; i < other.x.size(); ++i) x.push_back(other.x[i]); 126 | for (int i = 0; i < other.y.size(); ++i) y.push_back(other.y[i]); 127 | for (int i = 0; i < other.scale.size(); ++i) scale.push_back(other.scale[i]); 128 | 129 | assert (!other.x.empty()); 130 | ASSERT_EQUAL(x.size(), other.x.size()); 131 | } 132 | 133 | inline const int getId() { return id; } 134 | 135 | private: 136 | //! Mainly for debugging reasons, make sure we actually make copies at the right moment, etc. 137 | int id; 138 | }; 139 | 140 | 141 | /* ************************************************************************************** 142 | * Interface of PositionParticleFilter 143 | * **************************************************************************************/ 144 | 145 | /** 146 | * The particle filter that is used for tracking a 2D screen "position" plus some 147 | * additional state "elaborations", such as width, height, and histogram. 148 | */ 149 | class PositionParticleFilter: public ParticleFilter { 150 | public: 151 | //! Default constructor 152 | PositionParticleFilter(); 153 | 154 | //! Default destructor 155 | ~PositionParticleFilter(); 156 | 157 | /** 158 | * Set image and calculate everything necessary... Provide multiple times the same frame 159 | * if that is required. 160 | */ 161 | void Tick(CImg *img_frame, int subticks = 1); 162 | 163 | /** 164 | * Initialize particle cloud. 165 | * @param tracked_object_histogram histogram of the entity that needs to be tracked 166 | * @param coord CImg coordinates, careful: picks 0,1 3,4 (skips 2) 167 | * @param particle_count the number of particles to be generated 168 | * @return void 169 | */ 170 | void Init(NormalizedHistogramValues &tracked_object_histogram, CImg &coord, 171 | int particle_count); 172 | 173 | //! Transition of all particles following a certain motion model 174 | void Transition(); 175 | 176 | /** 177 | * Autoregressive model to estimate where an object will be next. See implementation 178 | * for the actual model used. 179 | */ 180 | void Transition(ParticleState &oldp); 181 | 182 | /** 183 | * Calculate likelihood of all particles 184 | */ 185 | void Likelihood(); 186 | 187 | /** 188 | * Return particles, or more specific, return the coordinates of the particles, ordered 189 | * on weight. 190 | */ 191 | void GetParticleCoordinates(std::vector *> & coordinates); 192 | 193 | /** 194 | * Return the likelihood of the histogram at all possible positions. 195 | */ 196 | void GetLikelihoods(CImg & result, RegionSize region_size, int block_size = 8); 197 | protected: 198 | 199 | /** 200 | * Calculate the likelihood of a player and the state indicated by the parameter 201 | * "state" which contains an x and y position, a width and a height. This is used 202 | * to define a rectangle for which a histogram is matched against the reference 203 | * histogram of the object that is tracked. 204 | * @param state the state of the particle (position, width, height) 205 | * @return conceptual "distance" to the reference (tracked) object 206 | */ 207 | float Likelihood(ParticleState & state); 208 | 209 | private: 210 | //! The number of bins 211 | int bins; 212 | 213 | //! The histogram of the object to be tracked 214 | NormalizedHistogramValues tracked_object_histogram; 215 | 216 | //! Image data 217 | // pDataMatrix data; 218 | 219 | //! Image to get data from 220 | CImg * img; 221 | 222 | //! Seed for random number generator 223 | int seed; 224 | 225 | //! See http://demonstrations.wolfram.com/AutoRegressiveSimulationSecondOrder/ 226 | std::vector auto_coeff; 227 | 228 | 229 | }; 230 | 231 | #endif /* POSITIONPARTICLEFILTER_H_ */ 232 | -------------------------------------------------------------------------------- /inc/IpcamImageSource.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file IpcamImageSource.h 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with nof_bytes_read lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Sep 18, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | 25 | #ifndef IPCAMIMAGESOURCE_H_ 26 | #define IPCAMIMAGESOURCE_H_ 27 | 28 | // General files 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | /* ************************************************************************************** 39 | * Interface of IpcamImageSource 40 | * **************************************************************************************/ 41 | 42 | template 43 | class IpcamImageSource: public ImageSource { 44 | public: 45 | //! Constructor IpcamImageSource 46 | IpcamImageSource(): quiet_flag(true), debug(false), 47 | connect_to_http_server_timeout(5), wait_per_package(1000) { 48 | http_server = "10.10.1.113"; 49 | http_port = 80; 50 | // dframes_per_second = 20; 51 | // reconnects = 0; 52 | // http_buffer = (char *)malloc(HTTP_BUFFER_SIZE); 53 | // char temp[8000]; 54 | 55 | access_string = ""; 56 | version = "0.7.9"; 57 | // content_length = 8000; 58 | } 59 | 60 | //! Destructor ~IpcamImageSource 61 | virtual ~IpcamImageSource() {} 62 | 63 | //! Perform functionality that is required to get images 64 | bool Update() { 65 | FILE *pFile = fopen("debug.jpeg", "w+"); 66 | fclose(pFile); 67 | // ; 68 | while(1) { 69 | if(connect_to_server(http_server.c_str(), http_port, &socketfd) ) { 70 | cout << __func__ << ": successfully connected to http server" << endl; 71 | break; 72 | } 73 | fprintf(stderr, "mcamip: could not connect to http server, retry.\n"); 74 | } 75 | 76 | if(debug) { 77 | fprintf(stderr, "socketfd=%d\n", socketfd); 78 | } 79 | return true; 80 | } 81 | 82 | //! Get an image (the next image if there are multiple). 83 | Image* getImage() { 84 | cout << __func__ << ": get image " << endl; 85 | char temp[8000]; 86 | 87 | sprintf(temp,"GET /video.cgi HTTP/1.1\n\ 88 | User-Agent: mcamip (rv:%s; X11; Linux)\n\ 89 | Accept: text/xml,application/xml,application/xhtml+xml,text/html;q=0.9,text/plain;q=0.8,video/x-mng,image/png,image/jpeg,image/gif;q=0.2,text/cs s,*/*;q=0.1\n\ 90 | Accept-Language: en-us, en;q=0.50\n\ 91 | Accept-Encoding: gzip, deflate,compress;q=0.9\n\ 92 | Accept-Charset: ISO-8859-1, utf-8;q=0.66, *;q=0.66\n\ 93 | Keep-Alive: 300\n\ 94 | Connection: Keep-Alive\n\ 95 | Authorization: Basic %s\n\ 96 | Referer: http://%s:%d/Jview.htm\n\n",\ 97 | version.c_str(), access_string.c_str(), server_ip_address, http_port); 98 | 99 | if(!send_to_server(socketfd, temp) ) { 100 | fprintf(stderr, "mcamip: could not send command to server, aborting.\n"); 101 | exit(1); 102 | } 103 | cout << __func__ << ": request image from server" << endl; 104 | 105 | uint32_t header_size, content_size; int item_size; 106 | do { 107 | usleep(wait_per_package); 108 | int bytes = img_buffer.read_from_socket(socketfd); 109 | if (bytes <= 0) continue; 110 | 111 | if (debug) 112 | cout << __func__ << ": received chunk" << endl; 113 | 114 | if (img_buffer.check_item_errors()) { 115 | cerr << __func__ << ": item contains errors" << endl; 116 | img_buffer.reset(); 117 | continue; 118 | } 119 | 120 | if (!img_buffer.get_item_size(header_size, content_size)) { 121 | cerr << __func__ << ": could not retrieve header and/or content size" << endl; 122 | continue; 123 | } 124 | 125 | if (!img_buffer.item_received(item_size)) { 126 | cerr << __func__ << ": item not yet received" << endl; 127 | continue; 128 | } 129 | 130 | std::ostringstream oss; oss.clear(); oss.str(""); 131 | oss << this->img_path << '/' << this->img_basename << img_buffer.get_frame_number() << this->img_extension; 132 | std::string file = oss.str(); 133 | 134 | // first write image to file (admittedly a roundabout way) 135 | img_buffer.write_image(header_size, content_size, file); 136 | 137 | img_buffer.next_item(header_size+content_size); 138 | 139 | img_buffer.update_frame_number(); 140 | 141 | // load image from file 142 | Image *img = new Image(file.c_str()); 143 | return img; 144 | 145 | } while(true); 146 | 147 | cout << "Done!" << endl; 148 | return NULL; 149 | } 150 | 151 | //! Get an image but shifted in maximum two directions. 152 | Image* getImageShifted(int shift_x, int shift_y) { 153 | return NULL; 154 | } 155 | 156 | protected: 157 | 158 | /** 159 | * Connect to the server at a specific address and port. 160 | */ 161 | int connect_to_server(const char *server, int port, int *socketfd) { 162 | cout << __func__ << ": connect to " << server << endl; 163 | struct hostent *hp; 164 | struct sockaddr_in sa; 165 | int a; 166 | time_t connect_timer; 167 | 168 | if(!server) return 0; 169 | if(port <= 0) return 0; 170 | 171 | if(!quiet_flag) { 172 | fprintf(stderr, "mcamip: getting host %s by name\n", server); 173 | } 174 | 175 | hp = gethostbyname(server); 176 | if(hp == 0) 177 | { 178 | fprintf(stderr,\ 179 | "gethostbyname: returned NULL cannot get host %s by name.\n", server); 180 | 181 | /* signal FD_SET (main) that this is no longer nof_bytes_read valid filedescriptor */ 182 | *socketfd = -1; 183 | return 0; 184 | } 185 | 186 | /* gethostbyname() leaves port and host address in network byte order */ 187 | 188 | bzero(&sa, sizeof(sa) ); 189 | bcopy(hp->h_addr, (char *)&sa.sin_addr, hp->h_length); 190 | 191 | 192 | sa.sin_family = AF_INET; 193 | sa.sin_port = htons( (u_short)port); 194 | 195 | /* sa.sin_addr and sa.sin_port now in network byte order */ 196 | 197 | /* create nof_bytes_read socket */ 198 | *socketfd = socket(hp->h_addrtype, SOCK_STREAM, 0); 199 | if(*socketfd < 0) 200 | { 201 | fprintf(stderr, "socket failed\n"); 202 | *socketfd = -1; 203 | return(0); 204 | } 205 | 206 | /* set for nonblocking socket */ 207 | if (fcntl(*socketfd, F_SETFL, O_NONBLOCK) < 0) 208 | { 209 | return(0); 210 | } 211 | 212 | sprintf(server_ip_address, "%s", inet_ntoa (sa.sin_addr) ); 213 | 214 | if(! quiet_flag) 215 | { 216 | fprintf(stderr,\ 217 | "mcamip: connecting to %s (%s) port %d timeout %d\n",\ 218 | server, server_ip_address, port, connect_to_http_server_timeout); 219 | } 220 | 221 | /* prevent the program from hanging if connect takes nof_bytes_read long time, now nof_bytes_read return 0 is forced. */ 222 | 223 | /* start the timer */ 224 | connect_timer = time(0); 225 | 226 | /* keep testing for a connect */ 227 | while(1) 228 | { 229 | /* connect */ 230 | a = connect(*socketfd, (struct sockaddr*)&sa, sizeof(sa) ); 231 | if(a == 0) break; /* connected */ 232 | if(a < 0) 233 | { 234 | if(debug) 235 | { 236 | fprintf(stderr, "mcamip: connect() failed because: "); 237 | perror(""); 238 | } 239 | 240 | /* test for connect time out */ 241 | if( (time(0) - connect_timer) > connect_to_http_server_timeout) 242 | { 243 | /* close the socket */ 244 | close(*socketfd); 245 | 246 | /* set socketfd to invalid, it was valid! */ 247 | *socketfd = -1; 248 | fprintf(stderr, "mcamip: connect timeout\n"); 249 | 250 | return 0; 251 | } 252 | 253 | }/* end connect < 0 */ 254 | }/* end while test for nof_bytes_read connect */ 255 | 256 | return 1; 257 | } 258 | 259 | /** 260 | * Send data or a command to the server. 261 | */ 262 | int send_to_server(int socketfd, char txbuf[]) { 263 | int a; 264 | 265 | if(debug) { 266 | fprintf(stderr, "send_to_server(): socketfd=%d txbuf=%p txbuf=\n\%s\n", socketfd, txbuf, txbuf); 267 | } 268 | 269 | if(socketfd < 0) return 0; 270 | if(! txbuf) return 0; 271 | 272 | a = write(socketfd, txbuf, strlen(txbuf) ); 273 | if(a < 0) { 274 | fprintf(stderr, "mcamip: send_to_server(): write failed because "); 275 | perror(""); 276 | return 0; 277 | } 278 | return 1; 279 | } 280 | 281 | private: 282 | 283 | bool quiet_flag; 284 | 285 | //! flag for debugging 286 | int debug; 287 | 288 | //! the actual IP address 289 | char server_ip_address[512]; 290 | 291 | //! the image buffer 292 | imgbuffer img_buffer; 293 | 294 | //! the name or IP address of the webcam 295 | std::string http_server; 296 | 297 | //! the port over which to access the webcam 298 | int http_port; 299 | 300 | //! required timeout 301 | int connect_to_http_server_timeout; 302 | 303 | //! the socket file descriptor that is returned 304 | int socketfd; 305 | 306 | // int dframes_per_second; 307 | int wait_per_package; 308 | 309 | //! Password or access string for the camera 310 | std::string access_string; 311 | 312 | //! Version of this software 313 | std::string version; 314 | }; 315 | 316 | #endif /* IPCAMIMAGESOURCE_H_ */ 317 | -------------------------------------------------------------------------------- /inc/ParticleFilter.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Implementation of a particle filter 3 | * @file ParticleFilter.hpp 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object against this software being used by the military, in the 12 | * bio-industry, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Jul 24, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | #ifndef PARTICLEFILTER_HPP_ 25 | #define PARTICLEFILTER_HPP_ 26 | 27 | // General files 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | /* ************************************************************************************** 38 | * Interface of ParticleFilter 39 | * **************************************************************************************/ 40 | 41 | 42 | /** 43 | * Local class that knows that colons can be treated as white spaces. It is used by the input stream. 44 | */ 45 | struct colonsep: std::ctype { 46 | colonsep(): std::ctype(get_table()) {} 47 | static std::ctype_base::mask const* get_table() { 48 | static std::vector 49 | rc(std::ctype::table_size,std::ctype_base::mask()); 50 | rc[':'] = std::ctype_base::space; 51 | rc[' '] = std::ctype_base::space; 52 | rc['\n'] = std::ctype_base::space; 53 | return &rc[0]; 54 | } 55 | }; 56 | 57 | /** 58 | * Local class that knows that commas can be treated as white spaces. It is by the input stream 59 | * operator. 60 | */ 61 | struct commasep: std::ctype { 62 | commasep(): std::ctype(get_table()) {} 63 | static std::ctype_base::mask const* get_table() { 64 | static std::vector 65 | rc(std::ctype::table_size,std::ctype_base::mask()); 66 | rc[','] = std::ctype_base::space; 67 | return &rc[0]; 68 | } 69 | }; 70 | 71 | /** 72 | * A particle in a particle filter is an entity that has state information, such as the 73 | * x-position and y-position of your cursor. Or the x,y coordinates plus the size of a 74 | * football player on your TV screen. Or the x,y location of a robot that moves through 75 | * an environment and needs to track its own position. It is anything that can be 76 | * estimated by a cloud of particles and for which this estimation might improve over 77 | * time my more (although noisy) measurements. 78 | */ 79 | template 80 | class Particle { 81 | public: 82 | Particle() { 83 | state = new State(); 84 | weight = 0; 85 | } 86 | 87 | Particle(State *state, double weight) { 88 | this->state = state; 89 | this->weight = weight; 90 | } 91 | 92 | ~Particle() { 93 | weight = 0; 94 | delete state; 95 | } 96 | 97 | inline State* getState() { return state; } 98 | 99 | inline double getWeight() { return weight; } 100 | 101 | inline void setState(State* state) { this->state = state; } 102 | 103 | inline void setWeight(double weight) { this->weight = weight; } 104 | 105 | /** 106 | * The clone function should not be really necessary, by implementing an assignment operator, a 107 | * copy constructor, and a swap function. However, I couldn't get it made to work. 108 | */ 109 | Particle *clone() { 110 | State *s = new State(*state); 111 | Particle *p = new Particle(s, 0); 112 | return p; 113 | } 114 | 115 | #ifdef WORKS 116 | void swap(Particle &p) { 117 | std::swap(state, p.state); 118 | std::swap(weight, p.weight); 119 | } 120 | 121 | Particle & operator=(Particle other) { 122 | std::cout << __func__ << ": Create particle from another using swap " << std::endl; 123 | swap(other); 124 | return *this; 125 | } 126 | #endif 127 | 128 | private: 129 | State *state; 130 | double weight; 131 | 132 | friend std::ostream& operator<<(std::ostream& os, const Particle & p) { 133 | os << p.weight << ',' << p->state; 134 | return os; 135 | } 136 | 137 | // append 138 | friend std::istream& operator>>( std::istream& is, Particle& p) { 139 | is.imbue(std::locale(std::locale(), new commasep)); 140 | is >> p.weight >> p->state; 141 | return is; 142 | } 143 | }; 144 | 145 | 146 | /** 147 | * Helper function for sorting (with highest weight first) 148 | */ 149 | template 150 | bool comp_particles(Particle * p0, Particle * p1) { 151 | return (p0->getWeight() > p1->getWeight()); 152 | } 153 | 154 | /** 155 | * Helper function for summing up and normalizing 156 | */ 157 | template 158 | double sum_particle_weight(double sum, Particle * p) { 159 | return sum + p->getWeight(); 160 | } 161 | 162 | /** 163 | * Helper function for normalizing. 164 | */ 165 | template 166 | class divide_weight { 167 | public: 168 | divide_weight(double factor): factor(factor) {} 169 | void operator()(Particle *p) const { 170 | p->setWeight(p->getWeight() / factor); 171 | } 172 | private: 173 | double factor; 174 | }; 175 | 176 | template 177 | class ParticleFilter; 178 | 179 | /** 180 | * Add particles to a set, and get them out. 181 | */ 182 | template 183 | class ParticleSet { 184 | public: 185 | ParticleSet() { particles.clear(); } 186 | 187 | ~ParticleSet() { } 188 | 189 | // Normalize such that total weight sums up to one 190 | void Normalize() { 191 | double w = std::accumulate(particles.begin(), particles.end(), double(0), sum_particle_weight); 192 | std::for_each(particles.begin(), particles.end(), divide_weight(w) ); 193 | #ifdef DEBUG 194 | double check = std::accumulate(particles.begin(), particles.end(), double(0), sum_particle_weight); 195 | std::cout << "Should now sum to 1: " << check << std::endl; 196 | #endif 197 | } 198 | 199 | protected: 200 | 201 | private: 202 | std::vector* > particles; 203 | 204 | friend class ParticleFilter; 205 | 206 | // extract 207 | friend std::ostream& operator<<(std::ostream& os, const ParticleSet & set) { 208 | std::copy(set.particles.begin(), set.particles.end(), std::ostream_iterator >(os, ',')); 209 | return os; 210 | } 211 | 212 | // append 213 | friend std::istream& operator>>( std::istream& is, ParticleSet& set) { 214 | is.imbue(std::locale(std::locale(), new colonsep)); 215 | Particle *particle = new Particle(); 216 | while(is >> particle) { 217 | set.particles.push_down(particle); 218 | particle = new Particle(); 219 | } 220 | delete particle; 221 | return is; 222 | } 223 | }; 224 | 225 | 226 | /** 227 | * The particle filter. 228 | * 229 | * It starts with an initial (so-called prior) distribution. This can for example be 230 | * initiated by the user, pointing to the current position of the football player. It 231 | * can be an exhaustive scan over an entire image to detect a target, which subsequently 232 | * needs to be tracked, or it can be a random spot (which defines the origin of the 233 | * coordinate system in a robot which tracks itself). 234 | * 235 | * Then the filter needs a transition model. This is an educated guess on how the thing 236 | * that is tracked changes from state. Something on an image stays which a high 237 | * probability near the location where it was at the previous time step. Moreover, most 238 | * likely it moves in the same direction in which it moved before. Of course, this 239 | * educated guess might be something else than the modeler's knowledge of the thing to 240 | * be tracked. The transition model might be obtained by a supervised learning process. 241 | * 242 | * Third, the filter needs an observation model. This is the probability that that 243 | * what we want to track is actually at the position that we are consider. So, this is 244 | * p(object|location), where we use "location" instead of the more general "state" for 245 | * the story. One way to do this in an image is to calculate the difference between the 246 | * two histograms, the reference histogram of the object, and the histogram at the 247 | * location into consideration. Note that if you use a supervised learning method for 248 | * the transition model, you'll probably need this observation model. However, it is 249 | * also not hard to imagine that the observation model on a robot is computationally 250 | * cheap, while for learning the transition model you will use something complex, so 251 | * you don't need to think about motion equations yourself. 252 | * 253 | * In the end we want to have p(location|object), the object that we saw at the start, 254 | * we want to be able to track over time till the very end of times... 255 | */ 256 | template 257 | class ParticleFilter { 258 | public: 259 | //! Constructor ParticleFilter 260 | ParticleFilter() {} 261 | 262 | //! Destructor ~ParticleFilter 263 | virtual ~ParticleFilter() {} 264 | 265 | //! The actual smart part of the particle filter 266 | void Resample() { 267 | set.Normalize(); 268 | // sort, with highest weight first 269 | std::sort(set.particles.begin(), set.particles.end(), comp_particles); 270 | // now we append 271 | int N = set.particles.size(); int newN = 0; 272 | for (int i = 0; i < N; ++i) { 273 | int copies = round(set.particles[i]->getWeight() * N); 274 | for (int j = 0; j < copies; ++j) { 275 | Particle *newp = set.particles[i]->clone(); 276 | set.particles.push_back(newp); 277 | newN++; 278 | if (newN == N) { 279 | // now remove old items 280 | set.particles.erase(set.particles.begin(), set.particles.begin()+N); 281 | assert ( set.particles.size() == newN); 282 | return; 283 | } 284 | } 285 | } 286 | while (newN < N) { 287 | // duplicate particle with highest weight to get exactly same number again 288 | Particle *newp = set.particles[0]->clone(); 289 | set.particles.push_back(newp); 290 | newN++; 291 | } 292 | set.particles.erase(set.particles.begin(), set.particles.begin()+N); 293 | assert ( set.particles.size() == newN); 294 | } 295 | 296 | //! Transition according to a certain model 297 | virtual void Transition() = 0; 298 | 299 | //! Observation model: 300 | //! - given a particle, how likely that it is corresponding to the tracked entity? 301 | //! This function should calculate this for all particles and update weights accordingly 302 | virtual void Likelihood() = 0; 303 | 304 | protected: 305 | //! Hand over access to particles to subclasses 306 | std::vector* >& getParticles() { return set.particles; } 307 | 308 | private: 309 | 310 | //! The actual cloud of particles 311 | ParticleSet set; 312 | }; 313 | 314 | #endif /* PARTICLEFILTER_HPP_ */ 315 | -------------------------------------------------------------------------------- /src/Histogram.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Crutchfield.cpp 3 | * @brief Calculate Crutchfield distance metric to sensors 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object against this software used by the military, in the 12 | * bio-industry, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2010 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Feb 22, 2011 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case Sensory processing 22 | */ 23 | 24 | #include 25 | 26 | // General files 27 | #include 28 | #include 29 | #include 30 | 31 | using namespace std; 32 | 33 | /* ************************************************************************************** 34 | * Implementation of Histogram 35 | * **************************************************************************************/ 36 | 37 | /** 38 | * For histogram calculations we only need to count 39 | */ 40 | Histogram::Histogram(int bins, int width, int height): ProbMatrix(bins, width, height) { 41 | 42 | } 43 | 44 | /** 45 | * The destructor deallocates everything. 46 | */ 47 | Histogram::~Histogram() { 48 | Clear(); 49 | bins = 0; 50 | p_width = 0; 51 | p_height = 0; 52 | bins_squared = 0; 53 | p_size = 0; 54 | frame_count = 0; 55 | } 56 | 57 | /** 58 | * Delete all arrays that might have been created to calculate the histograms. 59 | */ 60 | void Histogram::Clear() { 61 | if (freq != NULL) { 62 | #ifdef VERBOSE 63 | cout << __func__ << ": clear frequency table" << endl; 64 | #endif 65 | delete [] freq; 66 | } 67 | if (joint_freq != NULL) { 68 | #ifdef VERBOSE 69 | cout << __func__ << ": clear joint frequency table" << endl; 70 | #endif 71 | delete [] joint_freq; 72 | } 73 | freq = joint_freq = NULL; 74 | } 75 | 76 | /** 77 | * We need a function that calculates the conditional probability for two sensors over 78 | * a time series. We calculate this for all sensor values and calculate the conditional 79 | * entropy. A structure is needed to store all sensor values over a certain time. 80 | * 81 | * @param frames all data in the form of a vector of "matrices" 82 | * @return void 83 | * 84 | * Resulting side-information: 85 | * - frequency matrix (histogram) 86 | * - joint frequency matrix (conditional histogram) 87 | * - number of frames (frame_count) 88 | */ 89 | void Histogram::calcProbabilities(DataFrames & frames) { 90 | frame_count = frames.size(); 91 | 92 | // Delete previous matrices 93 | Clear(); 94 | 95 | // Create probability matrix 96 | #ifdef VERBOSE 97 | // Show size to the user 98 | int kB = (bins * p_size) >> 8; float MB = kB / (float)1024; int mb = MB * 100; MB = mb / (float)100; 99 | cout << __func__ << ": Create probability matrix of size " << bins << "x" << p_size << " (size = " << MB << "MB)" << endl; 100 | #endif 101 | freq = new HistogramValue[bins * p_size]; 102 | std::fill_n(freq, bins * p_size, (HistogramValue)0); 103 | if (freq == NULL) { 104 | cerr << "Probability matrix could not be allocated" << endl; 105 | QUIT_ON_ERROR; 106 | } 107 | 108 | // Fill probability matrix 109 | #ifdef VERBOSE 110 | cout << __func__ << ": Fill probability matrix" << endl; 111 | #endif 112 | for (int p = 0; p < p_size; ++p) { 113 | for (int t = 0; t < frame_count; ++t) { 114 | pDataMatrix data = frames[t]; 115 | int bin = value2bin(data[p]); 116 | #ifdef CAREFUL_USAGE 117 | assert (p*bins+bin < bins * p_size); 118 | #endif 119 | freq[p*bins+bin]++; 120 | } 121 | } 122 | #ifdef VERBOSE 123 | cout << __func__ << ": Matrices filled" << endl; 124 | #endif 125 | 126 | #ifdef CALC_JOINTFREQ 127 | // Create joint probability matrix 128 | cout << "Create joint probability matrix of size " << bins << "x" << bins << " * " << p_size * p_size << endl; 129 | joint_freq = new HistogramValue[bins_squared * p_size * p_size]; 130 | std::fill_n(joint_freq, bins_squared * p_size * p_size, (HistogramValue)0); 131 | if (joint_freq == NULL) { 132 | cerr << "Joint probability matrix could not be allocated" << endl; 133 | QUIT_ON_ERROR; 134 | } 135 | 136 | // Fill joint probability matrix 137 | cout << "Fill joint probability matrix" << endl; 138 | for (int t = 0; t < frame_count; ++t) { 139 | for (int p0 = 0; p0 < p_size; ++p0) { 140 | for (int p1 = 1; p1 < p_size; ++p1) { 141 | if (p0 <= p1) continue; // omit filling in bottom triangle of matrix 142 | pDataMatrix data = frames[t]; 143 | int bin0 = value2bin(data[p0]); 144 | int bin1 = value2bin(data[p1]); 145 | int m = p0*bins_squared + p1*bins_squared*p_size; //offset 146 | joint_freq[m+bin0+bins*bin1]++; 147 | #ifdef VERBOSE 148 | cout << "Calculate bin " << bin1 << "=" << s1.val[0] << "*" << bins << "/256" << endl; 149 | cout << "Joint freq " << p0 << "," << p1 << ":" << bin0 << "," << bin1 << " ++" << endl; 150 | cout << "Thus m becomes " << p0*bins*bins << "+" << p1*bins*bins*p_width*p_height << "=" << m << endl; 151 | cout << "Increment position m+" << bin0*bins << "+" << bin1 << "=" << m+bin0*bins+bin1 << endl; 152 | #endif 153 | } 154 | } 155 | } 156 | #endif 157 | } 158 | 159 | /** 160 | * Calculates conditional entropy of sensors with respect to each other for a time 161 | * series of length "frame_count". 162 | * 163 | * H(Y|X) = - SUM_x SUM_y p(x,y) log { p(x) / p(x,y) } 164 | * 165 | * where H is the entropy, SUM is a sigma sign, summing over all possibilities x 166 | * and y, log is log2, p(x,y) is the joint probability of P(X=x,Y=y). Here X is a given 167 | * information source, a sensor, and x is the number of states this information source 168 | * can occupy, in this case sensor values are divided into N bins. 169 | * 170 | * @param p0 Index of first sensor 171 | * @param p1 Index of second sensor 172 | * @result Conditional entropy 173 | * 174 | * Required side-information: 175 | * - previously calculated frequency matrix 176 | * - previously calculated joint frequency matrix 177 | * - previously set number of frames "frame_count" 178 | */ 179 | Value Histogram::getConditionalEntropy(int p0, int p1) { 180 | CHECK_FRAMECOUNT; 181 | CHECK_FREQ; 182 | CHECK_JOINTFREQ; 183 | Value sum = 0; 184 | if (p0 == p1) return 0; 185 | for (int b0 = 0; b0 < bins; b0++) { 186 | for (int b1 = 0; b1 < bins; b1++) { 187 | Value f01 = (Value)getJointFrequency(p0, b0, p1, b1); 188 | Value f0 = (Value)getFrequency(p0, b0); 189 | 190 | // remove division by 0 and log(0) 191 | if ( (f0 == 0) || (f01 == 0) ) continue; 192 | // use log = ln for now 193 | sum += (f01 / (Value)frame_count) * log ( f0 / f01 ) / log(2); 194 | } 195 | } 196 | return sum; 197 | } 198 | 199 | void Histogram::getFrequencies(vector &bin_result) { 200 | CHECK_FRAMECOUNT; 201 | CHECK_FREQ; 202 | bin_result.clear(); 203 | 204 | for (int b = 0; b < bins; ++b) { 205 | int f = 0; 206 | for (int p = 0; p < p_size; ++p) { 207 | f += freq[p*bins+b]; 208 | } 209 | bin_result.push_back(f); 210 | } 211 | } 212 | 213 | int Histogram::getSamples() { 214 | CHECK_FRAMECOUNT; 215 | CHECK_FREQ; 216 | int f = 0; 217 | for (int b = 0; b < bins; ++b) { 218 | for (int p = 0; p < p_size; ++p) { 219 | f += freq[p*bins+b]; 220 | } 221 | } 222 | return f; 223 | } 224 | 225 | void Histogram::getProbabilities(vector &bin_result) { 226 | CHECK_FRAMECOUNT; 227 | CHECK_FREQ; 228 | bin_result.clear(); 229 | 230 | int sum_f = 0; 231 | for (int b = 0; b < bins; ++b) { 232 | int f = 0; 233 | for (int p = 0; p < p_size; ++p) { 234 | f += freq[p*bins+b]; 235 | } 236 | sum_f += f; 237 | bin_result.push_back(f); 238 | } 239 | assert (sum_f != 0); 240 | for (int b = 0; b < bins; ++b) { 241 | bin_result[b] /= (Value)sum_f; 242 | } 243 | } 244 | 245 | #ifdef DEBUG 246 | 247 | void Histogram::printFrequencies(int bin) { 248 | for (int p = 0; p < p_size; ++p) { 249 | cout << freq[p*bins+bin] << ", "; 250 | } 251 | cout << endl; 252 | } 253 | 254 | void Histogram::printJointFrequencies(int bin0, int bin1) { 255 | for (int i0 = 0; i0 < p_width; i0++) { 256 | for (int j0 = 0; j0 < p_height; j0++) { 257 | for (int i1 = 0; i1 < p_width; i1++) { 258 | for (int j1 = 0; j1 < p_height; j1++) { 259 | int p0 = j0 * p_width + i0; 260 | int p1 = j1 * p_width + i1; 261 | int m = p0*bins*bins+p1*bins*bins*p_width*p_height; //offset 262 | cout << joint_freq[m+bin0+bins*bin1] << ", "; 263 | } 264 | } 265 | cout << endl; 266 | } 267 | } 268 | } 269 | 270 | void Histogram::printJointFrequenciesForPixels(int p0, int p1) { 271 | for (int bin0 = 0; bin0 < bins; bin0++) { 272 | for (int bin1 = 0; bin1 < bins; bin1++) { 273 | int m = p0*bins*bins+p1*bins*bins*p_width*p_height; 274 | cout << joint_freq[m+bin0+bins*bin1] << ", "; 275 | } 276 | cout << endl; 277 | } 278 | } 279 | 280 | void Histogram::printJointFrequencies(char printmode) { 281 | switch(printmode) { 282 | case 0: { 283 | int w = bins*bins;//*p_width*p_height; 284 | int h = p_width*p_height*p_width*p_height; 285 | cout << "Width = " << w << " and height = " << h << endl; 286 | for (int i = 0; i < h; i++) { 287 | for (int j = 0; j < w; j++) { 288 | cout << joint_freq[i*w+j] << ", "; 289 | } 290 | cout << endl; 291 | } 292 | break; 293 | } 294 | case 1: { 295 | for (int bin0 = 0; bin0 < bins; bin0++) { 296 | for (int bin1 = 0; bin1 < bins; bin1++) { 297 | cout << "bin[" << bin0 << ", " << bin1 << "]: " << endl; 298 | printJointFrequencies(bin0, bin1); 299 | } 300 | } 301 | break; 302 | } 303 | case 2: { 304 | int w = p_width*p_height; 305 | int h = p_width*p_height; 306 | cout << "Width = " << w << " and height = " << h << " and #bins = " << bins << endl; 307 | for (int i = 0; i < h; i++) { 308 | for (int j = 0; j < w; j++) { 309 | cout << "pixel pair {" << i << ", " << j << "}" << endl; 310 | printJointFrequenciesForPixels(i, j); 311 | } 312 | } 313 | break; 314 | } 315 | default: cerr << "Huh?" << endl; 316 | break; 317 | } 318 | } 319 | 320 | /** 321 | * Print distances 322 | */ 323 | void Histogram::printDistances() { 324 | cout << "All distances:" << endl; 325 | for (int i0 = 0; i0 < p_width; i0++) { 326 | for (int j0 = 0; j0 < p_height; j0++) { 327 | for (int i1 = 0; i1 < p_width; i1++) { 328 | for (int j1 = 0; j1 < p_height; j1++) { 329 | int p0 = j0 * p_width + i0; 330 | int p1 = j1 * p_width + i1; 331 | cout << (int)dist[p0*p_width*p_height+p1] << ", "; 332 | } 333 | } 334 | cout << endl; 335 | } 336 | } 337 | } 338 | 339 | pDataMatrix Histogram::drawDistances() { 340 | pDataMatrix result = new DataValue[p_size*p_size]; 341 | for (int p0 = 0; p0 < p_size; ++p0) { 342 | for (int p1 = 0; p1 < p_size; ++p1) { 343 | Value d = getDistance(p0, p1); 344 | result[p0 * p_size + p1] = d; 345 | } 346 | } 347 | return result; 348 | } 349 | 350 | 351 | void Histogram::printBins(pDataMatrix data) { 352 | if (p_width == 0) return; 353 | for (int i = 0; i < p_width; i++) { 354 | for (int j = 0; j < p_height; j++) { 355 | int p = j * p_width + i; 356 | Value v = data[p]; 357 | int bin = (v * bins) / 256; 358 | cout << bin << " "; 359 | } 360 | cout << endl; 361 | } 362 | } 363 | 364 | #endif 365 | -------------------------------------------------------------------------------- /inc/ConfigFile.hpp: -------------------------------------------------------------------------------- 1 | // ConfigFile.h 2 | // Class for reading named values from configuration files 3 | // Richard J. Wagner v2.1 24 May 2004 wagnerr@umich.edu 4 | 5 | // Copyright (c) 2004 Richard J. Wagner 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy 8 | // of this software and associated documentation files (the "Software"), to 9 | // deal in the Software without restriction, including without limitation the 10 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | // sell copies of the Software, and to permit persons to whom the Software is 12 | // furnished to do so, subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in 15 | // all copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | // IN THE SOFTWARE. 24 | 25 | // Typical usage 26 | // ------------- 27 | // 28 | // Given a configuration file "settings.inp": 29 | // atoms = 25 30 | // length = 8.0 # nanometers 31 | // name = Reece Surcher 32 | // 33 | // Named values are read in various ways, with or without default values: 34 | // ConfigFile config( "settings.inp" ); 35 | // int atoms = config.read( "atoms" ); 36 | // double length = config.read( "length", 10.0 ); 37 | // string author, title; 38 | // config.readInto( author, "name" ); 39 | // config.readInto( title, "title", string("Untitled") ); 40 | // 41 | // See file example.cpp for more examples. 42 | 43 | #ifndef CONFIGFILE_HPP 44 | #define CONFIGFILE_HPP 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | using std::string; 53 | 54 | namespace dobots { 55 | 56 | class ConfigFile { 57 | // Data 58 | protected: 59 | string myDelimiter; // separator between key and value 60 | string myComment; // separator between value and comments 61 | string mySentry; // optional string to signal end of file 62 | std::map myContents; // extracted keys and values 63 | 64 | typedef std::map::iterator mapi; 65 | typedef std::map::const_iterator mapci; 66 | 67 | // Methods 68 | public: 69 | ConfigFile( string filename, 70 | string delimiter = "=", 71 | string comment = "#", 72 | string sentry = "EndConfigFile" ) 73 | : myDelimiter(delimiter), myComment(comment), mySentry(sentry) 74 | { 75 | // Construct a ConfigFile, getting keys and values from given file 76 | std::ifstream in( filename.c_str() ); 77 | if( !in ) throw file_not_found( filename ); 78 | in >> (*this); 79 | } 80 | 81 | ConfigFile() 82 | : myDelimiter( string(1,'=') ), myComment( string(1,'#') ) 83 | { 84 | // Construct a ConfigFile without a file; empty 85 | } 86 | 87 | // Search for key and read value or optional default value 88 | template T read( const string& key ) const; // call as read 89 | template T read( const string& key, const T& value ) const; 90 | template bool readInto( T& var, const string& key ) const; 91 | template 92 | bool readInto( T& var, const string& key, const T& value ) const; 93 | 94 | // Modify keys and values 95 | template void add( string key, const T& value ); 96 | 97 | void remove( const string& key ) 98 | { 99 | // Remove key and its value 100 | myContents.erase( myContents.find( key ) ); 101 | return; 102 | } 103 | 104 | 105 | // Check whether key exists in configuration 106 | bool keyExists( const string& key ) const 107 | { 108 | // Indicate whether key is found 109 | mapci p = myContents.find( key ); 110 | return ( p != myContents.end() ); 111 | } 112 | 113 | // Check or change configuration syntax 114 | string getDelimiter() const { return myDelimiter; } 115 | string getComment() const { return myComment; } 116 | string getSentry() const { return mySentry; } 117 | string setDelimiter( const string& s ) 118 | { string old = myDelimiter; myDelimiter = s; return old; } 119 | string setComment( const string& s ) 120 | { string old = myComment; myComment = s; return old; } 121 | 122 | // Write or read configuration 123 | friend std::ostream& operator<<( std::ostream& os, const ConfigFile& cf ); 124 | friend std::istream& operator>>( std::istream& is, ConfigFile& cf ); 125 | 126 | protected: 127 | template static string T_as_string( const T& t ); 128 | template static T string_as_T( const string& s ); 129 | 130 | static void trim( string& s ) { 131 | // Remove leading and trailing whitespace 132 | static const char whitespace[] = " \n\t\v\r\f"; 133 | s.erase( 0, s.find_first_not_of(whitespace) ); 134 | s.erase( s.find_last_not_of(whitespace) + 1U ); 135 | } 136 | 137 | 138 | // Exception types 139 | public: 140 | struct file_not_found { 141 | string filename; 142 | file_not_found( const string& filename_ = string() ) 143 | : filename(filename_) {} }; 144 | struct key_not_found { // thrown only by T read(key) variant of read() 145 | string key; 146 | key_not_found( const string& key_ = string() ) 147 | : key(key_) {} }; 148 | }; 149 | 150 | 151 | /* static */ 152 | template 153 | string ConfigFile::T_as_string( const T& t ) 154 | { 155 | // Convert from a T to a string 156 | // Type T must support << operator 157 | std::ostringstream ost; 158 | ost << t; 159 | return ost.str(); 160 | } 161 | 162 | 163 | /* static */ 164 | template 165 | T ConfigFile::string_as_T( const string& s ) 166 | { 167 | // Convert from a string to a T 168 | // Type T must support >> operator 169 | T t; 170 | std::istringstream ist(s); 171 | ist >> t; 172 | return t; 173 | } 174 | 175 | 176 | /* static */ 177 | template<> 178 | inline string ConfigFile::string_as_T( const string& s ) 179 | { 180 | // Convert from a string to a string 181 | // In other words, do nothing 182 | return s; 183 | } 184 | 185 | 186 | /* static */ 187 | template<> 188 | inline bool ConfigFile::string_as_T( const string& s ) 189 | { 190 | // Convert from a string to a bool 191 | // Interpret "false", "F", "no", "n", "0" as false 192 | // Interpret "true", "T", "yes", "y", "1", "-1", or anything else as true 193 | bool b = true; 194 | string sup = s; 195 | for( string::iterator p = sup.begin(); p != sup.end(); ++p ) 196 | *p = toupper(*p); // make string all caps 197 | if( sup==string("FALSE") || sup==string("F") || 198 | sup==string("NO") || sup==string("N") || 199 | sup==string("0") || sup==string("NONE") ) 200 | b = false; 201 | return b; 202 | } 203 | 204 | 205 | template 206 | T ConfigFile::read( const string& key ) const 207 | { 208 | // Read the value corresponding to key 209 | mapci p = myContents.find(key); 210 | if( p == myContents.end() ) throw key_not_found(key); 211 | return string_as_T( p->second ); 212 | } 213 | 214 | 215 | template 216 | T ConfigFile::read( const string& key, const T& value ) const 217 | { 218 | // Return the value corresponding to key or given default value 219 | // if key is not found 220 | mapci p = myContents.find(key); 221 | if( p == myContents.end() ) return value; 222 | return string_as_T( p->second ); 223 | } 224 | 225 | 226 | template 227 | bool ConfigFile::readInto( T& var, const string& key ) const 228 | { 229 | // Get the value corresponding to key and store in var 230 | // Return true if key is found 231 | // Otherwise leave var untouched 232 | mapci p = myContents.find(key); 233 | bool found = ( p != myContents.end() ); 234 | if( found ) var = string_as_T( p->second ); 235 | return found; 236 | } 237 | 238 | 239 | template 240 | bool ConfigFile::readInto( T& var, const string& key, const T& value ) const 241 | { 242 | // Get the value corresponding to key and store in var 243 | // Return true if key is found 244 | // Otherwise set var to given default 245 | mapci p = myContents.find(key); 246 | bool found = ( p != myContents.end() ); 247 | if( found ) 248 | var = string_as_T( p->second ); 249 | else 250 | var = value; 251 | return found; 252 | } 253 | 254 | 255 | template 256 | void ConfigFile::add( string key, const T& value ) 257 | { 258 | // Add a key with given value 259 | string v = T_as_string( value ); 260 | trim(key); 261 | trim(v); 262 | myContents[key] = v; 263 | return; 264 | } 265 | 266 | std::ostream& operator<<( std::ostream& os, const ConfigFile& cf ) 267 | { 268 | // Save a ConfigFile to os 269 | for( ConfigFile::mapci p = cf.myContents.begin(); 270 | p != cf.myContents.end(); 271 | ++p ) 272 | { 273 | os << p->first << " " << cf.myDelimiter << " "; 274 | os << p->second << std::endl; 275 | } 276 | return os; 277 | } 278 | 279 | 280 | std::istream& operator>>( std::istream& is, ConfigFile& cf ) 281 | { 282 | // Load a ConfigFile from is 283 | // Read in keys and values, keeping internal whitespace 284 | typedef string::size_type pos; 285 | const string& delim = cf.myDelimiter; // separator 286 | const string& comm = cf.myComment; // comment 287 | const string& sentry = cf.mySentry; // end of file sentry 288 | const pos skip = delim.length(); // length of separator 289 | 290 | string nextline = ""; // might need to read ahead to see where value ends 291 | 292 | while( is || nextline.length() > 0 ) 293 | { 294 | // Read an entire line at a time 295 | string line; 296 | if( nextline.length() > 0 ) 297 | { 298 | line = nextline; // we read ahead; use it now 299 | nextline = ""; 300 | } 301 | else 302 | { 303 | std::getline( is, line ); 304 | } 305 | 306 | // Ignore comments 307 | line = line.substr( 0, line.find(comm) ); 308 | 309 | // Check for end of file sentry 310 | if( sentry != "" && line.find(sentry) != string::npos ) return is; 311 | 312 | // Parse the line if it contains a delimiter 313 | pos delimPos = line.find( delim ); 314 | if( delimPos < string::npos ) 315 | { 316 | // Extract the key 317 | string key = line.substr( 0, delimPos ); 318 | line.replace( 0, delimPos+skip, "" ); 319 | 320 | // See if value continues on the next line 321 | // Stop at blank line, next line with a key, end of stream, 322 | // or end of file sentry 323 | bool terminate = false; 324 | while( !terminate && is ) 325 | { 326 | std::getline( is, nextline ); 327 | terminate = true; 328 | 329 | string nlcopy = nextline; 330 | ConfigFile::trim(nlcopy); 331 | if( nlcopy == "" ) continue; 332 | 333 | nextline = nextline.substr( 0, nextline.find(comm) ); 334 | if( nextline.find(delim) != string::npos ) 335 | continue; 336 | if( sentry != "" && nextline.find(sentry) != string::npos ) 337 | continue; 338 | 339 | nlcopy = nextline; 340 | ConfigFile::trim(nlcopy); 341 | if( nlcopy != "" ) line += "\n"; 342 | line += nextline; 343 | terminate = false; 344 | } 345 | 346 | // Store key and value 347 | ConfigFile::trim(key); 348 | ConfigFile::trim(line); 349 | cf.myContents[key] = line; // overwrites if key is repeated 350 | } 351 | } 352 | 353 | return is; 354 | } 355 | 356 | } 357 | 358 | #endif // CONFIGFILE_HPP 359 | 360 | // Release notes: 361 | // v1.0 21 May 1999 362 | // + First release 363 | // + Template read() access only through non-member readConfigFile() 364 | // + ConfigurationFileBool is only built-in helper class 365 | // 366 | // v2.0 3 May 2002 367 | // + Shortened name from ConfigurationFile to ConfigFile 368 | // + Implemented template member functions 369 | // + Changed default comment separator from % to # 370 | // + Enabled reading of multiple-line values 371 | // 372 | // v2.1 24 May 2004 373 | // + Made template specializations inline to avoid compiler-dependent linkage 374 | // + Allowed comments within multiple-line values 375 | // + Enabled blank line termination for multiple-line values 376 | // + Added optional sentry to detect end of configuration file 377 | // + Rewrote messy trimWhitespace() function as elegant trim() 378 | // 379 | // v2.2 15 Aug 2012 380 | // + Moved everything into one .hpp file (Anne C. van Rossum) 381 | 382 | -------------------------------------------------------------------------------- /src/PositionParticleFilter.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief 3 | * @file PositionParticleFilter.cpp 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 14, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | #include 24 | 25 | using namespace dobots; 26 | 27 | #include 28 | 29 | /* ************************************************************************************** 30 | * Implementation of PositionParticleFilter 31 | * **************************************************************************************/ 32 | 33 | PositionParticleFilter::PositionParticleFilter() { 34 | bins = 16; 35 | seed = 234789; 36 | auto_coeff.clear(); 37 | auto_coeff.push_back(2.0); 38 | auto_coeff.push_back(-1.0); 39 | srand48(seed); 40 | img = NULL; 41 | } 42 | 43 | PositionParticleFilter::~PositionParticleFilter() { 44 | 45 | } 46 | 47 | /** 48 | * Do every action that is necessary to update all particles. This takes three steps: 49 | * - transition according to a certain motion model 50 | * - observing the likelihood of the object being at the translated position (results in a weight) 51 | * - resample according to that likelihood (given by the weight) 52 | * @param img_frame the image with the entitie(s) to be tracked 53 | * @param subticks the number of times this same image needs to be used 54 | */ 55 | void PositionParticleFilter::Tick(CImg *img_frame, int subticks) { 56 | img = img_frame; 57 | assert (subticks > 0); 58 | for (int i = 0; i < subticks; ++i) { 59 | cout << "Transition all particles" << endl; 60 | Transition(); 61 | cout << "Likelihood for all particles" << endl; 62 | Likelihood(); 63 | cout << "Resample all particles" << endl; 64 | Resample(); 65 | } 66 | } 67 | 68 | /** 69 | * Initialise the particle filter. 70 | */ 71 | void PositionParticleFilter::Init(NormalizedHistogramValues &tracked_object_histogram, 72 | CImg &coord, int particle_count) { 73 | 74 | getParticles().clear(); 75 | 76 | int width = coord(3) - coord(0); 77 | int height = coord(4) - coord(1); 78 | 79 | cout << "Width*height=" << width << '*' << height << endl; 80 | 81 | this->tracked_object_histogram = tracked_object_histogram; 82 | 83 | // generate duplicates of particles 84 | for (int i = 0; i < particle_count; ++i) { 85 | 86 | ParticleState *s = new ParticleState(i+1); 87 | s->width = width; 88 | s->height = height; 89 | s->x.clear(); 90 | s->y.clear(); 91 | int history_size = 2; 92 | for (int j = 0; j < history_size; ++j) { 93 | s->x.push_back(coord(0) + width / 2); 94 | s->y.push_back(coord(1) + height / 2); 95 | s->scale.push_back(1); 96 | } 97 | Particle *p = new Particle(s, 0); 98 | // s. scale/ histogram 99 | getParticles().push_back(p); 100 | } 101 | 102 | ASSERT_EQUAL(getParticles().size(), particle_count); 103 | } 104 | 105 | /** 106 | * Transition of all particles following a certain motion model. Due to the fact there is 107 | * no multiplication or removal in the number of particles at this moment, we can safely 108 | * iterate through the entire container and do the transition "in place". 109 | */ 110 | void PositionParticleFilter::Transition() { 111 | std::vector* >::iterator i; 112 | for (i = getParticles().begin(); i != getParticles().end(); ++i) { 113 | ParticleState *state = (*i)->getState(); 114 | assert (state != NULL); 115 | Transition(*state); 116 | } 117 | 118 | // ASSERT_EQUAL(getParticles().size(), particle_count); 119 | } 120 | 121 | void PositionParticleFilter::Likelihood() { 122 | std::vector* >::iterator i; 123 | for (i = getParticles().begin(); i != getParticles().end(); ++i) { 124 | ParticleState *state = (*i)->getState(); 125 | assert (state != NULL); 126 | state->likelihood = Likelihood(*state); 127 | (*i)->setWeight(state->likelihood); 128 | } 129 | 130 | // log for the user 131 | std::sort(getParticles().begin(), getParticles().end(), comp_particles); 132 | int max = 10; 133 | cout << "Likelihoods: "; 134 | for (i = getParticles().begin(); i != getParticles().end(); ++i) { 135 | ParticleState *state = (*i)->getState(); 136 | cout << '[' << state->getId() << ':' << state->likelihood << "] "; 137 | if (--max == 0) break; 138 | } 139 | cout << endl; 140 | 141 | // ASSERT_EQUAL(getParticles().size(), particle_count); 142 | } 143 | 144 | /** 145 | * Return the particle coordinates for display. 146 | */ 147 | void PositionParticleFilter::GetParticleCoordinates(std::vector *> & coordinates) { 148 | // sort to make sure the one with highest weight comes first 149 | std::sort(getParticles().begin(), getParticles().end(), comp_particles); 150 | 151 | std::vector* >::iterator i; 152 | for (i = getParticles().begin(); i != getParticles().end(); ++i) { 153 | CImg *coord = new CImg(6); 154 | ParticleState *state = (*i)->getState(); 155 | assert (state != NULL); 156 | assert (state->getId()); 157 | std::string msg; msg = "state is empty " + state->getId(); 158 | if(state->x.empty()) cout << msg << endl; 159 | assert (!state->x.empty()); 160 | assert (!state->y.empty()); 161 | assert (!state->scale.empty()); 162 | float x = state->x.front(); 163 | float y = state->y.front(); 164 | float scale = state->scale.front(); 165 | float width = state->width * scale; 166 | float height = state->height * scale; 167 | coord->_data[0] = x-width/2; 168 | coord->_data[1] = y-height/2; 169 | coord->_data[3] = x+width/2; 170 | coord->_data[4] = y+height/2; 171 | coordinates.push_back(coord); 172 | } 173 | } 174 | 175 | /** 176 | * Normal state of affairs is to use an autoregressive model to estimate where an 177 | * object will be next. There are however many different autoregressive models in use, 178 | * and it doesn't seem there is a "standard" way...: 179 | *
    180 | *
  • Robust Visual Tracking for Multiple Targets (Cai, Freitas, Little): 181 | * x[n] = A x[n-1] + B x[n-2] + C N(0,1)
    • 182 | *
  • CamShift Guided Particle Filter for Visual Tracking (Wang, Yang, Xy, Yu): 183 | * x[n] = 2*x[n-1] - x[n-2] + N(0,1)
    • 184 | *
  • Real-time Hand Tracking using a Mean Shift Embedded Particle Filter (Shan, 185 | * Tan, Wei): 186 | * (x[n] - x[n-1]) = (x[n-1] - x[n-2]) + N(0,1)
    • 187 | *
  • PFCHA: A New Moving Object Tracking Algorithms Based on Particle Filter 188 | * and Histogram (Qi, JiaFu): 189 | * (x[n] - x_avg) = A (x[n-1] - x_avg) + B (x[n-2] - x_avg) + N(0,1)
    • 190 | *
191 | * Obviously, the second and third are the same. To have a second-order autoregressive 192 | * process "over differences" it is for example also possible to use: 193 | * x[n+1] = x[n] + A (x[n] - x[n-1]) + B (x[n-1] - x[n-2]) + N(0,1) 194 | * etcetera, etcetera. 195 | * I think it's smart to consider a formulation where x[n], x[n-1], ... can be used 196 | * directly, so we do not need to store differences in our vector for "predict", hence 197 | * I opt for the second formulation (auto_coeff[0] = 2, auto_coeff[1] = -1). 198 | * 199 | */ 200 | void PositionParticleFilter::Transition(ParticleState &oldp) { 201 | 202 | //#define OVERWRITE 203 | 204 | int xn = dobots::predict(oldp.x.begin(), oldp.x.end(), auto_coeff.begin(), 0.0, 1.0); 205 | int yn = dobots::predict(oldp.y.begin(), oldp.y.end(), auto_coeff.begin(), 0.0, 1.0); 206 | Value scale = dobots::predict(oldp.scale.begin(), oldp.scale.end(), auto_coeff.begin(), 0.0, 0.001); 207 | 208 | xn = std::max(0, std::min((int)img->_width-1, xn)); 209 | yn = std::max(0, std::min((int)img->_height-1, yn)); 210 | scale = std::max(0.1, scale); // scale should not fall below 0.1.. 211 | 212 | #ifdef OVERWRITE 213 | xn = oldp.x[0]; 214 | yn = oldp.y[0]; 215 | scale = oldp.scale[0]; 216 | 217 | // static boost::mt19937 random_number_generator(autoregression_seed); 218 | // boost::normal_distribution<> normal_dist(0.0, 1); 219 | // boost::variate_generator > epsilon( 220 | // random_number_generator, normal_dist); 221 | 222 | // float p = drand48(); 223 | 224 | xn += (drand48() * 4 - 2); //epsilon(); 225 | yn += (drand48() * 4 - 2); //epsilon(); 226 | 227 | #endif 228 | scale = 1.0; 229 | 230 | dobots::pushpop(oldp.x.begin(), oldp.x.end(), xn); 231 | dobots::pushpop(oldp.y.begin(), oldp.y.end(), yn); 232 | dobots::pushpop(oldp.scale.begin(), oldp.scale.end(), scale); 233 | 234 | // cout << "Transition particle " << oldp << endl; 235 | } 236 | 237 | /** 238 | * The likelihood of a player at all locations in the image using a given region size. The result is 239 | * written back in the form of a picture with colour values. 240 | * This function takes VERY long if every pixel would have been considered, hence, a block_size 241 | * parameter exists which only calculates the probability around blocks of this given size. 242 | * @param result image with red colour indicating the likelihood, the more black, the likelier 243 | * @param region_size the rectangle over which to match the histograms 244 | * @param block_size (optional) parameter for subsampling 245 | * @return void 246 | */ 247 | void PositionParticleFilter::GetLikelihoods(CImg & result, RegionSize region_size, int block_size) { 248 | assert (img != NULL); 249 | 250 | cout << "Clean entire picture" << endl; 251 | for (int i = 0; i < result._width; ++i) { 252 | for (int j = 0; j < result._height; ++j) { 253 | const DataValue color[] = { 255,255,255 }; 254 | result.draw_point(i,j,color); 255 | } 256 | } 257 | cout << "Calculate likelihood for all pixels (except at distance \"width\" from border)" << endl; 258 | cout << " this ranges from " << region_size.width << " to " << result._width-region_size.width << " and "; 259 | cout << "from " << region_size.height << " to " << result._height-region_size.height << endl; 260 | 261 | ParticleState state; 262 | state.height = region_size.height; 263 | state.width = region_size.width; 264 | 265 | bool show_progress = true; 266 | if (show_progress) cout << endl; 267 | for (int j = region_size.height; j < result._height-region_size.height; j=j+block_size) { 268 | if (show_progress) if (!(j%10)) cout << j << ':' << ' '; 269 | for (int i = region_size.width; i < result._width-region_size.width; i=i+block_size) { 270 | state.x.clear(); 271 | state.y.clear(); 272 | state.x.push_back(i); 273 | state.y.push_back(j); 274 | float value = Likelihood(state); 275 | DataValue val = value*255; 276 | if (show_progress) if (!(j%10) && !(i%10)) cout << (int)val << ' '; 277 | const DataValue color[] = { val,0,0 }; 278 | result.draw_rectangle(i-block_size/2,j-block_size/2,i+block_size/2,j+block_size/2,color); 279 | if (show_progress) if (!(j%10) && !(i%10)) cout << '+'; 280 | } 281 | if (show_progress) if (!(j%10)) cout << endl; 282 | } 283 | } 284 | 285 | /** 286 | * Calculate the likelihood of a player and the state indicated by the parameter 287 | * "state" which contains an x and y position, a width and a height. This is used 288 | * to define a rectangle for which a histogram is matched against the reference 289 | * histogram of the object that is tracked. 290 | * @param state the state of the particle (position, width, height) 291 | * @return conceptual "distance" to the reference (tracked) object 292 | */ 293 | float PositionParticleFilter::Likelihood(ParticleState & state) { 294 | assert (img != NULL); 295 | CImg coord(6); 296 | float scale = state.scale.front(); 297 | scale = 1; 298 | coord._data[0] = state.x[0] - scale * state.width/2; 299 | coord._data[1] = state.y[0] - scale * state.height/2; 300 | coord._data[3] = state.x[0] + scale * state.width/2; 301 | coord._data[4] = state.y[0] + scale * state.height/2; 302 | CImg img_selection = img->get_crop(coord._data[0], coord._data[1], coord._data[3], coord._data[4]); 303 | DataFrames frames; 304 | frames.clear(); 305 | pDataMatrix data = img_selection._data; 306 | frames.push_back(data); 307 | 308 | Histogram histogram(bins, img_selection._width, img_selection._height); 309 | #ifdef VERBOSE 310 | cout << __func__ << ": Add data for histograms" << endl; 311 | #endif 312 | assert (frames.size() == 1); 313 | histogram.calcProbabilities(frames); 314 | 315 | #ifdef VERBOSE 316 | cout << __func__ << ": Get normalized probabilities" << endl; 317 | #endif 318 | NormalizedHistogramValues result; 319 | histogram.getProbabilities(result); 320 | 321 | #ifdef VERBOSE 322 | cout << __func__ << ": Calculate distance to histogram of the to-be-tracked object" << endl; 323 | #endif 324 | 325 | Value dist = dobots::distance(tracked_object_histogram.begin(), tracked_object_histogram.end(), result.begin(), result.end(), 326 | dobots::DM_SQUARED_HELLINGER); 327 | return std::exp(-20.0 * dist); 328 | } 329 | 330 | -------------------------------------------------------------------------------- /inc/alphanum.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ALPHANUM__HPP 2 | #define ALPHANUM__HPP 3 | 4 | /* 5 | The Alphanum Algorithm is an improved sorting algorithm for strings 6 | containing numbers. Instead of sorting numbers in ASCII order like a 7 | standard sort, this algorithm sorts numbers in numeric order. 8 | 9 | The Alphanum Algorithm is discussed at http://www.DaveKoelle.com 10 | 11 | This implementation is Copyright (c) 2008 Dirk Jagdmann . 12 | It is a cleanroom implementation of the algorithm and not derived by 13 | other's works. In contrast to the versions written by Dave Koelle this 14 | source code is distributed with the libpng/zlib license. 15 | 16 | This software is provided 'as-is', without any express or implied 17 | warranty. In no event will the authors be held liable for any damages 18 | arising from the use of this software. 19 | 20 | Permission is granted to anyone to use this software for any purpose, 21 | including commercial applications, and to alter it and redistribute it 22 | freely, subject to the following restrictions: 23 | 24 | 1. The origin of this software must not be misrepresented; you 25 | must not claim that you wrote the original software. If you use 26 | this software in a product, an acknowledgment in the product 27 | documentation would be appreciated but is not required. 28 | 29 | 2. Altered source versions must be plainly marked as such, and 30 | must not be misrepresented as being the original software. 31 | 32 | 3. This notice may not be removed or altered from any source 33 | distribution. */ 34 | 35 | /* $Header: /code/doj/alphanum.hpp,v 1.3 2008/01/28 23:06:47 doj Exp $ */ 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #ifdef ALPHANUM_LOCALE 43 | #include 44 | #endif 45 | 46 | #ifdef DOJDEBUG 47 | #include 48 | #include 49 | #endif 50 | 51 | // TODO: make comparison with hexadecimal numbers. Extend the alphanum_comp() function by traits to choose between decimal and hexadecimal. 52 | 53 | namespace doj 54 | { 55 | 56 | // anonymous namespace for functions we use internally. But if you 57 | // are coding in C, you can use alphanum_impl() directly, since it 58 | // uses not C++ features. 59 | namespace { 60 | 61 | // if you want to honour the locale settings for detecting digit 62 | // characters, you should define ALPHANUM_LOCALE 63 | #ifdef ALPHANUM_LOCALE 64 | /** wrapper function for ::isdigit() */ 65 | bool alphanum_isdigit(int c) 66 | { 67 | return isdigit(c); 68 | } 69 | #else 70 | /** this function does not consider the current locale and only 71 | works with ASCII digits. 72 | @return true if c is a digit character 73 | */ 74 | bool alphanum_isdigit(const char c) 75 | { 76 | return c>='0' && c<='9'; 77 | } 78 | #endif 79 | 80 | /** 81 | compare l and r with strcmp() semantics, but using 82 | the "Alphanum Algorithm". This function is designed to read 83 | through the l and r strings only one time, for 84 | maximum performance. It does not allocate memory for 85 | substrings. It can either use the C-library functions isdigit() 86 | and atoi() to honour your locale settings, when recognizing 87 | digit characters when you "#define ALPHANUM_LOCALE=1" or use 88 | it's own digit character handling which only works with ASCII 89 | digit characters, but provides better performance. 90 | 91 | @param l NULL-terminated C-style string 92 | @param r NULL-terminated C-style string 93 | @return negative if lr 94 | */ 95 | int alphanum_impl(const char *l, const char *r) 96 | { 97 | enum mode_t { STRING, NUMBER } mode=STRING; 98 | 99 | while(*l && *r) 100 | { 101 | if(mode == STRING) 102 | { 103 | char l_char, r_char; 104 | while((l_char=*l) && (r_char=*r)) 105 | { 106 | // check if this are digit characters 107 | const bool l_digit=alphanum_isdigit(l_char), r_digit=alphanum_isdigit(r_char); 108 | // if both characters are digits, we continue in NUMBER mode 109 | if(l_digit && r_digit) 110 | { 111 | mode=NUMBER; 112 | break; 113 | } 114 | // if only the left character is a digit, we have a result 115 | if(l_digit) return -1; 116 | // if only the right character is a digit, we have a result 117 | if(r_digit) return +1; 118 | // compute the difference of both characters 119 | const int diff=l_char - r_char; 120 | // if they differ we have a result 121 | if(diff != 0) return diff; 122 | // otherwise process the next characters 123 | ++l; 124 | ++r; 125 | } 126 | } 127 | else // mode==NUMBER 128 | { 129 | #ifdef ALPHANUM_LOCALE 130 | // get the left number 131 | char *end; 132 | unsigned long l_int=strtoul(l, &end, 0); 133 | l=end; 134 | 135 | // get the right number 136 | unsigned long r_int=strtoul(r, &end, 0); 137 | r=end; 138 | #else 139 | // get the left number 140 | unsigned long l_int=0; 141 | while(*l && alphanum_isdigit(*l)) 142 | { 143 | // TODO: this can overflow 144 | l_int=l_int*10 + *l-'0'; 145 | ++l; 146 | } 147 | 148 | // get the right number 149 | unsigned long r_int=0; 150 | while(*r && alphanum_isdigit(*r)) 151 | { 152 | // TODO: this can overflow 153 | r_int=r_int*10 + *r-'0'; 154 | ++r; 155 | } 156 | #endif 157 | 158 | // if the difference is not equal to zero, we have a comparison result 159 | const long diff=l_int-r_int; 160 | if(diff != 0) 161 | return diff; 162 | 163 | // otherwise we process the next substring in STRING mode 164 | mode=STRING; 165 | } 166 | } 167 | 168 | if(*r) return -1; 169 | if(*l) return +1; 170 | return 0; 171 | } 172 | 173 | } 174 | 175 | /** 176 | Compare left and right with the same semantics as strcmp(), but with the 177 | "Alphanum Algorithm" which produces more human-friendly 178 | results. The classes lT and rT must implement "std::ostream 179 | operator<< (std::ostream&, const Ty&)". 180 | 181 | @return negative if leftright. 182 | */ 183 | template 184 | int alphanum_comp(const lT& left, const rT& right) 185 | { 186 | #ifdef DOJDEBUG 187 | std::clog << "alphanum_comp<" << typeid(left).name() << "," << typeid(right).name() << "> " << left << "," << right << std::endl; 188 | #endif 189 | std::ostringstream l; l << left; 190 | std::ostringstream r; r << right; 191 | return alphanum_impl(l.str().c_str(), r.str().c_str()); 192 | } 193 | 194 | /** 195 | Compare l and r with the same semantics as strcmp(), but with 196 | the "Alphanum Algorithm" which produces more human-friendly 197 | results. 198 | 199 | @return negative if lr. 200 | */ 201 | template <> 202 | int alphanum_comp(const std::string& l, const std::string& r) 203 | { 204 | #ifdef DOJDEBUG 205 | std::clog << "alphanum_comp " << l << "," << r << std::endl; 206 | #endif 207 | return alphanum_impl(l.c_str(), r.c_str()); 208 | } 209 | 210 | //////////////////////////////////////////////////////////////////////////// 211 | 212 | // now follow a lot of overloaded alphanum_comp() functions to get a 213 | // direct call to alphanum_impl() upon the various combinations of c 214 | // and c++ strings. 215 | 216 | /** 217 | Compare l and r with the same semantics as strcmp(), but with 218 | the "Alphanum Algorithm" which produces more human-friendly 219 | results. 220 | 221 | @return negative if lr. 222 | */ 223 | int alphanum_comp(char* l, char* r) 224 | { 225 | assert(l); 226 | assert(r); 227 | #ifdef DOJDEBUG 228 | std::clog << "alphanum_comp " << l << "," << r << std::endl; 229 | #endif 230 | return alphanum_impl(l, r); 231 | } 232 | 233 | int alphanum_comp(const char* l, const char* r) 234 | { 235 | assert(l); 236 | assert(r); 237 | #ifdef DOJDEBUG 238 | std::clog << "alphanum_comp " << l << "," << r << std::endl; 239 | #endif 240 | return alphanum_impl(l, r); 241 | } 242 | 243 | int alphanum_comp(char* l, const char* r) 244 | { 245 | assert(l); 246 | assert(r); 247 | #ifdef DOJDEBUG 248 | std::clog << "alphanum_comp " << l << "," << r << std::endl; 249 | #endif 250 | return alphanum_impl(l, r); 251 | } 252 | 253 | int alphanum_comp(const char* l, char* r) 254 | { 255 | assert(l); 256 | assert(r); 257 | #ifdef DOJDEBUG 258 | std::clog << "alphanum_comp " << l << "," << r << std::endl; 259 | #endif 260 | return alphanum_impl(l, r); 261 | } 262 | 263 | int alphanum_comp(const std::string& l, char* r) 264 | { 265 | assert(r); 266 | #ifdef DOJDEBUG 267 | std::clog << "alphanum_comp " << l << "," << r << std::endl; 268 | #endif 269 | return alphanum_impl(l.c_str(), r); 270 | } 271 | 272 | int alphanum_comp(char* l, const std::string& r) 273 | { 274 | assert(l); 275 | #ifdef DOJDEBUG 276 | std::clog << "alphanum_comp " << l << "," << r << std::endl; 277 | #endif 278 | return alphanum_impl(l, r.c_str()); 279 | } 280 | 281 | int alphanum_comp(const std::string& l, const char* r) 282 | { 283 | assert(r); 284 | #ifdef DOJDEBUG 285 | std::clog << "alphanum_comp " << l << "," << r << std::endl; 286 | #endif 287 | return alphanum_impl(l.c_str(), r); 288 | } 289 | 290 | int alphanum_comp(const char* l, const std::string& r) 291 | { 292 | assert(l); 293 | #ifdef DOJDEBUG 294 | std::clog << "alphanum_comp " << l << "," << r << std::endl; 295 | #endif 296 | return alphanum_impl(l, r.c_str()); 297 | } 298 | 299 | //////////////////////////////////////////////////////////////////////////// 300 | 301 | /** 302 | Functor class to compare two objects with the "Alphanum 303 | Algorithm". If the objects are no std::string, they must 304 | implement "std::ostream operator<< (std::ostream&, const Ty&)". 305 | */ 306 | template 307 | struct alphanum_less : public std::binary_function 308 | { 309 | bool operator()(const Ty& left, const Ty& right) const 310 | { 311 | return alphanum_comp(left, right) < 0; 312 | } 313 | }; 314 | 315 | } 316 | 317 | #ifdef TESTMAIN 318 | #include 319 | #include 320 | #include 321 | #include 322 | #include 323 | #include 324 | int main() 325 | { 326 | // testcases for the algorithm 327 | assert(doj::alphanum_comp("","") == 0); 328 | assert(doj::alphanum_comp("","a") < 0); 329 | assert(doj::alphanum_comp("a","") > 0); 330 | assert(doj::alphanum_comp("a","a") == 0); 331 | assert(doj::alphanum_comp("","9") < 0); 332 | assert(doj::alphanum_comp("9","") > 0); 333 | assert(doj::alphanum_comp("1","1") == 0); 334 | assert(doj::alphanum_comp("1","2") < 0); 335 | assert(doj::alphanum_comp("3","2") > 0); 336 | assert(doj::alphanum_comp("a1","a1") == 0); 337 | assert(doj::alphanum_comp("a1","a2") < 0); 338 | assert(doj::alphanum_comp("a2","a1") > 0); 339 | assert(doj::alphanum_comp("a1a2","a1a3") < 0); 340 | assert(doj::alphanum_comp("a1a2","a1a0") > 0); 341 | assert(doj::alphanum_comp("134","122") > 0); 342 | assert(doj::alphanum_comp("12a3","12a3") == 0); 343 | assert(doj::alphanum_comp("12a1","12a0") > 0); 344 | assert(doj::alphanum_comp("12a1","12a2") < 0); 345 | assert(doj::alphanum_comp("a","aa") < 0); 346 | assert(doj::alphanum_comp("aaa","aa") > 0); 347 | assert(doj::alphanum_comp("Alpha 2","Alpha 2") == 0); 348 | assert(doj::alphanum_comp("Alpha 2","Alpha 2A") < 0); 349 | assert(doj::alphanum_comp("Alpha 2 B","Alpha 2") > 0); 350 | 351 | assert(doj::alphanum_comp(1,1) == 0); 352 | assert(doj::alphanum_comp(1,2) < 0); 353 | assert(doj::alphanum_comp(2,1) > 0); 354 | assert(doj::alphanum_comp(1.2,3.14) < 0); 355 | assert(doj::alphanum_comp(3.14,2.71) > 0); 356 | assert(doj::alphanum_comp(true,true) == 0); 357 | assert(doj::alphanum_comp(true,false) > 0); 358 | assert(doj::alphanum_comp(false,true) < 0); 359 | 360 | std::string str("Alpha 2"); 361 | assert(doj::alphanum_comp(str,"Alpha 2") == 0); 362 | assert(doj::alphanum_comp(str,"Alpha 2A") < 0); 363 | assert(doj::alphanum_comp("Alpha 2 B",str) > 0); 364 | 365 | assert(doj::alphanum_comp(str,strdup("Alpha 2")) == 0); 366 | assert(doj::alphanum_comp(str,strdup("Alpha 2A")) < 0); 367 | assert(doj::alphanum_comp(strdup("Alpha 2 B"),str) > 0); 368 | 369 | #if 1 370 | // show usage of the comparison functor with a set 371 | std::set > s; 372 | s.insert("Xiph Xlater 58"); 373 | s.insert("Xiph Xlater 5000"); 374 | s.insert("Xiph Xlater 500"); 375 | s.insert("Xiph Xlater 50"); 376 | s.insert("Xiph Xlater 5"); 377 | s.insert("Xiph Xlater 40"); 378 | s.insert("Xiph Xlater 300"); 379 | s.insert("Xiph Xlater 2000"); 380 | s.insert("Xiph Xlater 10000"); 381 | s.insert("QRS-62F Intrinsia Machine"); 382 | s.insert("QRS-62 Intrinsia Machine"); 383 | s.insert("QRS-60F Intrinsia Machine"); 384 | s.insert("QRS-60 Intrinsia Machine"); 385 | s.insert("Callisto Morphamax 7000 SE2"); 386 | s.insert("Callisto Morphamax 7000 SE"); 387 | s.insert("Callisto Morphamax 7000"); 388 | s.insert("Callisto Morphamax 700"); 389 | s.insert("Callisto Morphamax 600"); 390 | s.insert("Callisto Morphamax 5000"); 391 | s.insert("Callisto Morphamax 500"); 392 | s.insert("Callisto Morphamax"); 393 | s.insert("Alpha 2A-900"); 394 | s.insert("Alpha 2A-8000"); 395 | s.insert("Alpha 2A"); 396 | s.insert("Alpha 200"); 397 | s.insert("Alpha 2"); 398 | s.insert("Alpha 100"); 399 | s.insert("Allegia 60 Clasteron"); 400 | s.insert("Allegia 52 Clasteron"); 401 | s.insert("Allegia 51B Clasteron"); 402 | s.insert("Allegia 51 Clasteron"); 403 | s.insert("Allegia 500 Clasteron"); 404 | s.insert("Allegia 50 Clasteron"); 405 | s.insert("40X Radonius"); 406 | s.insert("30X Radonius"); 407 | s.insert("20X Radonius Prime"); 408 | s.insert("20X Radonius"); 409 | s.insert("200X Radonius"); 410 | s.insert("10X Radonius"); 411 | s.insert("1000X Radonius Maximus"); 412 | // print sorted set to cout 413 | std::copy(s.begin(), s.end(), std::ostream_iterator(std::cout, "\n")); 414 | 415 | // show usage of comparision functor with a map 416 | typedef std::map > m_t; 417 | m_t m; 418 | m["z1.doc"]=1; 419 | m["z10.doc"]=2; 420 | m["z100.doc"]=3; 421 | m["z101.doc"]=4; 422 | m["z102.doc"]=5; 423 | m["z11.doc"]=6; 424 | m["z12.doc"]=7; 425 | m["z13.doc"]=8; 426 | m["z14.doc"]=9; 427 | m["z15.doc"]=10; 428 | m["z16.doc"]=11; 429 | m["z17.doc"]=12; 430 | m["z18.doc"]=13; 431 | m["z19.doc"]=14; 432 | m["z2.doc"]=15; 433 | m["z20.doc"]=16; 434 | m["z3.doc"]=17; 435 | m["z4.doc"]=18; 436 | m["z5.doc"]=19; 437 | m["z6.doc"]=20; 438 | m["z7.doc"]=21; 439 | m["z8.doc"]=22; 440 | m["z9.doc"]=23; 441 | // print sorted map to cout 442 | for(m_t::iterator i=m.begin(); i!=m.end(); ++i) 443 | std::cout << i->first << '\t' << i->second << std::endl; 444 | 445 | // show usage of comparison functor with an STL algorithm on a vector 446 | std::vector v; 447 | // vector contents are reversed sorted contents of the old set 448 | std::copy(s.rbegin(), s.rend(), std::back_inserter(v)); 449 | // now sort the vector with the algorithm 450 | std::sort(v.begin(), v.end(), doj::alphanum_less()); 451 | // and print the vector to cout 452 | std::copy(v.begin(), v.end(), std::ostream_iterator(std::cout, "\n")); 453 | #endif 454 | 455 | return 0; 456 | } 457 | #endif 458 | 459 | #endif 460 | -------------------------------------------------------------------------------- /inc/Container.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Helper functions for C++ std containers 3 | * @file Container.hpp 4 | * 5 | * This file is created at Almende B.V. It is open-source software and part of the Common 6 | * Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools, ranging from 7 | * thread pools and TCP/IP components to control architectures and learning algorithms. 8 | * This software is published under the GNU Lesser General Public license (LGPL). 9 | * 10 | * It is not possible to add usage restrictions to an open-source license. Nevertheless, 11 | * we personally strongly object to this software being used by the military, in factory 12 | * farming, for animal experimentation, or anything that violates the Universal 13 | * Declaration of Human Rights. 14 | * 15 | * Copyright © 2012 Anne van Rossum 16 | * 17 | * @author Anne C. van Rossum 18 | * @date Aug 8, 2012 19 | * @project Replicator FP7 20 | * @company Almende B.V. 21 | * @case modular robotics / sensor fusion 22 | */ 23 | 24 | #ifndef CONTAINER_H_ 25 | #define CONTAINER_H_ 26 | 27 | #define _GLIBCXX_CONCEPT_CHECKS 28 | 29 | // General files 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | /**************************************************************************************************** 39 | * Helper functions for containers. 40 | ***************************************************************************************************/ 41 | 42 | /** 43 | * We are working here with containers. However, we made a typedef of the container class, in case 44 | * you might find use of something else. Hence, we are very "filthy" here and use a macro to 45 | * set our preference. To other containers have been used, but it should be okay to use a 46 | * "std::deque" or a "std::list" too. 47 | */ 48 | #define POINTTYPE vector 49 | 50 | #define SETTYPE set 51 | 52 | // Conveniences 53 | #define DATAPOINT std::POINTTYPE 54 | #define INCLUDEPOINT 55 | 56 | #define DATASET std::SETTYPE < DATAPOINT* > 57 | #define INCLUDESET 58 | 59 | // The container object to be used 60 | #include INCLUDEPOINT 61 | #include INCLUDESET 62 | 63 | // The "dobots" namespace is not really necessary, but will at times prevent clashes with 64 | // standard std functionality. 65 | namespace dobots { 66 | 67 | /** 68 | * For an explanation of the different metrics, see the "distance" function below. This distance 69 | * function does really calculate a distance between two containers, say two vectors, and is not 70 | * the std::distance function that just returns the distance between elements with respect to a 71 | * given iterator. 72 | * 73 | * Only metrics are defined (for now) that do not require additional information. For example, the 74 | * Mahalanobis distance requires the correlations between the variables as input (either by including 75 | * standard deviations, or the covariance matrix in the general case). 76 | */ 77 | enum DistanceMetric { 78 | DM_EUCLIDEAN, 79 | DM_DOTPRODUCT, 80 | DM_BHATTACHARYYA, 81 | DM_HELLINGER, 82 | DM_MANHATTAN, 83 | DM_CHEBYSHEV, 84 | DM_BHATTACHARYYA_COEFFICIENT, 85 | DM_SQUARED_HELLINGER, 86 | DM_TYPES 87 | }; 88 | 89 | enum SetDistanceMetric { 90 | SDM_INFIMIM, 91 | SDM_SUPREMUM, 92 | SDM_HAUSDORFF, 93 | SDM_SUPINF, 94 | SDM_TYPES 95 | }; 96 | 97 | enum Norm { 98 | N_EUCLIDEAN, 99 | N_TAXICAB, 100 | N_MAXIMUM, 101 | N_TYPES 102 | }; 103 | 104 | enum Mean { 105 | M_ARITHMETIC, 106 | M_GEOMETRIC, 107 | M_HARMONIC, 108 | M_QUADRATIC, 109 | M_TYPES 110 | }; 111 | 112 | /** 113 | * Somehow "std::max" is only given as a normal template function and not as a derivation of 114 | * a function object derived from binary_function. 115 | * Reason: none 116 | * Background: http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2001/n1293.html 117 | */ 118 | template 119 | struct max : public std::binary_function<_Tp, _Tp, _Tp> { 120 | _Tp operator()(const _Tp& __x, const _Tp& __y) const 121 | { return std::max(__x, __y); } 122 | }; 123 | 124 | /** 125 | * The p=2 norm, element-wise squaring the difference. 126 | */ 127 | template 128 | T euclidean(T x, T y) { 129 | return (x-y)*(x-y); 130 | } 131 | 132 | /** 133 | * The p=1 norm, can be used for the Manhattan (but also the Chebyshev distance). 134 | */ 135 | template 136 | T taxicab(const T & x, const T & y) { 137 | return std::abs(x-y); 138 | } 139 | 140 | /** 141 | * See: http://en.wikipedia.org/wiki/Bhattacharyya_distance 142 | * The Battacharyya distance is related to the Battacharyya coefficient. 143 | */ 144 | template 145 | T battacharyya(T x, T y) { 146 | return std::sqrt(x*y); 147 | } 148 | 149 | /** 150 | * See: http://en.wikipedia.org/wiki/Hellinger_distance 151 | * Related to the Battacharyya coefficient. 152 | */ 153 | template 154 | T hellinger(T x, T y) { 155 | T tmp = std::sqrt(x)-std::sqrt(y); 156 | return tmp*tmp; 157 | } 158 | 159 | /** 160 | * The hyperbolic distance is the absolute difference between the log of two variables. 161 | */ 162 | template 163 | T hyperbolic(const T & x, const T & y) { 164 | return std::abs(std::log(x) - std::log(y)); 165 | } 166 | 167 | /** 168 | * Template function for the square of a variable. In the std library only the square 169 | * root is defined (as std::sqrt). 170 | */ 171 | template 172 | T square(T x) { return x * x; } 173 | 174 | /** 175 | * Template function for 1/x. 176 | */ 177 | template 178 | T inverse(T x) { return T(1)/x; } 179 | 180 | /** 181 | * Create a template function which moves container x from or towards y with a learning rate "mu". 182 | * A positive mu will move "x" away, while a negative mu will move "x" towards "y". 183 | */ 184 | template 185 | class op_adjust: std::binary_function { 186 | T mu_; 187 | public: 188 | op_adjust(T mu): mu_(mu) {} 189 | T operator()(T x, T y) const { 190 | //fancy: std::multiplies( std::minus(x,y), mu_); 191 | T result = x + (x-y)*mu_; 192 | return result; 193 | } 194 | }; 195 | 196 | /** 197 | * @brief Accumulate values in a range with one operation for the accumulation and 198 | * one operation that has to be performed on the to be accumulated element. 199 | * 200 | * Accumulates the values in the range [first,last) using the function 201 | * object @a binary_op. The initial value is @a init. The values are 202 | * processed in order. 203 | * 204 | * @param first Start of range. 205 | * @param last End of range. 206 | * @param init Starting value to add other values to. 207 | * @param binary_op Function object to accumulate with. 208 | * @param unary_op Function object on element before accumulation. 209 | * @return The final sum. 210 | */ 211 | template 212 | inline T accumulate(InputIterator first, InputIterator last, T init, 213 | BinaryOperation binary_op, UnaryOperation unary_op) 214 | { 215 | // concept requirements 216 | __glibcxx_function_requires(_InputIteratorConcept); 217 | __glibcxx_requires_valid_range(first, last); 218 | 219 | for (; first != last; ++first) 220 | init = binary_op(init, unary_op(*first)); 221 | return init; 222 | } 223 | 224 | /** 225 | * @brief Return the maximum element in a range given one additional operation that 226 | * is performed on it beforehand. For example if @ a unary_op is std::abs it returns 227 | * the location of the element with the maximum absolute value. 228 | * @ingroup sorting_algorithms 229 | * @param first start of range 230 | * @param last end of range 231 | * @param unary_op unary operation 232 | * @return Iterator referencing the first instance of the largest value. 233 | */ 234 | template 235 | ForwardIterator 236 | max_element(ForwardIterator first, ForwardIterator last, UnaryOperation unary_op) 237 | { 238 | // concept requirements 239 | __glibcxx_function_requires(ForwardIteratorConcept); 240 | __glibcxx_function_requires(LessThanComparableConcept< 241 | typename iterator_traits::value_type>); 242 | __glibcxx_requires_valid_range(first, last); 243 | 244 | if (first == last) 245 | return first; 246 | ForwardIterator result = first; 247 | while (++first != last) 248 | if (unary_op(*result) < unary_op(*first)) 249 | result = first; 250 | return result; 251 | } 252 | 253 | 254 | /** 255 | * This function tells something about the "size" or "length" of a container, mathematically called "norm". 256 | * There are currently several norms implemented: 257 | * N_EUCLIDEAN: return sqrt (sum_i { (x_i)^2 } ) (L2 norm) 258 | * N_TAXICAB: return sum_i { abs(x_i) } (L1 norm) 259 | * N_MAXIMUM return max_i (x_i) (maximum norm) 260 | * @param first start of the container 261 | * @param last end of the container 262 | * @param norm a certain type of norm, e.g. N_EUCLIDEAN 263 | * @return the norm 264 | */ 265 | template 266 | T norm(InputIterator first, InputIterator last, Norm norm) { 267 | __glibcxx_function_requires(_InputIteratorConcept); 268 | __glibcxx_requires_valid_range(first, last); 269 | 270 | switch (norm) { 271 | case N_EUCLIDEAN: 272 | return std::sqrt(accumulate(first, last, T(0), std::plus(), square ) ); 273 | case N_TAXICAB: 274 | return accumulate(first, last, T(0), std::plus(), std::abs); 275 | case N_MAXIMUM: 276 | if (std::distance(first,last) == 0) return T(0); 277 | return *max_element(first, last, std::abs); 278 | default: 279 | std::cerr << "Unknown norm" << std::endl; 280 | return T(-1); 281 | } 282 | } 283 | 284 | /** 285 | * This function tells something about the static ensemble behaviour in the form of a "mean" 286 | * There are currently several means implemented: 287 | * M_ARITHMETIC: return 1/n (sum_i { (x_i) } ) 288 | * M_GEOMETRIC: return exp ( 1/n (sum_i { log(x_i) } ) ) 289 | * M_HARMONIC: return max_i (x_i) (maximum norm) 290 | * @param first start of the container 291 | * @param last end of the container 292 | * @param mean a certain type of norm, e.g. N_EUCLIDEAN 293 | * @return the norm 294 | */ 295 | template 296 | T mean(InputIterator first, InputIterator last, Norm norm) { 297 | __glibcxx_function_requires(_InputIteratorConcept); 298 | __glibcxx_requires_valid_range(first, last); 299 | typedef typename std::iterator_traits::difference_type DistanceType1; 300 | 301 | DistanceType1 dist = std::distance(first, last); 302 | if (!dist) return T(0); 303 | 304 | switch (norm) { 305 | case M_ARITHMETIC: 306 | return 1/T(dist)*std::accumulate(first, last, T(0)); 307 | case M_GEOMETRIC: 308 | return std::exp(1/T(dist)*accumulate(first, last, T(0), std::plus(), std::log)); 309 | case M_HARMONIC: 310 | return T(dist)/accumulate(first, last, T(0), std::plus(), inverse); 311 | default: 312 | std::cerr << "Unknown norm" << std::endl; 313 | return T(-1); 314 | } 315 | } 316 | 317 | /** 318 | * Incremental adjustment of a standard container towards a reference container. 319 | * d = d + mu ( ref - d) 320 | * So: 321 | * delta_d = mu ( ref - d) 322 | * If "ref" is smaller than "d", "delta_d" will be negative: it will make "d" smaller. 323 | * Note that this function does not make use of the different distance metrics that can be defined. 324 | * It will use std::minus (normal -sign) to judge the distance over all elements of the container 325 | * and adjust them in the same fashion by the multiplication factor mu. 326 | * If "mu" is 1, it will set the "tomove" container equal to the "reference" container. 327 | * @param tomove the container to-be-moved 328 | * @param reference the reference container towards the movements happens, the "attractor" 329 | * @param mu the step size (0 < mu <= 1) 330 | */ 331 | template 332 | void increaseDistance(InputIterator1 tomove_first, InputIterator1 tomove_last, InputIterator2 reference_first, T mu) { 333 | __glibcxx_function_requires(_InputIteratorConcept); 334 | __glibcxx_function_requires(_InputIteratorConcept); 335 | __glibcxx_requires_valid_range(first1, last1); 336 | assert (mu > T(0)); assert (mu <= T(1)); 337 | std::transform(tomove_first, tomove_last, reference_first, tomove_first, op_adjust(mu)); 338 | } 339 | 340 | /** 341 | * Incremental adjustment of a container back from a reference container. 342 | * d = d - mu ( ref - d) 343 | * @param tomove the container to-be-moved 344 | * @param reference the reference container that functions as a "repeller" 345 | * @param mu the step size (0 < mu <= 1) 346 | * @return void 347 | */ 348 | template 349 | void decreaseDistance(InputIterator1 tomove_first, InputIterator1 tomove_last, InputIterator2 reference_first, T mu) { 350 | __glibcxx_function_requires(_InputIteratorConcept); 351 | __glibcxx_function_requires(_InputIteratorConcept); 352 | __glibcxx_requires_valid_range(first1, last1); 353 | assert (mu > T(0)); assert (mu <= T(1)); 354 | std::transform(tomove_first, tomove_last, reference_first, tomove_first, op_adjust(-mu)); 355 | } 356 | 357 | /** 358 | * This function tells something about the "distance" between containers, in other words the similarity or 359 | * dissimilarity. There are currently several metrics implemented: 360 | * DM_DOTPRODUCT: return sum_i { x_i*y_i } 361 | * DM_EUCLIDEAN: return sqrt (sum_i { (x_i-y_i)^2 } ) 362 | * DM_BHATTACHARYYA: return -ln (sum_i { sqrt (x_i*y_i) } ) 363 | * DM_HELLINGER: return sqrt (sum_i { (sqrt(x_i)-sqrt(y_i))^2 } ) * 1/sqrt(2) 364 | * DM_CHEBYSHEV: return max_i abs(x_i-y_i) 365 | * DM_MANHATTAN: return sum_i { abs(x_i-y_i) } 366 | * And there are some other measures that can be used as metrics. Such as the Bhattacharyya coefficient 367 | * and the squared Hellinger distance. 368 | * It is assumed that the containers are of equal size. 369 | * @param first1 start of the first container 370 | * @param last1 end of the first container 371 | * @param first2 start of the second container 372 | * @param last2 end of the second container 373 | * @param metric a certain distance metric 374 | * @return the distance between the two containers 375 | */ 376 | template 377 | T distance(InputIterator1 first1, InputIterator1 last1, InputIterator2 first2, InputIterator2 last2, DistanceMetric metric) { 378 | __glibcxx_function_requires(_InputIteratorConcept); 379 | __glibcxx_function_requires(_InputIteratorConcept); 380 | __glibcxx_requires_valid_range(first1, last1); 381 | __glibcxx_requires_valid_range(first2, last2); 382 | if (std::distance(first2,last2) != std::distance(first1,last1)) { 383 | std::cerr << "Container size unequal: " << std::distance(first1,last1) << " vs " << std::distance(first2,last2) << std::endl; 384 | assert (std::distance(first2,last2) == std::distance(first1,last1)); 385 | } 386 | switch (metric) { 387 | case DM_DOTPRODUCT: 388 | return std::inner_product(first1, last1, first2, T(0)); 389 | case DM_EUCLIDEAN: 390 | return std::sqrt(std::inner_product(first1, last1, first2, T(0), std::plus(), euclidean)); 391 | case DM_BHATTACHARYYA: 392 | return -std::log(std::inner_product(first1, last1, first2, T(0), std::plus(), battacharyya)); 393 | case DM_HELLINGER: 394 | return (std::sqrt(std::inner_product(first1, last1, first2, T(0), std::plus(), hellinger))) / 395 | std::sqrt(2); 396 | case DM_CHEBYSHEV: 397 | return std::inner_product(first1, last1, first2, T(0), max(), taxicab); 398 | case DM_MANHATTAN: 399 | return std::inner_product(first1, last1, first2, T(0), std::plus(), taxicab); 400 | case DM_BHATTACHARYYA_COEFFICIENT: 401 | return std::inner_product(first1, last1, first2, T(0), std::plus(), battacharyya); 402 | case DM_SQUARED_HELLINGER: 403 | //return std::inner_product(first1, last1, first2, T(0), std::plus(), hellinger) * T(1)/T(2); 404 | // faster to calculate: 405 | return std::sqrt(T(1) - std::inner_product(first1, last1, first2, T(0), std::plus(), battacharyya)); 406 | default: 407 | std::cerr << "Unknown distance metric" << std::endl; 408 | return T(-1); 409 | } 410 | } 411 | 412 | /** 413 | * Provide a similar template function, but now with containers instead of iterators. Be careful that now the 414 | * typename Point is not checked for having actually "begin() and end()" operators. 415 | */ 416 | template 417 | T distance_impl(Point point1, Point point2, DistanceMetric metric) { 418 | return distance(point1.begin(), point1.end(), point2.begin(), point2.end(), metric); 419 | } 420 | 421 | 422 | /** 423 | * A wrapper struct for "distance" to be used as function object (it is a binary function) to calculate 424 | * distances between sets. It is a comparison functor. To do the comparison the distance between a 425 | * reference point, previously given, and each of the argument is calculated. If the first argument is 426 | * smaller, the function returns true. 427 | * @template_param Point should be a pointer to a container, e.g. std::vector* 428 | * @template_param Iterator should be an iterator over the same type of container 429 | * @template_param Value should be the value of the elements in the container 430 | * @param point_metric to be used, e.g. Euclidean 431 | * @param first_ref is reference to point 432 | * @param last_ref is reference to point 433 | * @return true if x is closer to the reference point than y 434 | * This struct requires the function of above. 435 | */ 436 | template 437 | struct comp_point_distance: public std::binary_function { 438 | comp_point_distance(DistanceMetric point_metric, PointIterator first_ref, PointIterator last_ref): 439 | point_metric(point_metric), first_ref(first_ref), last_ref(last_ref) { 440 | } 441 | bool operator()(Point x, Point y) { 442 | return distance(x->begin(), x->end(), first_ref, last_ref, point_metric) < 443 | distance(y->begin(), y->end(), first_ref, last_ref, point_metric); 444 | } 445 | DistanceMetric point_metric; 446 | PointIterator first_ref; 447 | PointIterator last_ref; 448 | }; 449 | 450 | /* 451 | * A function calculating the distance of a point to a set. 452 | * SDM_INFIMIM the minimum distance to this point, for Euclidean/Manhattan in 1D example, d(1,[3,6]) = 2 and d(7,[3,6]) = 1. 453 | * TODO: make sure that the values of the iterator over the set correspond with the container over which the second iterator 454 | * runs. 455 | */ 456 | template 457 | T distance_to_point(SetIterator first_set, SetIterator last_set, PointIterator first_point, PointIterator last_point, 458 | SetDistanceMetric set_metric, DistanceMetric point_metric) { 459 | __glibcxx_function_requires(_InputIteratorConcept); 460 | __glibcxx_function_requires(_InputIteratorConcept); 461 | __glibcxx_requires_valid_range(first1, last1); 462 | __glibcxx_requires_valid_range(first2, last2); 463 | typedef typename std::iterator_traits::value_type PointType; // e.g. std::vector* 464 | typedef typename std::iterator_traits::value_type PointValueType; // e.g. double 465 | 466 | T result = T(-1); 467 | PointType tmp; 468 | switch(set_metric) { 469 | case SDM_INFIMIM: // the smallest distance between the point and any point in the set 470 | tmp = *std::min_element(first_set, last_set, comp_point_distance( 471 | point_metric, first_point, last_point)); 472 | return distance(tmp->begin(), tmp->end(), first_point, last_point, point_metric); 473 | case SDM_SUPREMUM: // the largest distance between the point and any point in the set 474 | tmp = *std::max_element(first_set, last_set, comp_point_distance( 475 | point_metric, first_point, last_point)); 476 | return distance(tmp->begin(), tmp->end(), first_point, last_point, point_metric); 477 | default: 478 | std::cerr << "Not yet implemented" << std::endl; 479 | break; 480 | } 481 | 482 | return result; 483 | } 484 | 485 | /** 486 | * Same function as above, but using iterators implicitly. Not safe. 487 | */ 488 | template 489 | T distance_impl(SetContainer set, Point point, SetDistanceMetric set_metric, DistanceMetric point_metric) { 490 | return distance_to_point(set.begin(), set.end(), point.begin(), point.end(), set_metric, point_metric); 491 | } 492 | 493 | /** 494 | * @template_param Point should be something like std::vector * 495 | * @template_param SetIterator should be an iterator over a set 496 | * @template_param PointIterator should be an iterator over the point container 497 | * @template_param Value should be the type of the values in the point container, e.g. float 498 | * @return true if point is closer to 499 | */ 500 | template 501 | struct comp_point_set_distance: public std::binary_function { 502 | comp_point_set_distance(SetDistanceMetric set_metric, DistanceMetric point_metric, SetIterator first_set, 503 | SetIterator last_set): 504 | set_metric(set_metric), point_metric(point_metric), first_set(first_set), last_set(last_set) { 505 | } 506 | 507 | bool operator()(const Point & x, const Point & y) const { 508 | return distance_to_point(first_set, last_set, x->begin(), x->end(), set_metric, point_metric) < 509 | distance_to_point(first_set, last_set, y->begin(), y->end(), set_metric, point_metric); 510 | } 511 | SetDistanceMetric set_metric; 512 | DistanceMetric point_metric; 513 | SetIterator first_set; 514 | SetIterator last_set; 515 | }; 516 | 517 | 518 | /** 519 | * Different metrics that exist between sets of points. 520 | * SDM_HAUSDORFF longest distance you can be forced to travel by an adversary who chooses a point in one of the two sets, 521 | * from where you then must travel to the other set (wikipedia) 522 | * SDM_SUPINF calculates the smallest distance between a point and any other, then picks the point that is most remote 523 | * of the others (Hausdorff is then just doing this also in the opposite direction in case distances are not 524 | * symmetric) 525 | * For now only Hausdorff is implemented and the supremum-infimim operator. An example of the Hausdorff metric when the point metric 526 | * is d([1,3,6,7], [3,6]) = 2, but d([3,6], [1,3,6,7]) = 0. 527 | * @param set0 a set of data points (each data point can be a vector or list or matrix) 528 | * @param set1 another set of data points 529 | * @param set_metric the metric to be used between the two sets 530 | * @param metric the metric to be used to define distances between points 531 | * 532 | * Caution! The SetIterator and the PointIterator do not correspond to the first set of two iterators, respectively the last set of two 533 | * iterators. All of these iterators should be of the same type SetIterator. However, they should be decomposable into PointIterators. 534 | * In other words, the set entities should have the PointIterator as valid iterator defined over each of their elements. This definitely 535 | * requires you to define the template variables (because they cannot be retrieved from the arguments). 536 | */ 537 | template 538 | T distance_to_set(SetIterator first1, SetIterator last1, SetIterator first2, SetIterator last2, 539 | SetDistanceMetric set_metric, DistanceMetric point_metric) { 540 | __glibcxx_function_requires(_InputIteratorConcept); 541 | __glibcxx_function_requires(_InputIteratorConcept); 542 | __glibcxx_requires_valid_range(first1, last1); 543 | __glibcxx_requires_valid_range(first2, last2); 544 | typedef typename std::iterator_traits::value_type PointType; // e.g. std::vector* 545 | typedef typename std::iterator_traits::value_type PointValueType; // e.g. double 546 | 547 | SetDistanceMetric overwrite_set_metric; 548 | PointType tmp; 549 | switch(set_metric) { 550 | case SDM_HAUSDORFF: { 551 | T dist_xy = distance_to_set(first1, last1, first2, last2, SDM_SUPINF, point_metric); 552 | T dist_yx = distance_to_set(first2, last2, first1, last1, SDM_SUPINF, point_metric); 553 | return std::max(dist_xy, dist_yx); 554 | } 555 | case SDM_SUPINF: 556 | overwrite_set_metric = SDM_INFIMIM; 557 | tmp = *std::max_element(first1, last1, 558 | comp_point_set_distance( 559 | overwrite_set_metric, point_metric, first2, last2)); 560 | return distance_to_point( 561 | first2, last2, tmp->begin(), tmp->end(), overwrite_set_metric, point_metric); 562 | break; 563 | default: 564 | std::cerr << "Not yet implemented" << std::endl; 565 | break; 566 | } 567 | } 568 | 569 | /** 570 | * 571 | */ 572 | template 573 | T zero_func() { return T(0); } 574 | 575 | template 576 | void clean(InputIterator1 first1, InputIterator1 last1) { 577 | typedef typename std::iterator_traits::value_type ValueType1; 578 | std::generate_n(first1, last1 - first1, zero_func); 579 | } 580 | 581 | /** 582 | * Calculate the integral of a function given a kernel in the discrete case. 583 | * 584 | * Functions like remove_copy which return a series of values in the STL C++ library, require the 585 | * user to allocate the proper number of items beforehand. This seems to me a pain for functions 586 | * where this is not known beforehand. However, to be consistent with that thought package, you will 587 | * need to have the output iterator pointed to a container that is large enough. 588 | */ 589 | template 590 | OutputIterator 591 | integral(InputIterator1 first1, InputIterator1 last1, InputIterator2 first2, 592 | OutputIterator result) { 593 | 594 | typedef typename std::iterator_traits::value_type ValueType1; 595 | 596 | // concept requirements 597 | __glibcxx_function_requires(_InputIteratorConcept); 598 | __glibcxx_function_requires(_InputIteratorConcept); 599 | __glibcxx_function_requires(_OutputIteratorConcept); 600 | __glibcxx_requires_valid_range(first1,last1); 601 | 602 | if (first1 == last1) 603 | return result; 604 | ValueType1 value = *first1 * *first2; 605 | *result = value; 606 | while (++first1 != last1) 607 | { 608 | value = value + *first1 * *++first2; 609 | *++result = value; 610 | } 611 | return ++result; 612 | } 613 | 614 | /** 615 | * 616 | */ 617 | template 618 | OutputIterator 619 | integral(InputIterator1 first1, InputIterator1 last1, InputIterator2 first2, 620 | OutputIterator result, BinaryOperation1 binary_op1, BinaryOperation2 binary_op2) { 621 | 622 | typedef typename std::iterator_traits::value_type ValueType1; 623 | 624 | // concept requirements 625 | __glibcxx_function_requires(_InputIteratorConcept); 626 | __glibcxx_function_requires(_InputIteratorConcept); 627 | __glibcxx_function_requires(_OutputIteratorConcept); 628 | __glibcxx_requires_valid_range(first1,last1); 629 | 630 | if (first1 == last1) 631 | return result; 632 | ValueType1 value = *first1 * *first2 633 | *result = value; 634 | while (++first1 != last1) 635 | { 636 | value = binary_op1(value, binary_op2(*first1, *++first2)); 637 | *++result = value; 638 | 639 | } 640 | return ++result; 641 | } 642 | 643 | /** 644 | * The Cauchy product: 645 | * c_n = \sum_{k=0}^n { a_k * b_{n-k} } 646 | * This product is very similar to the integral. It is not calculated only for the total size of 647 | * the vector, but for each element of the vector. 648 | * @param last2 is an iterator to the LAST element of the second vector 649 | */ 650 | template 651 | OutputIterator 652 | cauchy_product(InputIterator1 first1, InputIterator1 last1, InputIterator2 last2, 653 | OutputIterator result) { 654 | 655 | typedef typename std::iterator_traits::value_type ValueType1; 656 | 657 | // concept requirements 658 | __glibcxx_function_requires(_InputIteratorConcept); 659 | __glibcxx_function_requires(_InputIteratorConcept); 660 | __glibcxx_function_requires(_OutputIteratorConcept); 661 | __glibcxx_requires_valid_range(first1,last1); 662 | 663 | if (first1 == last1) 664 | return result; 665 | 666 | ValueType1 value = *first1 * *--last2; 667 | *result = value; 668 | while (++first1 != last1) 669 | { 670 | value = value + *first1 * *--last2; 671 | *++result = value; 672 | } 673 | return ++result; 674 | } 675 | 676 | /** 677 | * Inner product where we iterate backwards over the second container. 678 | */ 679 | template 680 | inline Tp 681 | reverse_inner_product(InputIterator1 first1, InputIterator1 last1, 682 | InputIterator2 last2, Tp init) 683 | { 684 | // concept requirements 685 | __glibcxx_function_requires(_InputIteratorConcept); 686 | __glibcxx_function_requires(_InputIteratorConcept); 687 | __glibcxx_requires_valid_range(first1, last1); 688 | --last2; 689 | for (; first1 != last1; ++first1, --last2) 690 | init = init + (*first1 * *last2); 691 | return init; 692 | } 693 | 694 | /** 695 | * The function argmin calculates unary_op(x_i) and unary_op(x_j) for all i and j in vector X and returns 696 | * that index that is minimum. 697 | * {x*} = argmin_x f(x) 698 | * The {} mean that it is in principle possible to return a set, which we will not. The *means that it is 699 | * the minimum of all possible x. The argmin_x will input every possible x in function f. And the function 700 | * takes only one argument, it is a unary_op. 701 | * Note, that this function is actually almost the same as std::min_element() except that the latter takes 702 | * a comparison functor as (optional) argument and this function takes a unary operator. 703 | * @param first beginning of container 704 | * @param last end of container 705 | * @param unary_op function to perform 706 | * @param init minimum value (set it to first element in container if you don't want it) 707 | * @return index to minimum value 708 | */ 709 | template 710 | ForwardIterator 711 | argmin(ForwardIterator first, ForwardIterator last, UnaryOperation unary_op) { 712 | 713 | typedef typename std::iterator_traits::value_type ValueType; 714 | 715 | // concept requirements 716 | __glibcxx_function_requires(_ForwardIteratorConcept); 717 | __glibcxx_requires_valid_range(first,last); 718 | 719 | if (first == last) 720 | return first; 721 | ForwardIterator result = first; 722 | while (++first != last) 723 | if (unary_op(*first) < unary_op(*result)) 724 | result = first; 725 | return result; 726 | } 727 | 728 | /** 729 | * This function calculates the discrete convolution between two functions represented by for example 730 | * vectors or sets. It calculates the product of x[i] and y[shift-i]. So, with shift of 1, it multiplies 731 | * x[0] with x[1], x[1] with y[0], x[2] with y[-1], etc. So, a normal convolution would not work for a 732 | * finite range of values (like a vector). The circular_convolution however, works fine with a limited 733 | * sequence. It calculates: 734 | * conv_n(shift) = \sum_{k=0}^{n-1} { a_k * b_{(shift-k) % n} } 735 | * 736 | * In other words, using vector terminology. It calculates the inner product between the two vectors with 737 | * the second one reversed and each time shifted a bit more with "shift" (default 1) for in total N times, 738 | * the results which are written into an output container. 739 | * 740 | * @param first1 beginning of container 741 | * @param last1 end of container 742 | * @param first2 beginning of second container 743 | * @param last2 end of second container (required for rotation) 744 | * @param result begin of container for results (needs capacity last1 - first1) 745 | * @param (optional) shift with which to calculate the convolution, default 1 746 | * @return end of result container 747 | */ 748 | template 749 | OutputIterator circular_convolution(ForwardIterator1 first1, ForwardIterator1 last1, ForwardIterator2 first2, ForwardIterator2 last2, 750 | OutputIterator result, int shift = 1) { 751 | __glibcxx_function_requires(_ForwardIteratorConcept); 752 | __glibcxx_function_requires(_ForwardIteratorConcept); 753 | __glibcxx_function_requires(_OutputIteratorConcept); 754 | __glibcxx_requires_valid_range(first1, last1); 755 | __glibcxx_requires_valid_range(first2, last2); 756 | 757 | typedef typename std::iterator_traits::value_type ValueType1; 758 | typedef typename std::iterator_traits::difference_type DistanceType1; 759 | 760 | DistanceType1 dist = std::distance(first1, last1); 761 | while (dist--) { 762 | std::rotate(first2, last2-shift, last2); 763 | *result++ = reverse_inner_product(first1, last1, last2, ValueType1(0)); 764 | } 765 | return result; 766 | } 767 | 768 | 769 | } 770 | 771 | #endif /* CONTAINER_H_ */ 772 | --------------------------------------------------------------------------------