diff --git a/RWR/src/mean-shift/catkin_simple b/RWR/src/mean-shift/catkin_simple
new file mode 160000
index 0000000000000000000000000000000000000000..0e62848b12da76c8cc58a1add42b4f894d1ac21e
--- /dev/null
+++ b/RWR/src/mean-shift/catkin_simple
@@ -0,0 +1 @@
+Subproject commit 0e62848b12da76c8cc58a1add42b4f894d1ac21e
diff --git a/RWR/src/mean-shift/dvs_meanshift/.vscode/settings.json b/RWR/src/mean-shift/dvs_meanshift/.vscode/settings.json
new file mode 100644
index 0000000000000000000000000000000000000000..c925ebcc2f19d9a7598fae22842622ffddaf96d2
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/.vscode/settings.json
@@ -0,0 +1,3 @@
+{
+ "C_Cpp.intelliSenseEngineFallback": "Disabled"
+}
\ No newline at end of file
diff --git a/RWR/src/mean-shift/dvs_meanshift/CMakeLists.txt b/RWR/src/mean-shift/dvs_meanshift/CMakeLists.txt
new file mode 100755
index 0000000000000000000000000000000000000000..033e4c8867fd73ff4a11926e83c2d0a553476466
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/CMakeLists.txt
@@ -0,0 +1,76 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(dvs_meanshift)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ cv_bridge
+ dvs_msgs
+ image_transport
+ pcl_ros
+ roscpp
+ rospy
+ sensor_msgs
+ std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(PCL 1.7 REQUIRED)
+
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+ INCLUDE_DIRS include
+# LIBRARIES dvs_meanshift
+ CATKIN_DEPENDS cv_bridge dvs_msgs image_transport pcl_ros roscpp rospy sensor_msgs std_msgs
+ DEPENDS OpenCV boost
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+ include
+ ${catkin_INCLUDE_DIRS}
+ ${OpenCV_INCLUDE_DIRS}
+ ${Boost_INCLUDE_DIRS}
+)
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(dvs_meanshift ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+# add_executable(dvs_meanshift_node src/dvs_meanshift_node.cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(dvs_meanshift_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(dvs_meanshift_node
+# ${catkin_LIBRARIES}
+# )
+
+add_executable(dvs_meanshift src/meanshift.cpp src/meanshift_node.cpp src/color_meanshift.cpp src/segment.cpp src/image_reconst.cpp)
+target_link_libraries(dvs_meanshift ${catkin_LIBRARIES} ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${OpenCV_LIBRARIES})
+add_dependencies(dvs_meanshift ${catkin_EXPORTED_TARGETS} )
diff --git a/RWR/src/mean-shift/dvs_meanshift/README.md b/RWR/src/mean-shift/dvs_meanshift/README.md
new file mode 100755
index 0000000000000000000000000000000000000000..38ec2e83c7469b0fa76b7c4c45c429753dc53d04
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/README.md
@@ -0,0 +1 @@
+# dvs_meanshift
\ No newline at end of file
diff --git a/RWR/src/mean-shift/dvs_meanshift/include/dvs_meanshift/color_meanshift.h b/RWR/src/mean-shift/dvs_meanshift/include/dvs_meanshift/color_meanshift.h
new file mode 100755
index 0000000000000000000000000000000000000000..bd20d1b0b4edec2b1411e8887c4b0920d30b29ed
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/include/dvs_meanshift/color_meanshift.h
@@ -0,0 +1,18 @@
+/*
+ * color_meanshift.h
+ *
+ * Created on: Nov 2, 2016
+ * Author: fran
+ */
+
+#ifndef COLOR_MEANSHIFT_H_
+#define COLOR_MEANSHIFT_H_
+
+#include <ros/ros.h>
+#include <opencv2/core/core.hpp>
+
+void colorMeanShiftFilt(double *newPixelsPtr, const double *pixelsPtr, int mPixels, int maxPixelDim, const double *imagePtr, int numRows, int numCols, float spaceDivider, char *kernelFun, int maxIterNum, float tolFun);
+//void colorMeanShiftFilt(double *newPixelsPtr, const double *pixelsPtr, int mPixels, int maxPixelDim, const double *imagePtr, int numRows, int numCols, float spaceDivider, char *kernelFun, int maxIterNum, float tolFun, double *newImagePtr);
+void meanshiftCluster_Gaussian(cv::Mat dataPts, std::vector<double> *clusterCenterX, std::vector<double> *clusterCenterY, std::vector<double> *clusterCenterZ, std::vector<int> *point2Clusters, double bandwidth);
+
+#endif /* COLOR_MEANSHIFT_H_ */
diff --git a/RWR/src/mean-shift/dvs_meanshift/include/dvs_meanshift/meanshift.h b/RWR/src/mean-shift/dvs_meanshift/include/dvs_meanshift/meanshift.h
new file mode 100755
index 0000000000000000000000000000000000000000..a3f9200de5b647a9071f74515196be1df294c5df
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/include/dvs_meanshift/meanshift.h
@@ -0,0 +1,144 @@
+/*
+ * dvs_meanshift.h
+ *
+ * Created on: Nov 2, 2016
+ * Author: fran
+ */
+
+#ifndef MEANSHIFT_H_
+#define MEANSHIFT_H_
+
+#include <ros/ros.h>
+#include <iostream>
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/photo/photo.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/video/video.hpp>
+
+#include <dvs_msgs/Event.h>
+#include <dvs_msgs/EventArray.h>
+#include "graph3d/segment.h"
+
+#define TEST 0
+#define KALMAN_FILTERING 1
+#define BG_FILTERING 1
+
+
+#define DVSW 240 //128
+#define DVSH 180 //128
+
+//#define DVSW 240
+//#define DVSH 180
+
+namespace dvs_meanshift
+{
+
+class Meanshift {
+public:
+ Meanshift(ros::NodeHandle & nh, ros::NodeHandle nh_private);
+ virtual ~Meanshift();
+
+private:
+ ros::NodeHandle nh_;
+
+ void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg);
+ void eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg);
+ void eventsCallback_simple(const dvs_msgs::EventArray::ConstPtr& msg);
+
+ //void assignClusterColor(std::vector<int> * positionClusterColor, int numClusters, std::vector<int> matches, std::vector<int> oldPositions);
+ void assignClusterColor(std::vector<int> * positionClusterColor, int numClusters, std::vector<int> matches, std::vector<int> oldPositions, std::vector<int> activeTrajectories);
+ //void createKalmanFilter();
+ void createKalmanFilter(cv::KalmanFilter *kf, cv::Mat *state, cv::Mat *meas);
+
+ int lastPositionClusterColor;
+
+ cv::Mat camera_matrix_, dist_coeffs_;
+
+ ros::Subscriber event_sub_;
+ ros::Subscriber camera_info_sub_;
+
+ image_transport::Publisher image_pub_;
+ image_transport::Publisher image_segmentation_pub_;
+
+
+ //DEBUG
+ image_transport::Publisher image_debug1_pub_;
+ image_transport::Publisher image_debug2_pub_;
+ //NOT DEBUG
+
+
+ cv::Mat last_image_;
+ bool used_last_image_;
+
+ enum DisplayMethod
+ {
+ GRAYSCALE, RED_BLUE
+ } display_method_;
+
+ //Meanshift params
+ //computing image calibration
+ double *outputFeatures;
+ int mPixels, maxPixelDim;
+ double *pixelsPtr;
+ double *imagePtr;
+ //double *positionsPtr;
+ double *initializationPtr;
+ int numRows, numCols, maxIterNum;
+ float spaceDivider, timeDivider;
+ char kernelFun[8];
+ float tolFun;
+
+ //Params for graph3d segmentation
+ static const int MAX_NUM_CLUSTERS = 256;
+ static const int MAX_NUM_TRAJECTORY_POINTS=16;
+ std::vector<cv::Vec3b> RGBColors;
+ std::vector<cv::Point> *allTrajectories; //It is an array of vectors of Points for storing trajectories (a maximum of 8 points should be stored)
+ std::vector<int> counterTrajectories; //It stores the last position occupied for each trajectory in allTrajectories
+
+ float sigma;
+ int k;
+ int min_region;
+ int num_components;
+
+ //Clusters
+ std::vector<double> prev_clustCentX, prev_clustCentY, prev_clustCentZ;
+ std::vector<int> prev_positionClusterColor;
+ std::vector<int> clustColor;
+ std::vector<int> prev_activeTrajectories;
+
+ //Trajectories
+ //cv::Mat trajectories=cv::Mat(DVSH, DVSW, CV_8UC3);
+ cv::Mat trajectories;
+
+ //Kalman filter parameters
+ //cv::KalmanFilter kf;
+ //cv::Mat state;
+ //cv::Mat meas;
+
+ std::vector<cv::KalmanFilter> vector_of_kf;
+ std::vector<cv::Mat> vector_of_state;
+ std::vector<cv::Mat> vector_of_meas;
+
+ //bool foundBlobs;
+ std::vector<bool> vector_of_foundBlobs;
+ int notFoundBlobsCount;
+ std::vector<bool> foundTrajectory;
+ //bool foundTrajectory;
+ int selectedTrajectory;
+ //double ticks;
+ std::vector<double> vector_of_ticks;
+
+ //frame of times
+ cv::Mat BGAFframe;
+};
+
+} // namespace
+
+#endif // MEANSHIFT_H_
diff --git a/RWR/src/mean-shift/dvs_meanshift/include/graph3d/convolve.h b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/convolve.h
new file mode 100755
index 0000000000000000000000000000000000000000..6c2a77339823de1835642bc9df0f8bd0754448c5
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/convolve.h
@@ -0,0 +1,69 @@
+/*
+Copyright (C) 2006 Pedro Felzenszwalb
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation; either version 2 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+*/
+
+/* convolution */
+
+#ifndef CONVOLVE_H
+#define CONVOLVE_H
+
+#include <vector>
+#include <algorithm>
+#include <cmath>
+#include "graph3d/image.h"
+
+/* convolve src with mask. dst is flipped! */
+static void convolve_even(image<float> *src, image<float> *dst,
+ std::vector<float> &mask) {
+ int width = src->width();
+ int height = src->height();
+ int len = mask.size();
+
+ for (int y = 0; y < height; y++) {
+ for (int x = 0; x < width; x++) {
+ float sum = mask[0] * imRef(src, x, y);
+ for (int i = 1; i < len; i++) {
+ sum += mask[i] *
+ (imRef(src, std::max(x-i,0), y) +
+ imRef(src, std::min(x+i, width-1), y));
+ }
+ imRef(dst, y, x) = sum;
+ }
+ }
+}
+
+/* convolve src with mask. dst is flipped! */
+static void convolve_odd(image<float> *src, image<float> *dst,
+ std::vector<float> &mask) {
+ int width = src->width();
+ int height = src->height();
+ int len = mask.size();
+
+ for (int y = 0; y < height; y++) {
+ for (int x = 0; x < width; x++) {
+ float sum = mask[0] * imRef(src, x, y);
+ for (int i = 1; i < len; i++) {
+ sum += mask[i] *
+ (imRef(src, std::max(x-i,0), y) -
+ imRef(src, std::min(x+i, width-1), y));
+ }
+ imRef(dst, y, x) = sum;
+ }
+ }
+}
+
+#endif
diff --git a/RWR/src/mean-shift/dvs_meanshift/include/graph3d/disjoint-set.h b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/disjoint-set.h
new file mode 100755
index 0000000000000000000000000000000000000000..061b68c8a4f382c96c6c0bf5b1fe08ce12c81f5f
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/disjoint-set.h
@@ -0,0 +1,79 @@
+/*
+Copyright (C) 2006 Pedro Felzenszwalb
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation; either version 2 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+*/
+
+#ifndef DISJOINT_SET
+#define DISJOINT_SET
+
+// disjoint-set forests using union-by-rank and path compression (sort of).
+
+typedef struct {
+ int rank;
+ int p;
+ int size;
+} uni_elt;
+
+class universe {
+public:
+ universe(int elements);
+ ~universe();
+ int find(int x);
+ void join(int x, int y);
+ int size(int x) const { return elts[x].size; }
+ int num_sets() const { return num; }
+
+private:
+ uni_elt *elts;
+ int num;
+};
+
+universe::universe(int elements) {
+ elts = new uni_elt[elements];
+ num = elements;
+ for (int i = 0; i < elements; i++) {
+ elts[i].rank = 0;
+ elts[i].size = 1;
+ elts[i].p = i;
+ }
+}
+
+universe::~universe() {
+ delete [] elts;
+}
+
+int universe::find(int x) {
+ int y = x;
+ while (y != elts[y].p)
+ y = elts[y].p;
+ elts[x].p = y;
+ return y;
+}
+
+void universe::join(int x, int y) {
+ if (elts[x].rank > elts[y].rank) {
+ elts[y].p = x;
+ elts[x].size += elts[y].size;
+ } else {
+ elts[x].p = y;
+ elts[y].size += elts[x].size;
+ if (elts[x].rank == elts[y].rank)
+ elts[y].rank++;
+ }
+ num--;
+}
+
+#endif
diff --git a/RWR/src/mean-shift/dvs_meanshift/include/graph3d/filter.h b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/filter.h
new file mode 100755
index 0000000000000000000000000000000000000000..438c5acadb0374b669769b45a282051ab1fc4c79
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/filter.h
@@ -0,0 +1,100 @@
+/*
+Copyright (C) 2006 Pedro Felzenszwalb
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation; either version 2 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+*/
+
+/* simple filters */
+
+#ifndef FILTER_H
+#define FILTER_H
+
+#include <vector>
+#include <cmath>
+#include "graph3d/image.h"
+#include "graph3d/misc.h"
+#include "graph3d/convolve.h"
+#include "graph3d/imconv.h"
+
+#define WIDTH 4.0
+
+/* normalize mask so it integrates to one */
+static void normalize(std::vector<float> &mask) {
+ int len = mask.size();
+ float sum = 0;
+ for (int i = 1; i < len; i++) {
+ sum += fabs(mask[i]);
+ }
+ sum = 2*sum + fabs(mask[0]);
+ for (int i = 0; i < len; i++) {
+ mask[i] /= sum;
+ }
+}
+
+/* make filters */
+#define MAKE_FILTER(name, fun) \
+static std::vector<float> make_ ## name (float sigma) { \
+ sigma = std::max(sigma, 0.01F); \
+ int len = (int)ceil(sigma * WIDTH) + 1; \
+ std::vector<float> mask(len); \
+ for (int i = 0; i < len; i++) { \
+ mask[i] = fun; \
+ } \
+ return mask; \
+}
+
+MAKE_FILTER(fgauss, exp(-0.5*square(i/sigma)));
+
+/* convolve image with gaussian filter */
+static image<float> *smooth(image<float> *src, float sigma) {
+ std::vector<float> mask = make_fgauss(sigma);
+ normalize(mask);
+
+ image<float> *tmp = new image<float>(src->height(), src->width(), false);
+ image<float> *dst = new image<float>(src->width(), src->height(), false);
+ convolve_even(src, tmp, mask);
+ convolve_even(tmp, dst, mask);
+
+ delete tmp;
+ return dst;
+}
+
+/* convolve image with gaussian filter */
+image<float> *smooth(image<uchar> *src, float sigma) {
+ image<float> *tmp = imageUCHARtoFLOAT(src);
+ image<float> *dst = smooth(tmp, sigma);
+ delete tmp;
+ return dst;
+}
+
+/* compute laplacian */
+static image<float> *laplacian(image<float> *src) {
+ int width = src->width();
+ int height = src->height();
+ image<float> *dst = new image<float>(width, height);
+
+ for (int y = 1; y < height-1; y++) {
+ for (int x = 1; x < width-1; x++) {
+ float d2x = imRef(src, x-1, y) + imRef(src, x+1, y) -
+ 2*imRef(src, x, y);
+ float d2y = imRef(src, x, y-1) + imRef(src, x, y+1) -
+ 2*imRef(src, x, y);
+ imRef(dst, x, y) = d2x + d2y;
+ }
+ }
+ return dst;
+}
+
+#endif
diff --git a/RWR/src/mean-shift/dvs_meanshift/include/graph3d/image.h b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/image.h
new file mode 100755
index 0000000000000000000000000000000000000000..c542465dc10b5029753e2758288b34338015e006
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/image.h
@@ -0,0 +1,101 @@
+/*
+Copyright (C) 2006 Pedro Felzenszwalb
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation; either version 2 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+*/
+
+/* a simple image class */
+
+#ifndef IMAGE_H
+#define IMAGE_H
+
+#include <cstring>
+
+template <class T>
+class image {
+ public:
+ /* create an image */
+ image(const int width, const int height, const bool init = true);
+
+ /* delete an image */
+ ~image();
+
+ /* init an image */
+ void init(const T &val);
+
+ /* copy an image */
+ image<T> *copy() const;
+
+ /* get the width of an image. */
+ int width() const { return w; }
+
+ /* get the height of an image. */
+ int height() const { return h; }
+
+ /* image data. */
+ T *data;
+
+ /* row pointers. */
+ T **access;
+
+ private:
+ int w, h;
+};
+
+/* use imRef to access image data. */
+#define imRef(im, x, y) (im->access[y][x])
+
+/* use imPtr to get pointer to image data. */
+#define imPtr(im, x, y) &(im->access[y][x])
+
+template <class T>
+image<T>::image(const int width, const int height, const bool init) {
+ w = width;
+ h = height;
+ data = new T[w * h]; // allocate space for image data
+ access = new T*[h]; // allocate space for row pointers
+
+ // initialize row pointers
+ for (int i = 0; i < h; i++)
+ access[i] = data + (i * w);
+
+ if (init)
+ memset(data, 0, w * h * sizeof(T));
+}
+
+template <class T>
+image<T>::~image() {
+ delete [] data;
+ delete [] access;
+}
+
+template <class T>
+void image<T>::init(const T &val) {
+ T *ptr = imPtr(this, 0, 0);
+ T *end = imPtr(this, w-1, h-1);
+ while (ptr <= end)
+ *ptr++ = val;
+}
+
+
+template <class T>
+image<T> *image<T>::copy() const {
+ image<T> *im = new image<T>(w, h, false);
+ memcpy(im->data, data, w * h * sizeof(T));
+ return im;
+}
+
+#endif
+
diff --git a/RWR/src/mean-shift/dvs_meanshift/include/graph3d/imconv.h b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/imconv.h
new file mode 100755
index 0000000000000000000000000000000000000000..ba78bcc622bf39c8e04dbb9fabf9a22fb71c8edd
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/imconv.h
@@ -0,0 +1,177 @@
+/*
+Copyright (C) 2006 Pedro Felzenszwalb
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation; either version 2 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+*/
+
+/* image conversion */
+
+#ifndef CONV_H
+#define CONV_H
+
+#include <climits>
+#include "graph3d/image.h"
+#include "graph3d/imutil.h"
+#include "graph3d/misc.h"
+
+#define RED_WEIGHT 0.299
+#define GREEN_WEIGHT 0.587
+#define BLUE_WEIGHT 0.114
+
+static image<uchar> *imageRGBtoGRAY(image<rgb> *input) {
+ int width = input->width();
+ int height = input->height();
+ image<uchar> *output = new image<uchar>(width, height, false);
+
+ for (int y = 0; y < height; y++) {
+ for (int x = 0; x < width; x++) {
+ imRef(output, x, y) = (uchar)
+ (imRef(input, x, y).r * RED_WEIGHT +
+ imRef(input, x, y).g * GREEN_WEIGHT +
+ imRef(input, x, y).b * BLUE_WEIGHT);
+ }
+ }
+ return output;
+}
+
+static image<rgb> *imageGRAYtoRGB(image<uchar> *input) {
+ int width = input->width();
+ int height = input->height();
+ image<rgb> *output = new image<rgb>(width, height, false);
+
+ for (int y = 0; y < height; y++) {
+ for (int x = 0; x < width; x++) {
+ imRef(output, x, y).r = imRef(input, x, y);
+ imRef(output, x, y).g = imRef(input, x, y);
+ imRef(output, x, y).b = imRef(input, x, y);
+ }
+ }
+ return output;
+}
+
+static image<float> *imageUCHARtoFLOAT(image<uchar> *input) {
+ int width = input->width();
+ int height = input->height();
+ image<float> *output = new image<float>(width, height, false);
+
+ for (int y = 0; y < height; y++) {
+ for (int x = 0; x < width; x++) {
+ imRef(output, x, y) = imRef(input, x, y);
+ }
+ }
+ return output;
+}
+
+static image<float> *imageINTtoFLOAT(image<int> *input) {
+ int width = input->width();
+ int height = input->height();
+ image<float> *output = new image<float>(width, height, false);
+
+ for (int y = 0; y < height; y++) {
+ for (int x = 0; x < width; x++) {
+ imRef(output, x, y) = imRef(input, x, y);
+ }
+ }
+ return output;
+}
+
+static image<uchar> *imageFLOATtoUCHAR(image<float> *input,
+ float min, float max) {
+ int width = input->width();
+ int height = input->height();
+ image<uchar> *output = new image<uchar>(width, height, false);
+
+ if (max == min)
+ return output;
+
+ float scale = UCHAR_MAX / (max - min);
+ for (int y = 0; y < height; y++) {
+ for (int x = 0; x < width; x++) {
+ uchar val = (uchar)((imRef(input, x, y) - min) * scale);
+ imRef(output, x, y) = bound(val, (uchar)0, (uchar)UCHAR_MAX);
+ }
+ }
+ return output;
+}
+
+static image<uchar> *imageFLOATtoUCHAR(image<float> *input) {
+ float min, max;
+ min_max(input, &min, &max);
+ return imageFLOATtoUCHAR(input, min, max);
+}
+
+static image<long> *imageUCHARtoLONG(image<uchar> *input) {
+ int width = input->width();
+ int height = input->height();
+ image<long> *output = new image<long>(width, height, false);
+
+ for (int y = 0; y < height; y++) {
+ for (int x = 0; x < width; x++) {
+ imRef(output, x, y) = imRef(input, x, y);
+ }
+ }
+ return output;
+}
+
+static image<uchar> *imageLONGtoUCHAR(image<long> *input, long min, long max) {
+ int width = input->width();
+ int height = input->height();
+ image<uchar> *output = new image<uchar>(width, height, false);
+
+ if (max == min)
+ return output;
+
+ float scale = UCHAR_MAX / (float)(max - min);
+ for (int y = 0; y < height; y++) {
+ for (int x = 0; x < width; x++) {
+ uchar val = (uchar)((imRef(input, x, y) - min) * scale);
+ imRef(output, x, y) = bound(val, (uchar)0, (uchar)UCHAR_MAX);
+ }
+ }
+ return output;
+}
+
+static image<uchar> *imageLONGtoUCHAR(image<long> *input) {
+ long min, max;
+ min_max(input, &min, &max);
+ return imageLONGtoUCHAR(input, min, max);
+}
+
+static image<uchar> *imageSHORTtoUCHAR(image<short> *input,
+ short min, short max) {
+ int width = input->width();
+ int height = input->height();
+ image<uchar> *output = new image<uchar>(width, height, false);
+
+ if (max == min)
+ return output;
+
+ float scale = UCHAR_MAX / (float)(max - min);
+ for (int y = 0; y < height; y++) {
+ for (int x = 0; x < width; x++) {
+ uchar val = (uchar)((imRef(input, x, y) - min) * scale);
+ imRef(output, x, y) = bound(val, (uchar)0, (uchar)UCHAR_MAX);
+ }
+ }
+ return output;
+}
+
+static image<uchar> *imageSHORTtoUCHAR(image<short> *input) {
+ short min, max;
+ min_max(input, &min, &max);
+ return imageSHORTtoUCHAR(input, min, max);
+}
+
+#endif
diff --git a/RWR/src/mean-shift/dvs_meanshift/include/graph3d/imutil.h b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/imutil.h
new file mode 100755
index 0000000000000000000000000000000000000000..f59c65d5dc5b9b7a749c44918321a0702c75a523
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/imutil.h
@@ -0,0 +1,66 @@
+/*
+Copyright (C) 2006 Pedro Felzenszwalb
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation; either version 2 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+*/
+
+/* some image utilities */
+
+#ifndef IMUTIL_H
+#define IMUTIL_H
+
+#include "graph3d/image.h"
+#include "graph3d/misc.h"
+
+/* compute minimum and maximum value in an image */
+template <class T>
+void min_max(image<T> *im, T *ret_min, T *ret_max) {
+ int width = im->width();
+ int height = im->height();
+
+ T min = imRef(im, 0, 0);
+ T max = imRef(im, 0, 0);
+ for (int y = 0; y < height; y++) {
+ for (int x = 0; x < width; x++) {
+ T val = imRef(im, x, y);
+ if (min > val)
+ min = val;
+ if (max < val)
+ max = val;
+ }
+ }
+
+ *ret_min = min;
+ *ret_max = max;
+}
+
+/* threshold image */
+template <class T>
+image<uchar> *threshold(image<T> *src, int t) {
+ int width = src->width();
+ int height = src->height();
+ image<uchar> *dst = new image<uchar>(width, height);
+
+ for (int y = 0; y < height; y++) {
+ for (int x = 0; x < width; x++) {
+ imRef(dst, x, y) = (imRef(src, x, y) >= t);
+ }
+ }
+
+ return dst;
+}
+
+#endif
+
diff --git a/RWR/src/mean-shift/dvs_meanshift/include/graph3d/misc.h b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/misc.h
new file mode 100755
index 0000000000000000000000000000000000000000..4e4a43961ef134a232badaf3c7ca1052b2fbca0e
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/misc.h
@@ -0,0 +1,65 @@
+/*
+Copyright (C) 2006 Pedro Felzenszwalb
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation; either version 2 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+*/
+
+/* random stuff */
+
+#ifndef MISC_H
+#define MISC_H
+
+#include <cmath>
+
+#ifndef M_PI
+#define M_PI 3.141592653589793
+#endif
+
+typedef unsigned char uchar;
+
+typedef struct { uchar r, g, b; } rgb;
+
+inline bool operator==(const rgb &a, const rgb &b) {
+ return ((a.r == b.r) && (a.g == b.g) && (a.b == b.b));
+}
+
+template <class T>
+inline T abs(const T &x) { return (x > 0 ? x : -x); };
+
+template <class T>
+inline int sign(const T &x) { return (x >= 0 ? 1 : -1); };
+
+template <class T>
+inline T square(const T &x) { return x*x; };
+
+template <class T>
+inline T bound(const T &x, const T &min, const T &max) {
+ return (x < min ? min : (x > max ? max : x));
+}
+
+template <class T>
+inline bool check_bound(const T &x, const T&min, const T &max) {
+ return ((x < min) || (x > max));
+}
+
+inline int vlib_round(float x) { return (int)(x + 0.5F); }
+
+inline int vlib_round(double x) { return (int)(x + 0.5); }
+
+inline double gaussian(double val, double sigma) {
+ return exp(-square(val/sigma)/2)/(sqrt(2*M_PI)*sigma);
+}
+
+#endif
diff --git a/RWR/src/mean-shift/dvs_meanshift/include/graph3d/pnmfile.h b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/pnmfile.h
new file mode 100755
index 0000000000000000000000000000000000000000..acc98bebd5f11ab19299bb102aa23fc40b3e0cfa
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/pnmfile.h
@@ -0,0 +1,211 @@
+/*
+Copyright (C) 2006 Pedro Felzenszwalb
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation; either version 2 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+*/
+
+/* basic image I/O */
+
+#ifndef PNM_FILE_H
+#define PNM_FILE_H
+
+#include <cstdlib>
+#include <climits>
+#include <cstring>
+#include <fstream>
+#include "graph3d/image.h"
+#include "graph3d/misc.h"
+
+#define BUF_SIZE 256
+
+class pnm_error { };
+
+static void read_packed(unsigned char *data, int size, std::ifstream &f) {
+ unsigned char c = 0;
+
+ int bitshift = -1;
+ for (int pos = 0; pos < size; pos++) {
+ if (bitshift == -1) {
+ c = f.get();
+ bitshift = 7;
+ }
+ data[pos] = (c >> bitshift) & 1;
+ bitshift--;
+ }
+}
+
+static void write_packed(unsigned char *data, int size, std::ofstream &f) {
+ unsigned char c = 0;
+
+ int bitshift = 7;
+ for (int pos = 0; pos < size; pos++) {
+ c = c | (data[pos] << bitshift);
+ bitshift--;
+ if ((bitshift == -1) || (pos == size-1)) {
+ f.put(c);
+ bitshift = 7;
+ c = 0;
+ }
+ }
+}
+
+/* read PNM field, skipping comments */
+static void pnm_read(std::ifstream &file, char *buf) {
+ char doc[BUF_SIZE];
+ char c;
+
+ file >> c;
+ while (c == '#') {
+ file.getline(doc, BUF_SIZE);
+ file >> c;
+ }
+ file.putback(c);
+
+ file.width(BUF_SIZE);
+ file >> buf;
+ file.ignore();
+}
+
+static image<uchar> *loadPBM(const char *name) {
+ char buf[BUF_SIZE];
+
+ /* read header */
+ std::ifstream file(name, std::ios::in | std::ios::binary);
+ pnm_read(file, buf);
+ if (strncmp(buf, "P4", 2))
+ throw pnm_error();
+
+ pnm_read(file, buf);
+ int width = atoi(buf);
+ pnm_read(file, buf);
+ int height = atoi(buf);
+
+ /* read data */
+ image<uchar> *im = new image<uchar>(width, height);
+ for (int i = 0; i < height; i++)
+ read_packed(imPtr(im, 0, i), width, file);
+
+ return im;
+}
+
+static void savePBM(image<uchar> *im, const char *name) {
+ int width = im->width();
+ int height = im->height();
+ std::ofstream file(name, std::ios::out | std::ios::binary);
+
+ file << "P4\n" << width << " " << height << "\n";
+ for (int i = 0; i < height; i++)
+ write_packed(imPtr(im, 0, i), width, file);
+}
+
+static image<uchar> *loadPGM(const char *name) {
+ char buf[BUF_SIZE];
+
+ /* read header */
+ std::ifstream file(name, std::ios::in | std::ios::binary);
+ pnm_read(file, buf);
+ if (strncmp(buf, "P5", 2))
+ throw pnm_error();
+
+ pnm_read(file, buf);
+ int width = atoi(buf);
+ pnm_read(file, buf);
+ int height = atoi(buf);
+
+ pnm_read(file, buf);
+ if (atoi(buf) > UCHAR_MAX)
+ throw pnm_error();
+
+ /* read data */
+ image<uchar> *im = new image<uchar>(width, height);
+ file.read((char *)imPtr(im, 0, 0), width * height * sizeof(uchar));
+
+ return im;
+}
+
+static void savePGM(image<uchar> *im, const char *name) {
+ int width = im->width();
+ int height = im->height();
+ std::ofstream file(name, std::ios::out | std::ios::binary);
+
+ file << "P5\n" << width << " " << height << "\n" << UCHAR_MAX << "\n";
+ file.write((char *)imPtr(im, 0, 0), width * height * sizeof(uchar));
+}
+
+static image<rgb> *loadPPM(const char *name) {
+ char buf[BUF_SIZE], doc[BUF_SIZE];
+
+ /* read header */
+ std::ifstream file(name, std::ios::in | std::ios::binary);
+ pnm_read(file, buf);
+ if (strncmp(buf, "P6", 2))
+ throw pnm_error();
+
+ pnm_read(file, buf);
+ int width = atoi(buf);
+ pnm_read(file, buf);
+ int height = atoi(buf);
+
+ pnm_read(file, buf);
+ if (atoi(buf) > UCHAR_MAX)
+ throw pnm_error();
+
+ /* read data */
+ image<rgb> *im = new image<rgb>(width, height);
+ file.read((char *)imPtr(im, 0, 0), width * height * sizeof(rgb));
+
+ return im;
+}
+
+static void savePPM(image<rgb> *im, const char *name) {
+ int width = im->width();
+ int height = im->height();
+ std::ofstream file(name, std::ios::out | std::ios::binary);
+
+ file << "P6\n" << width << " " << height << "\n" << UCHAR_MAX << "\n";
+ file.write((char *)imPtr(im, 0, 0), width * height * sizeof(rgb));
+}
+
+template <class T>
+void load_image(image<T> **im, const char *name) {
+ char buf[BUF_SIZE];
+
+ /* read header */
+ std::ifstream file(name, std::ios::in | std::ios::binary);
+ pnm_read(file, buf);
+ if (strncmp(buf, "VLIB", 9))
+ throw pnm_error();
+
+ pnm_read(file, buf);
+ int width = atoi(buf);
+ pnm_read(file, buf);
+ int height = atoi(buf);
+
+ /* read data */
+ *im = new image<T>(width, height);
+ file.read((char *)imPtr((*im), 0, 0), width * height * sizeof(T));
+}
+
+template <class T>
+void save_image(image<T> *im, const char *name) {
+ int width = im->width();
+ int height = im->height();
+ std::ofstream file(name, std::ios::out | std::ios::binary);
+
+ //file << "VLIB\n" << width << " " << height << "\n";
+ file.write((char *)imPtr(im, 0, 0), width * height * sizeof(T));
+}
+
+#endif
diff --git a/RWR/src/mean-shift/dvs_meanshift/include/graph3d/segment-graph.h b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/segment-graph.h
new file mode 100755
index 0000000000000000000000000000000000000000..ec97b7f59d059fc4ce5750123caa6716bfacf23c
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/segment-graph.h
@@ -0,0 +1,83 @@
+/*
+Copyright (C) 2006 Pedro Felzenszwalb
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation; either version 2 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+*/
+
+#ifndef SEGMENT_GRAPH
+#define SEGMENT_GRAPH
+
+#include <algorithm>
+#include <cmath>
+#include "graph3d/disjoint-set.h"
+
+// threshold function
+#define THRESHOLD(size, c) (c/size)
+
+typedef struct {
+ float w;
+ int a, b;
+} edge;
+
+bool operator<(const edge &a, const edge &b) {
+ return a.w < b.w;
+}
+
+/*
+ * Segment a graph
+ *
+ * Returns a disjoint-set forest representing the segmentation.
+ *
+ * num_vertices: number of vertices in graph.
+ * num_edges: number of edges in graph
+ * edges: array of edges.
+ * c: constant for treshold function.
+ */
+universe *segment_graph(int num_vertices, int num_edges, edge *edges,
+ float c) {
+ // sort edges by weight
+ std::sort(edges, edges + num_edges);
+
+ // make a disjoint-set forest
+ universe *u = new universe(num_vertices);
+
+ // init thresholds
+ float *threshold = new float[num_vertices];
+ for (int i = 0; i < num_vertices; i++)
+ threshold[i] = THRESHOLD(1,c);
+
+ // for each edge, in non-decreasing weight order...
+ for (int i = 0; i < num_edges; i++) {
+ edge *pedge = &edges[i];
+
+ // components conected by this edge
+ int a = u->find(pedge->a);
+ int b = u->find(pedge->b);
+ if (a != b) {
+ if ((pedge->w <= threshold[a]) &&
+ (pedge->w <= threshold[b])) {
+ u->join(a, b);
+ a = u->find(a);
+ threshold[a] = pedge->w + THRESHOLD(u->size(a), c);
+ }
+ }
+ }
+
+ // free up
+ delete threshold;
+ return u;
+}
+
+#endif
diff --git a/RWR/src/mean-shift/dvs_meanshift/include/graph3d/segment-image.h b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/segment-image.h
new file mode 100755
index 0000000000000000000000000000000000000000..6079e799f8194dfcdb27758f8d4a14d6f9bd122f
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/segment-image.h
@@ -0,0 +1,153 @@
+/*
+Copyright (C) 2006 Pedro Felzenszwalb
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation; either version 2 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+*/
+
+#ifndef SEGMENT_IMAGE
+#define SEGMENT_IMAGE
+
+#include <cstdlib>
+#include "graph3d/image.h"
+#include "graph3d/misc.h"
+#include "graph3d/filter.h"
+#include "graph3d/segment-graph.h"
+
+// random color
+rgb random_rgb(){
+ rgb c;
+ double r;
+
+ c.r = (uchar)random();
+ c.g = (uchar)random();
+ c.b = (uchar)random();
+
+ return c;
+}
+
+// dissimilarity measure between pixels
+static inline float diff(image<float> *r, image<float> *g, image<float> *b,
+ int x1, int y1, int x2, int y2) {
+ return sqrt(square(imRef(r, x1, y1)-imRef(r, x2, y2)) +
+ square(imRef(g, x1, y1)-imRef(g, x2, y2)) +
+ square(imRef(b, x1, y1)-imRef(b, x2, y2)));
+}
+
+/*
+ * Segment an image
+ *
+ * Returns a color image representing the segmentation.
+ *
+ * im: image to segment.
+ * sigma: to smooth the image.
+ * c: constant for treshold function.
+ * min_size: minimum component size (enforced by post-processing stage).
+ * num_ccs: number of connected components in the segmentation.
+ */
+image<rgb> *segment_image(image<rgb> *im, float sigma, float c, int min_size, int *num_ccs) {
+ int width = im->width();
+ int height = im->height();
+
+ image<float> *r = new image<float>(width, height);
+ image<float> *g = new image<float>(width, height);
+ image<float> *b = new image<float>(width, height);
+
+ // smooth each color channel
+ for (int y = 0; y < height; y++) {
+ for (int x = 0; x < width; x++) {
+ imRef(r, x, y) = imRef(im, x, y).r;
+ imRef(g, x, y) = imRef(im, x, y).g;
+ imRef(b, x, y) = imRef(im, x, y).b;
+ }
+ }
+ image<float> *smooth_r = smooth(r, sigma);
+ image<float> *smooth_g = smooth(g, sigma);
+ image<float> *smooth_b = smooth(b, sigma);
+ delete r;
+ delete g;
+ delete b;
+
+ // build graph
+ edge *edges = new edge[width*height*4];
+ int num = 0;
+ for (int y = 0; y < height; y++) {
+ for (int x = 0; x < width; x++) {
+ if (x < width-1) {
+ edges[num].a = y * width + x;
+ edges[num].b = y * width + (x+1);
+ edges[num].w = diff(smooth_r, smooth_g, smooth_b, x, y, x+1, y);
+ num++;
+ }
+
+ if (y < height-1) {
+ edges[num].a = y * width + x;
+ edges[num].b = (y+1) * width + x;
+ edges[num].w = diff(smooth_r, smooth_g, smooth_b, x, y, x, y+1);
+ num++;
+ }
+
+ if ((x < width-1) && (y < height-1)) {
+ edges[num].a = y * width + x;
+ edges[num].b = (y+1) * width + (x+1);
+ edges[num].w = diff(smooth_r, smooth_g, smooth_b, x, y, x+1, y+1);
+ num++;
+ }
+
+ if ((x < width-1) && (y > 0)) {
+ edges[num].a = y * width + x;
+ edges[num].b = (y-1) * width + (x+1);
+ edges[num].w = diff(smooth_r, smooth_g, smooth_b, x, y, x+1, y-1);
+ num++;
+ }
+ }
+ }
+ delete smooth_r;
+ delete smooth_g;
+ delete smooth_b;
+
+ // segment
+ universe *u = segment_graph(width*height, num, edges, c);
+
+ // post process small components
+ for (int i = 0; i < num; i++) {
+ int a = u->find(edges[i].a);
+ int b = u->find(edges[i].b);
+ if ((a != b) && ((u->size(a) < min_size) || (u->size(b) < min_size)))
+ u->join(a, b);
+ }
+ delete [] edges;
+ *num_ccs = u->num_sets();
+
+ image<rgb> *output = new image<rgb>(width, height);
+
+ // pick random colors for each component
+ rgb *colors = new rgb[width*height];
+ for (int i = 0; i < width*height; i++)
+ colors[i] = random_rgb();
+
+ for (int y = 0; y < height; y++) {
+ for (int x = 0; x < width; x++) {
+ int comp = u->find(y * width + x);
+ imRef(output, x, y) = colors[comp];
+ }
+ }
+
+ delete [] colors;
+ delete u;
+
+ return output;
+}
+
+#endif
diff --git a/RWR/src/mean-shift/dvs_meanshift/include/graph3d/segment.h b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/segment.h
new file mode 100755
index 0000000000000000000000000000000000000000..3696ec7e5f4c688630a7c65438ac2d71c5bf48fe
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/include/graph3d/segment.h
@@ -0,0 +1,19 @@
+/*
+ * segment.h
+ *
+ * Created on: Nov 3, 2016
+ * Author: fran
+ */
+
+#ifndef SEGMENT_H_
+#define SEGMENT_H_
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+
+void imadjust(const cv::Mat & src, cv::Mat & dst);
+void im2segment(cv::Mat input, cv::Mat cv_image_output, float sigma, float k, int min_size, int *num_ccs);
+
+#endif /* SEGMENT_H_ */
diff --git a/RWR/src/mean-shift/dvs_meanshift/include/image_reconstruct/image_reconst.h b/RWR/src/mean-shift/dvs_meanshift/include/image_reconstruct/image_reconst.h
new file mode 100755
index 0000000000000000000000000000000000000000..21877cd7ae229ba2f479c248202af9801847a469
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/include/image_reconstruct/image_reconst.h
@@ -0,0 +1,41 @@
+/*
+ * image_reconst.h
+ *
+ * Created on: Nov 3, 2016
+ * Author: fran
+ */
+
+#ifndef IMAGE_RECONST_H_
+#define IMAGE_RECONST_H_
+
+//#include <opencv2/opencv.hpp>
+//#include "opencv/cxcore.h"
+//#include "opencv/highgui.h"
+//#include "opencv2/core/core.hpp"
+//#include <opencv2/highgui/highgui.hpp>
+//#include <opencv2/highgui/highgui_c.h>
+//#include <opencv2/core/core.hpp>
+//#include <opencv2/imgproc/imgproc.hpp>
+#include "opencv2/core/core_c.h"
+#include "opencv2/videoio/legacy/constants_c.h"
+#include "opencv2/highgui/highgui_c.h"
+
+
+void dst(double *btest, double *bfinal, int h, int w);
+void idst(double *btest, double *bfinal, int h, int w);
+void transpose(double *mat, double *mat_t, int h, int w);
+
+void getGradientX(cv::Mat &img, cv::Mat &gx);
+void getGradientY(cv::Mat &img, cv::Mat &gy);
+void lapx(cv::Mat &gx, cv::Mat &gxx);
+void lapy(cv::Mat &gy, cv::Mat &gyy);
+
+void poisson_solver(cv::Mat &img, cv::Mat &gxx, cv::Mat &gyy, cv::Mat &result);
+void poisson_solver_jacobi(cv::Mat &img, cv::Mat &gxx, cv::Mat &gyy, cv::Mat &result);
+
+
+//IPL Image versions (legacy)
+void lapx( const IplImage *img, IplImage *gxx);
+void lapy( const IplImage *img, IplImage *gyy);
+void poisson_solver(const IplImage *img, IplImage *gxx , IplImage *gyy, cv::Mat &result);
+#endif /* IMAGE_RECONST_H_ */
diff --git a/RWR/src/mean-shift/dvs_meanshift/launch/dvs_mono.launch b/RWR/src/mean-shift/dvs_meanshift/launch/dvs_mono.launch
new file mode 100755
index 0000000000000000000000000000000000000000..07ee3e77b014e886dc38e111e162bb0223923097
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/launch/dvs_mono.launch
@@ -0,0 +1,39 @@
+<launch>
+ <!-- camera driver -->
+ <node name="dvs_ros_driver" pkg="dvs_ros_driver" type="dvs_ros_driver" output = "screen">
+ <rosparam command="load" file="$(find dvs_ros_driver)/config/fast.yaml" />
+ </node>
+
+ <!-- visualization -->
+ <node name="dvs_meanshift" pkg="dvs_meanshift" type="dvs_meanshift" output = "screen">
+ <!-- <param name="display_method" value="grayscale"/> -->
+ <param name="display_method" value="red-blue"/>
+ <remap from="events" to="/dvs/events" />
+ <remap from="camera_info" to="/dvs/camera_info" />
+ </node>
+
+ <!-- display -->
+ <node name="image_view" pkg="image_view" type="image_view" output = "screen">
+ <remap from="image" to="dvs_rendering"/>
+ </node>
+
+ <node name="image_view2" pkg="image_view" type="image_view" output = "screen">
+ <remap from="image" to="dvs_segmentation"/>
+ </node>
+
+ <node name="image_view3" pkg="image_view" type="image_view" output = "screen">
+ <remap from="image" to="dvs_debug1"/>
+ </node>
+
+ <node name="image_view4" pkg="image_view" type="image_view" output = "screen">
+ <remap from="image" to="dvs_debug2"/>
+ </node>
+
+ <!-- <node name="image_view2" pkg="image_view" type="image_view" output = "screen"> -->
+ <!-- <remap from="image" to="dvs_undistorted"/> -->
+ <!-- </node> -->
+
+ <!-- configure -->
+ <!-- <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" /> -->
+
+</launch>
diff --git a/RWR/src/mean-shift/dvs_meanshift/launch/dvs_segmentation.launch b/RWR/src/mean-shift/dvs_meanshift/launch/dvs_segmentation.launch
new file mode 100755
index 0000000000000000000000000000000000000000..8e1718f5e0a0f3fd2923a98319808bc320ee6c72
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/launch/dvs_segmentation.launch
@@ -0,0 +1,31 @@
+<launch>
+ <!-- camera driver -->
+ <node name="dvs_ros_driver" pkg="dvs_ros_driver" type="dvs_ros_driver" output = "screen">
+ <rosparam command="load" file="$(find dvs_ros_driver)/config/fast.yaml" />
+ </node>
+
+ <!-- visualization -->
+ <node name="dvs_meanshift" pkg="dvs_meanshift" type="dvs_meanshift" output = "screen">
+ <!-- <param name="display_method" value="grayscale"/> -->
+ <param name="display_method" value="red-blue"/>
+ <remap from="events" to="/dvs/events" />
+ <remap from="camera_info" to="/dvs/camera_info" />
+ </node>
+
+ <!-- display -->
+ <node name="image_view" pkg="image_view" type="image_view" output = "screen">
+ <remap from="image" to="dvs_rendering"/>
+ </node>
+
+ <node name="image_view2" pkg="rqt_image_view" type="rqt_image_view" output = "screen">
+ <remap from="image" to="dvs_segmentation"/>
+ </node>
+
+ <!-- <node name="image_view3" pkg="image_view" type="image_view" output = "screen"> -->
+ <!-- <remap from="image" to="dvs_debug1"/> -->
+ <!-- </node> -->
+
+ <!-- configure -->
+ <!-- <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" /> -->
+
+</launch>
diff --git a/RWR/src/mean-shift/dvs_meanshift/package.xml b/RWR/src/mean-shift/dvs_meanshift/package.xml
new file mode 100755
index 0000000000000000000000000000000000000000..05e2835bf6f75312d4d6c7d91b5ae1dba83d411f
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/package.xml
@@ -0,0 +1,66 @@
+<?xml version="1.0"?>
+<package>
+ <name>dvs_meanshift</name>
+ <version>0.0.0</version>
+ <description>The dvs_meanshift package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fran@todo.todo">fran</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/dvs_meanshift</url> -->
+
+
+ <!-- Author tags are optional, mutiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintianers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>cv_bridge</build_depend>
+ <build_depend>dvs_msgs</build_depend>
+ <build_depend>image_transport</build_depend>
+ <build_depend>pcl_ros</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>rospy</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <run_depend>cv_bridge</run_depend>
+ <run_depend>dvs_msgs</run_depend>
+ <run_depend>image_transport</run_depend>
+ <run_depend>pcl_ros</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>rospy</run_depend>
+ <run_depend>sensor_msgs</run_depend>
+ <run_depend>std_msgs</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
\ No newline at end of file
diff --git a/RWR/src/mean-shift/dvs_meanshift/src/COPYING b/RWR/src/mean-shift/dvs_meanshift/src/COPYING
new file mode 100755
index 0000000000000000000000000000000000000000..d511905c1647a1e311e8b20d5930a37a9c2531cd
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/src/COPYING
@@ -0,0 +1,339 @@
+ GNU GENERAL PUBLIC LICENSE
+ Version 2, June 1991
+
+ Copyright (C) 1989, 1991 Free Software Foundation, Inc.,
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+ Preamble
+
+ The licenses for most software are designed to take away your
+freedom to share and change it. By contrast, the GNU General Public
+License is intended to guarantee your freedom to share and change free
+software--to make sure the software is free for all its users. This
+General Public License applies to most of the Free Software
+Foundation's software and to any other program whose authors commit to
+using it. (Some other Free Software Foundation software is covered by
+the GNU Lesser General Public License instead.) You can apply it to
+your programs, too.
+
+ When we speak of free software, we are referring to freedom, not
+price. Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+this service if you wish), that you receive source code or can get it
+if you want it, that you can change the software or use pieces of it
+in new free programs; and that you know you can do these things.
+
+ To protect your rights, we need to make restrictions that forbid
+anyone to deny you these rights or to ask you to surrender the rights.
+These restrictions translate to certain responsibilities for you if you
+distribute copies of the software, or if you modify it.
+
+ For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must give the recipients all the rights that
+you have. You must make sure that they, too, receive or can get the
+source code. And you must show them these terms so they know their
+rights.
+
+ We protect your rights with two steps: (1) copyright the software, and
+(2) offer you this license which gives you legal permission to copy,
+distribute and/or modify the software.
+
+ Also, for each author's protection and ours, we want to make certain
+that everyone understands that there is no warranty for this free
+software. If the software is modified by someone else and passed on, we
+want its recipients to know that what they have is not the original, so
+that any problems introduced by others will not reflect on the original
+authors' reputations.
+
+ Finally, any free program is threatened constantly by software
+patents. We wish to avoid the danger that redistributors of a free
+program will individually obtain patent licenses, in effect making the
+program proprietary. To prevent this, we have made it clear that any
+patent must be licensed for everyone's free use or not licensed at all.
+
+ The precise terms and conditions for copying, distribution and
+modification follow.
+
+ GNU GENERAL PUBLIC LICENSE
+ TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+ 0. This License applies to any program or other work which contains
+a notice placed by the copyright holder saying it may be distributed
+under the terms of this General Public License. The "Program", below,
+refers to any such program or work, and a "work based on the Program"
+means either the Program or any derivative work under copyright law:
+that is to say, a work containing the Program or a portion of it,
+either verbatim or with modifications and/or translated into another
+language. (Hereinafter, translation is included without limitation in
+the term "modification".) Each licensee is addressed as "you".
+
+Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope. The act of
+running the Program is not restricted, and the output from the Program
+is covered only if its contents constitute a work based on the
+Program (independent of having been made by running the Program).
+Whether that is true depends on what the Program does.
+
+ 1. You may copy and distribute verbatim copies of the Program's
+source code as you receive it, in any medium, provided that you
+conspicuously and appropriately publish on each copy an appropriate
+copyright notice and disclaimer of warranty; keep intact all the
+notices that refer to this License and to the absence of any warranty;
+and give any other recipients of the Program a copy of this License
+along with the Program.
+
+You may charge a fee for the physical act of transferring a copy, and
+you may at your option offer warranty protection in exchange for a fee.
+
+ 2. You may modify your copy or copies of the Program or any portion
+of it, thus forming a work based on the Program, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+ a) You must cause the modified files to carry prominent notices
+ stating that you changed the files and the date of any change.
+
+ b) You must cause any work that you distribute or publish, that in
+ whole or in part contains or is derived from the Program or any
+ part thereof, to be licensed as a whole at no charge to all third
+ parties under the terms of this License.
+
+ c) If the modified program normally reads commands interactively
+ when run, you must cause it, when started running for such
+ interactive use in the most ordinary way, to print or display an
+ announcement including an appropriate copyright notice and a
+ notice that there is no warranty (or else, saying that you provide
+ a warranty) and that users may redistribute the program under
+ these conditions, and telling the user how to view a copy of this
+ License. (Exception: if the Program itself is interactive but
+ does not normally print such an announcement, your work based on
+ the Program is not required to print an announcement.)
+
+These requirements apply to the modified work as a whole. If
+identifiable sections of that work are not derived from the Program,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works. But when you
+distribute the same sections as part of a whole which is a work based
+on the Program, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Program.
+
+In addition, mere aggregation of another work not based on the Program
+with the Program (or with a work based on the Program) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+ 3. You may copy and distribute the Program (or a work based on it,
+under Section 2) in object code or executable form under the terms of
+Sections 1 and 2 above provided that you also do one of the following:
+
+ a) Accompany it with the complete corresponding machine-readable
+ source code, which must be distributed under the terms of Sections
+ 1 and 2 above on a medium customarily used for software interchange; or,
+
+ b) Accompany it with a written offer, valid for at least three
+ years, to give any third party, for a charge no more than your
+ cost of physically performing source distribution, a complete
+ machine-readable copy of the corresponding source code, to be
+ distributed under the terms of Sections 1 and 2 above on a medium
+ customarily used for software interchange; or,
+
+ c) Accompany it with the information you received as to the offer
+ to distribute corresponding source code. (This alternative is
+ allowed only for noncommercial distribution and only if you
+ received the program in object code or executable form with such
+ an offer, in accord with Subsection b above.)
+
+The source code for a work means the preferred form of the work for
+making modifications to it. For an executable work, complete source
+code means all the source code for all modules it contains, plus any
+associated interface definition files, plus the scripts used to
+control compilation and installation of the executable. However, as a
+special exception, the source code distributed need not include
+anything that is normally distributed (in either source or binary
+form) with the major components (compiler, kernel, and so on) of the
+operating system on which the executable runs, unless that component
+itself accompanies the executable.
+
+If distribution of executable or object code is made by offering
+access to copy from a designated place, then offering equivalent
+access to copy the source code from the same place counts as
+distribution of the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+ 4. You may not copy, modify, sublicense, or distribute the Program
+except as expressly provided under this License. Any attempt
+otherwise to copy, modify, sublicense or distribute the Program is
+void, and will automatically terminate your rights under this License.
+However, parties who have received copies, or rights, from you under
+this License will not have their licenses terminated so long as such
+parties remain in full compliance.
+
+ 5. You are not required to accept this License, since you have not
+signed it. However, nothing else grants you permission to modify or
+distribute the Program or its derivative works. These actions are
+prohibited by law if you do not accept this License. Therefore, by
+modifying or distributing the Program (or any work based on the
+Program), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Program or works based on it.
+
+ 6. Each time you redistribute the Program (or any work based on the
+Program), the recipient automatically receives a license from the
+original licensor to copy, distribute or modify the Program subject to
+these terms and conditions. You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties to
+this License.
+
+ 7. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License. If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Program at all. For example, if a patent
+license would not permit royalty-free redistribution of the Program by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Program.
+
+If any portion of this section is held invalid or unenforceable under
+any particular circumstance, the balance of the section is intended to
+apply and the section as a whole is intended to apply in other
+circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system, which is
+implemented by public license practices. Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+ 8. If the distribution and/or use of the Program is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Program under this License
+may add an explicit geographical distribution limitation excluding
+those countries, so that distribution is permitted only in or among
+countries not thus excluded. In such case, this License incorporates
+the limitation as if written in the body of this License.
+
+ 9. The Free Software Foundation may publish revised and/or new versions
+of the General Public License from time to time. Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+Each version is given a distinguishing version number. If the Program
+specifies a version number of this License which applies to it and "any
+later version", you have the option of following the terms and conditions
+either of that version or of any later version published by the Free
+Software Foundation. If the Program does not specify a version number of
+this License, you may choose any version ever published by the Free Software
+Foundation.
+
+ 10. If you wish to incorporate parts of the Program into other free
+programs whose distribution conditions are different, write to the author
+to ask for permission. For software which is copyrighted by the Free
+Software Foundation, write to the Free Software Foundation; we sometimes
+make exceptions for this. Our decision will be guided by the two goals
+of preserving the free status of all derivatives of our free software and
+of promoting the sharing and reuse of software generally.
+
+ NO WARRANTY
+
+ 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
+FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN
+OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
+PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
+OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS
+TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE
+PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
+REPAIR OR CORRECTION.
+
+ 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
+REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
+INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
+OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
+TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
+YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
+PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGES.
+
+ END OF TERMS AND CONDITIONS
+
+ How to Apply These Terms to Your New Programs
+
+ If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+ To do so, attach the following notices to the program. It is safest
+to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+ <one line to give the program's name and a brief idea of what it does.>
+ Copyright (C) <year> <name of author>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License along
+ with this program; if not, write to the Free Software Foundation, Inc.,
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+
+Also add information on how to contact you by electronic and paper mail.
+
+If the program is interactive, make it output a short notice like this
+when it starts in an interactive mode:
+
+ Gnomovision version 69, Copyright (C) year name of author
+ Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+ This is free software, and you are welcome to redistribute it
+ under certain conditions; type `show c' for details.
+
+The hypothetical commands `show w' and `show c' should show the appropriate
+parts of the General Public License. Of course, the commands you use may
+be called something other than `show w' and `show c'; they could even be
+mouse-clicks or menu items--whatever suits your program.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the program, if
+necessary. Here is a sample; alter the names:
+
+ Yoyodyne, Inc., hereby disclaims all copyright interest in the program
+ `Gnomovision' (which makes passes at compilers) written by James Hacker.
+
+ <signature of Ty Coon>, 1 April 1989
+ Ty Coon, President of Vice
+
+This General Public License does not permit incorporating your program into
+proprietary programs. If your program is a subroutine library, you may
+consider it more useful to permit linking proprietary applications with the
+library. If this is what you want to do, use the GNU Lesser General
+Public License instead of this License.
diff --git a/RWR/src/mean-shift/dvs_meanshift/src/README b/RWR/src/mean-shift/dvs_meanshift/src/README
new file mode 100755
index 0000000000000000000000000000000000000000..f8e9166aaa4f44a48c9e7f907db28909edfc6890
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/src/README
@@ -0,0 +1,25 @@
+
+Implementation of the segmentation algorithm described in:
+
+Efficient Graph-Based Image Segmentation
+Pedro F. Felzenszwalb and Daniel P. Huttenlocher
+International Journal of Computer Vision, 59(2) September 2004.
+
+The program takes a color image (PPM format) and produces a segmentation
+with a random color assigned to each region.
+
+1) Type "make" to compile "segment".
+
+2) Run "segment sigma k min input output".
+
+The parameters are: (see the paper for details)
+
+sigma: Used to smooth the input image before segmenting it.
+k: Value for the threshold function.
+min: Minimum component size enforced by post-processing.
+input: Input image.
+output: Output image.
+
+Typical parameters are sigma = 0.5, k = 500, min = 20.
+Larger values for k result in larger components in the result.
+
diff --git a/RWR/src/mean-shift/dvs_meanshift/src/color_meanshift.cpp b/RWR/src/mean-shift/dvs_meanshift/src/color_meanshift.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..f1c8d2c021ec0a1f9ad62c933c3d28579b26d853
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/src/color_meanshift.cpp
@@ -0,0 +1,739 @@
+#include <cstring>
+#include <cmath>
+#include <cstdlib>
+#include "dvs_meanshift/color_meanshift.h"
+
+#define max(x,y) ((x)>(y)?(x):(y))
+#define min(x,y) ((x)<(y)?(x):(y))
+
+#define _2D_to_linear(row, col, maxCol) ((row)*(maxCol)+(col))
+//#define _3D_to_linear(row, col, color, maxRow, maxCol) ((maxRow)*(maxCol)*(color)+(row)*(maxCol)+(col))
+
+//#define _2D_to_linear(row, col, maxRow) ((col)*(maxRow)+(row))
+//#define _3D_to_linear(row, col, color, maxRow, maxCol) ((maxRow)*(maxCol)*(color)+(col)*(maxRow)+(row))
+//void colorMeanShiftFilt_helper_C(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
+
+/*void colorMeanShiftFilt(double *newPixelsPtr, const double *pixelsPtr, int mPixels, int maxPixelDim, const double *imagePtr, int numRows, int numCols, float spaceDivider, char *kernelFun, int maxIterNum, float tolFun, double *newImagePtr)
+{
+ // Copy the original pixel values to the new pixel matrix
+ memcpy(newPixelsPtr, pixelsPtr, maxPixelDim*mPixels*sizeof(double));
+
+ // Copy the original image to a new image
+ memcpy(newImagePtr, imagePtr, mPixels*sizeof(double));
+
+ // Help variables
+ double x,y;
+ int rX, rY;
+ int neighX, neighY;
+ bool bRepeat=false;
+
+ double *delta = (double *) calloc(maxPixelDim,sizeof(double));
+ double *dX = (double *) calloc(maxPixelDim,sizeof(double));
+ double *neighVec = (double *) calloc(maxPixelDim,sizeof(double));
+ double deltaGrad2;
+
+ // Create an array that indicates if we reached the end of the optimization for each pixel
+ double *optimizedPointArray=new double[mPixels];
+
+ for (int iterNum=0; iterNum<maxIterNum; iterNum++)
+ {
+ bRepeat=false;
+
+ for (int pointNum=0; pointNum<mPixels; pointNum++)
+ {
+ if (optimizedPointArray[pointNum]==1) // The optimization for this pixel has reached the threshold
+ continue;
+
+ x=newPixelsPtr[_2D_to_linear(0, pointNum, maxPixelDim)]*spaceDivider;
+ y=newPixelsPtr[_2D_to_linear(1, pointNum, maxPixelDim)]*spaceDivider;
+
+ // Reset the delta variables
+ delta[0]=0; delta[1]=0; delta[2]=0;
+ dX[0]=0; dX[1]=0; dX[2]=0;
+
+ //ROS_ERROR("y = %d, x = %d, pointNum = %d, spaceDivider = %g", (int)floor(y), (int)floor(x), pointNum, spaceDivider);
+
+ double sumF=0;
+ double dist2;
+
+
+ // Find the neighbors around point (rX,rY)
+ //for (neighX=max(0, floor(x)-1-spaceDivider); neighX<min(numCols, ceil(x)+spaceDivider); neighX++)
+ for (neighX=max(0, floor(x)-spaceDivider); neighX<min(numCols, ceil(x)+spaceDivider); neighX++)
+ {
+ //for (neighY=max(0,floor(y)-1-spaceDivider); neighY<min(numRows, ceil(y)+spaceDivider); neighY++)
+ for (neighY=max(0,floor(y)-spaceDivider); neighY<min(numRows, ceil(y)+spaceDivider); neighY++)
+ {
+ neighVec[0]=(neighX)/spaceDivider;
+ neighVec[1]=(neighY)/spaceDivider;
+ neighVec[2]=newImagePtr[neighY*numCols+neighX];
+
+
+ delta[0]=newPixelsPtr[_2D_to_linear(0,pointNum,maxPixelDim)]-neighVec[0];
+ delta[1]=newPixelsPtr[_2D_to_linear(1,pointNum,maxPixelDim)]-neighVec[1];
+ delta[2]=newPixelsPtr[_2D_to_linear(2,pointNum,maxPixelDim)]-neighVec[2];
+
+ dist2 = delta[0]*delta[0] + delta[1]*delta[1] + delta[2]*delta[2];
+
+ double kres=(exp(-dist2/2.0));
+
+ dX[0] += kres*neighVec[0];
+ dX[1] += kres*neighVec[1];
+ dX[2] += kres*neighVec[2];
+
+ sumF+=kres;
+
+ //ROS_ERROR("newImagePtr[%d, %d]= %g", (int)(neighX), (int)(neighY), neighVec[2]);
+ }
+ }
+
+ deltaGrad2=0;
+
+
+ dX[0]=(sumF==0)?0:dX[0]/sumF; //check for NaNs
+ dX[1]=(sumF==0)?0:dX[1]/sumF; //check for NaNs
+ dX[2]=(sumF==0)?0:dX[2]/sumF; //check for NaNs
+
+ double tmp0=newPixelsPtr[_2D_to_linear(0, pointNum, maxPixelDim)]-dX[0];
+ double tmp1=newPixelsPtr[_2D_to_linear(1, pointNum, maxPixelDim)]-dX[1];
+ double tmp2=newPixelsPtr[_2D_to_linear(2, pointNum, maxPixelDim)]-dX[2];
+
+ //double tmp1=newPixelsPtr[_2D_to_linear(i, pointNum, mPixels)]-dX[i];
+ deltaGrad2 = tmp0*tmp0 + tmp1*tmp1 + tmp2*tmp2;
+ newPixelsPtr[_2D_to_linear(0, pointNum, maxPixelDim)]=dX[0];
+ newPixelsPtr[_2D_to_linear(1, pointNum, maxPixelDim)]=dX[1];
+ newPixelsPtr[_2D_to_linear(2, pointNum, maxPixelDim)]=dX[2];
+
+ //ROS_ERROR("newImagePtr[%d, %d]= %g", (int)pointNum/numCols, (int)pointNum%numCols, dX[0]);
+
+ // Find the corresponding row and column of the point
+ newImagePtr[pointNum]=dX[2];
+
+ //ROS_ERROR("newImagePtr[%d, %d] = %g", pointCol, pointRow, dX[0]);
+
+ if (deltaGrad2<tolFun)
+ optimizedPointArray[pointNum]=1;
+ else
+ bRepeat=true;
+
+ } // end pointNum loop
+
+ if (bRepeat==false)
+ break;
+
+ } // end for(iterNum) loop
+
+ // CLEAN-UP
+ //delete[] newImagePtr;
+ delete[] optimizedPointArray;
+ delete(delta);
+ delete(dX);
+ delete(neighVec);
+}*/
+
+/*void colorMeanShiftFilt(double *newPixelsPtr, const double *pixelsPtr, int mPixels, int maxPixelDim, const double *imagePtr, int numRows, int numCols, float spaceDivider, char *kernelFun, int maxIterNum, float tolFun)
+{
+ // Copy the original pixel values to the new pixel matrix
+ memcpy(newPixelsPtr, pixelsPtr, maxPixelDim*mPixels*sizeof(double));
+
+ // Copy the original image to a new image
+ double *newImagePtr=new double[mPixels];
+ memcpy(newImagePtr, imagePtr, mPixels*sizeof(double));
+
+ // Help variables
+ double x,y;
+ int neighX, neighY;
+ bool bRepeat=false;
+
+ double *delta = (double *) calloc(maxPixelDim,sizeof(double));
+ double *dX = (double *) calloc(maxPixelDim,sizeof(double));
+ double *neighVec = (double *) calloc(maxPixelDim,sizeof(double));
+ double deltaGrad2;
+
+ // Create an array that indicates if we reached the end of the optimization for each pixel
+ double *optimizedPointArray=new double[mPixels];
+
+ for (int iterNum=0; iterNum<maxIterNum; iterNum++)
+ {
+ bRepeat=false;
+ for (int pointNum=0; pointNum<mPixels; pointNum++)
+ {
+ if (optimizedPointArray[pointNum]==1) // The optimization for this pixel has reached the threshold
+ continue;
+
+ x=newPixelsPtr[_2D_to_linear(0, pointNum, mPixels)]*spaceDivider;
+ y=newPixelsPtr[_2D_to_linear(1, pointNum, mPixels)]*spaceDivider;
+
+ delta[0]=0; delta[1]=0; delta[2]=0;
+ dX[0]=0; dX[1]=0; dX[2]=0;
+
+ //ROS_ERROR("y = %d, x = %d, pointNum = %d, spaceDivider = %g", (int)floor(y), (int)floor(x), pointNum, spaceDivider);
+
+ double sumF=0;
+ double dist2, kres;
+
+ // Find the neighbors around point (rX,rY)
+ for (neighX=max(0, floor(x)-1-spaceDivider); neighX<min(numCols, ceil(x)+spaceDivider); neighX++)
+ {
+ for (neighY=max(0,floor(y)-1-spaceDivider); neighY<min(numRows, ceil(y)+spaceDivider); neighY++)
+ {
+ neighVec[0]=(neighX+1)/spaceDivider;
+ neighVec[1]=(neighY+1)/spaceDivider;
+ neighVec[2]=newImagePtr[neighY*numCols+neighX];
+
+ delta[0]=newPixelsPtr[_2D_to_linear(0,pointNum,mPixels)]-neighVec[0];
+ delta[1]=newPixelsPtr[_2D_to_linear(1,pointNum,mPixels)]-neighVec[1];
+ delta[2]=newPixelsPtr[_2D_to_linear(2,pointNum,mPixels)]-neighVec[2];
+
+ dist2 = delta[0]*delta[0]+delta[1]*delta[1]+delta[2]*delta[2];
+
+ kres =(exp(-dist2/2.0));
+
+ dX[0]+= kres*neighVec[0];
+ dX[1]+= kres*neighVec[1];
+ dX[2]+= kres*neighVec[2];
+ sumF +=kres;
+ }
+ }
+
+ // Find the corresponding row and column of the point
+ int pointRow = pointNum/numCols;
+ int pointCol = pointNum%numCols;
+
+ dX[0]=(sumF==0)?0:dX[0]/sumF; //check for NaNs
+ dX[1]=(sumF==0)?0:dX[1]/sumF; //check for NaNs
+ dX[2]=(sumF==0)?0:dX[2]/sumF; //check for NaNs
+ //double tmp1=newPixelsPtr[_2D_to_linear(i, pointNum, maxPixelDim)]-dX[i];
+ double tmp1=newPixelsPtr[_2D_to_linear(0, pointNum, mPixels)]-dX[0];
+ double tmp2=newPixelsPtr[_2D_to_linear(1, pointNum, mPixels)]-dX[1];
+ double tmp3=newPixelsPtr[_2D_to_linear(2, pointNum, mPixels)]-dX[2];
+
+ deltaGrad2=tmp1*tmp1+tmp2*tmp2+tmp3*tmp3;
+ newPixelsPtr[_2D_to_linear(0, pointNum, mPixels)]=dX[0];
+ newPixelsPtr[_2D_to_linear(1, pointNum, mPixels)]=dX[1];
+ newPixelsPtr[_2D_to_linear(2, pointNum, mPixels)]=dX[2];
+ //newImagePtr[pointRow*numCols+pointCol]=dX[2];
+ newImagePtr[pointNum]=dX[2];
+
+ if (deltaGrad2<tolFun)
+ optimizedPointArray[pointNum]=1;
+ else
+ bRepeat=true;
+
+ } // end pointNum loop
+
+ if (bRepeat==false)
+ break;
+
+ } // end for(iterNum) loop
+
+
+ // CLEAN-UP
+ delete[] newImagePtr;
+ delete[] optimizedPointArray;
+ delete(delta);
+ delete(dX);
+ delete(neighVec);
+}*/
+
+/*void meanshiftCluster_Gaussian(cv::Mat dataPts, std::vector<double> *clusterCenterX, std::vector<double> *clusterCenterY, std::vector<double> *clusterCenterZ, std::vector<int> *point2Clusters, double bandwidth)
+{
+ //Initialization
+ int numPts = dataPts.cols;
+ int numDim = dataPts.rows;
+ int numClust = 0;
+ double bandSq = bandwidth*bandwidth;
+ cv::Range rng = cv::Range(1,numPts);
+
+ cv::Mat onesAux = cv::Mat::ones(1, dataPts.cols, dataPts.type());
+
+ std::vector<int> initPtInds;
+ for (int i = rng.start; i <= rng.end; i++) initPtInds.push_back(i);
+ double stopThresh = 1E-3*bandwidth; //when mean has converged
+ int numInitPts = numPts; //track if a points been seen already
+ cv::Mat beenVisitedFlag = cv::Mat(1, numPts, CV_32S, cv::Scalar::all(0)); //number of points to posibaly use as initilization points
+ cv::Mat clusterVotes; //used to resolve conflicts on cluster membership
+
+ double lambda = 10.;
+ double myMeanX, myMeanY, myMeanZ, myOldMeanX, myOldMeanY, myOldMeanZ, totalWeightX, totalWeightY, totalWeightZ;
+ int stInd;
+
+ //std::vector<double> clusterCenterX, clusterCenterY, clusterCenterZ;
+
+ int tempInd;
+
+ while (numInitPts>0)
+ {
+ tempInd = (rand()%numInitPts); //pick a random seed point
+ stInd = initPtInds[tempInd]; //use this point as start of mean
+ myMeanX = dataPts.at<double>(cv::Point(stInd, 0)); //intilize mean to this points location
+ myMeanY = dataPts.at<double>(cv::Point(stInd, 1));
+ myMeanZ = dataPts.at<double>(cv::Point(stInd, 2));
+ cv::Mat thisClusterVotes = cv::Mat(1, numPts, CV_32S, cv::Scalar::all(0)); //used to resolve conflicts on cluster membership
+ //ROS_ERROR_STREAM("Before getting into while myMean = ["<<myMeanX<<", "<<myMeanY<<", "<<myMeanZ<<"]");
+ cv::Mat_<double> myMean = ( cv::Mat_<double>(3, 1) << myMeanX, myMeanY, myMeanZ);
+
+
+
+ while(true)
+ {
+ cv::Mat dataX, dataY, dataZ;
+ dataX.push_back(dataPts.row(0));
+ dataY.push_back(dataPts.row(1));
+ dataZ.push_back(dataPts.row(2));
+
+ cv::Mat diffX = dataX - myMeanX;
+ cv::Mat diffY = dataY - myMeanY;
+ cv::Mat diffZ = dataZ - myMeanZ;
+
+ ROS_ERROR_STREAM("Original = "<<diffX);
+ ROS_ERROR_STREAM("onesAux.rows = "<<onesAux.rows<<" cols = "<<onesAux.cols);
+ ROS_ERROR_STREAM("dataPts.rows = "<<dataPts.rows<<" cols = "<<dataPts.cols);
+ ROS_ERROR_STREAM("myMean.rows = "<<myMean.rows<<" cols = "<<myMean.cols);
+
+ cv::Mat diff = dataPts - myMean*onesAux;
+ diffX.push_back(diff.row(0));
+ diffY.push_back(diff.row(1));
+ diffZ.push_back(diff.row(2));
+
+ ROS_ERROR_STREAM("Matricial "<<diff.row(0));
+ exit(0);
+
+ cv::Mat diffXX = diffX.mul(diffX);
+ cv::Mat diffYY = diffY.mul(diffY);
+ cv::Mat diffZZ = diffZ.mul(diffZ);
+
+ cv::Mat sqDistToAll = diffXX + diffYY + diffZZ; //dist squared from mean to all points still active
+ cv::Mat inInds = (sqDistToAll < bandSq); // Puts 255 wherever it is true
+ //ROS_ERROR_STREAM("This is the size "<<inInds.size());
+ //ROS_ERROR_STREAM("This is the size "<<thisClusterVotes.size());
+ //inInds.convertTo(inInds, CV_8UC1);
+ //cv::add(thisClusterVotes, cv::Mat::ones(1, numPts, CV_32S), thisClusterVotes, inInds); //add a vote for all the in points belonging to this cluster
+ cv::add(thisClusterVotes, cv::Scalar(1), thisClusterVotes, inInds); //add a vote for all the in points belonging to this cluster
+ //ROS_ERROR_STREAM("thisclusterVotes = "<<inInds);
+ //ROS_ERROR_STREAM("thisclusterVotes = "<<thisClusterVotes);
+ //ROS_ERROR_STREAM("thisclusterVotes = "<<sqDistToAll);
+ //exit(0);
+
+ cv::Mat selectedDataX, selectedDataY, selectedDataZ;
+ //cv::bitwise_and(dataX, cv::Scalar(255), selectedDataX, inInds);
+ //cv::bitwise_and(dataY, cv::Scalar(255), selectedDataY, inInds);
+ //cv::bitwise_and(dataZ, cv::Scalar(255), selectedDataZ, inInds);
+ //ROS_ERROR_STREAM("Before: dataX ="<<dataX);
+ dataX.setTo(0., inInds==0);
+ dataY.setTo(0., inInds==0);
+ dataZ.setTo(0., inInds==0);
+ //ROS_ERROR_STREAM("inInds ="<<inInds);
+ //ROS_ERROR_STREAM("After: dataX ="<<dataX);
+
+ double numOfOnes = (double) (cv::sum(inInds)[0])/255; //inInds is active if inInds[i]==255
+ //ROS_ERROR_STREAM("Num of Ones ="<<numOfOnes);
+ //ROS_ERROR_STREAM("inInds ="<<inInds);
+ //exit(0);
+ myOldMeanX = myMeanX; myOldMeanY = myMeanY; myOldMeanZ = myMeanZ;
+ myMeanX = cv::sum(dataX)[0]/numOfOnes;
+ myMeanY = cv::sum(dataY)[0]/numOfOnes;
+ myMeanZ = cv::sum(dataZ)[0]/numOfOnes;
+
+
+ //ROS_ERROR_STREAM("inInds="<<inInds);
+ //ROS_ERROR_STREAM("After: dataX="<<dataX);
+
+ diffX = dataX - myMeanX;
+ diffY = dataY - myMeanY;
+ diffZ = dataZ - myMeanZ;
+
+ diffXX = diffXX.mul(diffXX);
+ diffYY = diffYY.mul(diffYY);
+ diffZZ = diffZZ.mul(diffZZ);
+
+ cv::Mat weightX, weightY, weightZ;
+ cv::exp(diffXX/lambda, weightX);
+ cv::exp(diffYY/lambda, weightY);
+ cv::exp(diffZZ/lambda, weightZ);
+ //cv::Mat wDataX = weightX.mul(selectedDataX);
+ //cv::Mat wDataY = weightY.mul(selectedDataY);
+ //cv::Mat wDataZ = weightZ.mul(selectedDataZ);
+
+ weightX.setTo(0., inInds==0); //Again, because the exp and the x - u make invalid values non-zero (inInds)
+ weightY.setTo(0., inInds==0);
+ weightZ.setTo(0., inInds==0);
+
+ cv::Mat wDataX = weightX.mul(dataX);
+ cv::Mat wDataY = weightY.mul(dataY);
+ cv::Mat wDataZ = weightZ.mul(dataZ);
+ totalWeightX = cv::sum(weightX)[0];
+ totalWeightY = cv::sum(weightY)[0];
+ totalWeightZ = cv::sum(weightZ)[0];
+ myMeanX = cv::sum(wDataX)[0]/totalWeightX;
+ myMeanY = cv::sum(wDataY)[0]/totalWeightY;
+ myMeanZ = cv::sum(wDataZ)[0]/totalWeightZ;
+
+ //cv::add(beenVisitedFlag, cv::Scalar(1.f), beenVisitedFlag, inInds);
+ beenVisitedFlag.setTo(1, inInds);
+ //ROS_ERROR_STREAM("myMean = ["<<myMeanX<<", "<<myMeanY<<", "<<myMeanZ<<"]");
+ //exit(0);
+ // if mean doesn't move much stop this cluster
+ if((myMeanX-myOldMeanX)*(myMeanX-myOldMeanX) + (myMeanY-myOldMeanY)*(myMeanY-myOldMeanY) + (myMeanZ-myOldMeanZ)*(myMeanZ-myOldMeanZ) < stopThresh)
+ {
+ //check for merge possibilities
+ int mergeWith = -1;
+ double distToOther;
+ for(int cN = 0; cN<numClust; cN++) //Careful!! cN goes from 1 to numClust!!!
+ {
+ double distToOther = (myMeanX - (*clusterCenterX)[cN])*(myMeanX - (*clusterCenterX)[cN]) + (myMeanY - (*clusterCenterY)[cN])*(myMeanY - (*clusterCenterY)[cN]) + (myMeanZ - (*clusterCenterZ)[cN])*(myMeanZ - (*clusterCenterZ)[cN]); //distance from posible new clust max to old clust max
+ //ROS_ERROR_STREAM("Distance to others "<<distToOther<<" and cN "<<cN);
+ //ROS_ERROR_STREAM("myMean = ["<<myMeanX<<", "<<myMeanY<<", "<<myMeanZ<<"]");
+ //ROS_ERROR_STREAM("clusterCenter = ["<<clusterCenterX[cN]<<", "<<clusterCenterY[cN]<<", "<<clusterCenterZ[cN]<<"]");
+ if(distToOther < bandwidth/2) //if its within bandwidth/2 merge new and old
+ {
+ mergeWith = cN;
+ break;
+ }
+ }
+
+ if(mergeWith > -1)
+ {
+ (*clusterCenterX)[mergeWith] = 0.5*(myMeanX+(*clusterCenterX)[mergeWith]);
+ (*clusterCenterY)[mergeWith] = 0.5*(myMeanY+(*clusterCenterY)[mergeWith]);
+ (*clusterCenterZ)[mergeWith] = 0.5*(myMeanZ+(*clusterCenterZ)[mergeWith]);
+
+ cv::Mat newClusterVotes = cv::Mat(clusterVotes.row(mergeWith)) + thisClusterVotes;
+ newClusterVotes.copyTo(clusterVotes.row(mergeWith));
+ }
+ else
+ {
+ numClust = numClust+1; //increment clusters
+ (*clusterCenterX).push_back(myMeanX); //record the mean
+ (*clusterCenterY).push_back(myMeanY);
+ (*clusterCenterZ).push_back(myMeanZ);
+
+ clusterVotes.push_back(thisClusterVotes); // add the new row for the new cluster
+ }
+ break;
+ }
+ }
+
+ std::vector<int> newInitPtInds; //we can initialize with any of the points not yet visited
+ for(int i=0; i<numPts; i++)
+ if (beenVisitedFlag.at<int>(cv::Point(i, 0)) == 0)
+ newInitPtInds.push_back(i);
+ initPtInds = newInitPtInds;
+ numInitPts = initPtInds.size(); //number of active points in set
+
+ //ROS_ERROR_STREAM("Num. of values still un clustered "<<numInitPts);
+ //ROS_ERROR_STREAM("beenVisitedFlag "<<beenVisitedFlag);
+ //for(int i=0; i<initPtInds.size(); ++i)
+ // std::cerr<<initPtInds[i] << ' ';
+ }
+ //exit(0);
+ //[val,data2cluster] = max(clusterVotes,[],1); //a point belongs to the cluster with the most votes
+
+ cv::Mat TclusterVotes = clusterVotes.t();
+
+ double min, max;
+ for(int i=0; i<TclusterVotes.rows; i++)
+ {
+ cv::minMaxIdx(cv::Mat(TclusterVotes.row(i)), &min, &max);
+ (*point2Clusters).push_back((int)max);
+ }
+
+ //ROS_ERROR_STREAM("Inside "<<(*point2Clusters).size());
+ //ROS_ERROR_STREAM("Number of clusters " <<numClust<<"and "<<clusterCenterX.size());
+}*/
+
+
+
+void meanshiftCluster_Gaussian(cv::Mat dataPts, std::vector<double> *clusterCenterX, std::vector<double> *clusterCenterY, std::vector<double> *clusterCenterZ, std::vector<int> *point2Clusters, double bandwidth)
+{
+ //Initialization
+ int numPts = dataPts.cols;
+ int numDim = dataPts.rows;
+ int numClust = 0;
+ double bandSq = bandwidth*bandwidth;
+ cv::Range rng = cv::Range(0,numPts-1);
+
+ cv::Mat onesAux = cv::Mat::ones(1, dataPts.cols, dataPts.type());
+
+ std::vector<int> initPtInds;
+ for (int i = rng.start; i <= rng.end; i++) initPtInds.push_back(i);
+ double stopThresh = 1E-3*bandwidth; //when mean has converged
+ int numInitPts = numPts; //track if a points been seen already
+ cv::Mat beenVisitedFlag = cv::Mat(1, numPts, CV_32S, cv::Scalar::all(0)); //number of points to posibaly use as initilization points
+ cv::Mat clusterVotes; //used to resolve conflicts on cluster membership
+
+ double lambda = 10.;
+ double myMeanX, myMeanY, myMeanZ, myOldMeanX, myOldMeanY, myOldMeanZ, totalWeightX, totalWeightY, totalWeightZ;
+ int stInd;
+
+ //std::vector<double> clusterCenterX, clusterCenterY, clusterCenterZ;
+
+
+ int tempInd;
+
+ while (numInitPts>0)
+ {
+ //ROS_ERROR_STREAM("iniPtInds");
+ //for(int i=0; i<initPtInds.size(); i++)
+ // std::cerr<<initPtInds[i]<<" ";
+ //std::cerr<<std::endl;
+
+
+ tempInd = (rand()%numInitPts); //pick a random seed point
+ //ROS_ERROR_STREAM("numInitPts ="<<numInitPts);
+
+ //std::cin.ignore();
+ stInd = initPtInds[tempInd]; //use this point as start of mean
+ /**********REMOVE THIS******************************/
+ //stInd = initPtInds[numInitPts-1];
+ /**********REMOVE THIS******************************/
+ myMeanX = dataPts.at<double>(cv::Point(stInd, 0)); //intilize mean to this points location
+ myMeanY = dataPts.at<double>(cv::Point(stInd, 1));
+ myMeanZ = dataPts.at<double>(cv::Point(stInd, 2));
+ cv::Mat thisClusterVotes = cv::Mat(1, numPts, CV_32S, cv::Scalar::all(0)); //used to resolve conflicts on cluster membership
+ //ROS_ERROR_STREAM("Before getting into while myMean = ["<<myMeanX<<", "<<myMeanY<<", "<<myMeanZ<<"]");
+ cv::Mat_<double> myMean = ( cv::Mat_<double>(3, 1) << myMeanX, myMeanY, myMeanZ);
+
+
+ while(true)
+ {
+ cv::Mat myDataPts;
+ dataPts.copyTo(myDataPts);
+
+ //ROS_ERROR_STREAM("Original = "<<diffX);
+ //ROS_ERROR_STREAM("onesAux.rows = "<<onesAux.rows<<" cols = "<<onesAux.cols);
+ //ROS_ERROR_STREAM("dataPts.rows = "<<dataPts.rows<<" cols = "<<dataPts.cols);
+ //ROS_ERROR_STREAM("myMean.rows = "<<myMean.rows<<" cols = "<<myMean.cols);
+
+ cv::Mat diff = myDataPts - myMean*onesAux;
+ cv::Mat diffdiff = diff.mul(diff);
+ cv::Mat sqDistToAll = diffdiff.row(0) +diffdiff.row(1) + diffdiff.row(2); //dist squared from mean to all points still active
+
+ cv::Mat inInds = (sqDistToAll < bandSq); // Puts 255 wherever it is true
+ cv::add(thisClusterVotes, cv::Scalar(1), thisClusterVotes, inInds); //add a vote for all the in points belonging to this cluster
+ cv::Mat mask3 = cv::repeat(inInds==0, 3, 1);
+
+ //ROS_ERROR_STREAM("BEFORE myMean = "<<myMean);
+ //ROS_ERROR_STREAM("sqDistToAll = "<<sqDistToAll);
+ //ROS_ERROR_STREAM("diffX = "<<diff.row(0));
+ //ROS_ERROR_STREAM("diffdiffX = "<<diffdiff.row(0));
+ //ROS_ERROR_STREAM("thisClusterVotes = "<<thisClusterVotes);
+ //ROS_ERROR_STREAM("dataPts = "<<dataPts);
+
+ myDataPts.setTo(0, mask3);
+ double numOfOnes = (double) (cv::sum(inInds)[0])/255; //inInds is active if inInds[i]==255
+
+ myOldMeanX = myMeanX; myOldMeanY = myMeanY; myOldMeanZ = myMeanZ;
+
+ myMeanX = cv::sum(myDataPts.row(0))[0]/numOfOnes;
+ myMeanY = cv::sum(myDataPts.row(1))[0]/numOfOnes;
+ myMeanZ = cv::sum(myDataPts.row(2))[0]/numOfOnes;
+
+ myMean = ( cv::Mat_<double>(3, 1) << myMeanX, myMeanY, myMeanZ);
+
+ //ROS_ERROR_STREAM("MIDDLE myMean = "<<myMean);
+
+ diff = myDataPts - myMean*onesAux;
+ diffdiff = diff.mul(diff);
+
+ cv::Mat weight;
+ cv::exp(diffdiff/lambda, weight);
+
+ weight.setTo(0., mask3); //Again, because the exp and the x - u make invalid values non-zero (inInds)
+ cv::Mat wData = weight.mul(myDataPts);
+
+ totalWeightX = cv::sum(weight.row(0))[0];
+ totalWeightY = cv::sum(weight.row(1))[0];
+ totalWeightZ = cv::sum(weight.row(2))[0];
+ myMeanX = cv::sum(wData.row(0))[0]/totalWeightX;
+ myMeanY = cv::sum(wData.row(1))[0]/totalWeightY;
+ myMeanZ = cv::sum(wData.row(2))[0]/totalWeightZ;
+
+ myMean = ( cv::Mat_<double>(3, 1) << myMeanX, myMeanY, myMeanZ);
+ //ROS_ERROR_STREAM("AFTER: myMean = "<<myMeanX<<","<<myMeanY<<","<<myMeanZ);
+ //exit(0);
+
+ beenVisitedFlag.setTo(1, inInds);
+ //ROS_ERROR_STREAM("beenVisitedFlag = "<<beenVisitedFlag);
+
+ // if mean doesn't move much stop this cluster
+ //ROS_ERROR_STREAM("Norm = "<<(myMeanX-myOldMeanX)*(myMeanX-myOldMeanX) + (myMeanY-myOldMeanY)*(myMeanY-myOldMeanY) + (myMeanZ-myOldMeanZ)*(myMeanZ-myOldMeanZ));
+ if((myMeanX-myOldMeanX)*(myMeanX-myOldMeanX) + (myMeanY-myOldMeanY)*(myMeanY-myOldMeanY) + (myMeanZ-myOldMeanZ)*(myMeanZ-myOldMeanZ) < stopThresh*stopThresh)
+ {
+ //check for merge posibilities
+ //ROS_ERROR_STREAM("Dentro!! ");
+ int mergeWith = -1;
+ double distToOther;
+ for(int cN = 0; cN<numClust; cN++) //Careful!! cN goes from 1 to numClust!!!
+ {
+ double distToOther = (myMeanX - (*clusterCenterX)[cN])*(myMeanX - (*clusterCenterX)[cN]) + (myMeanY - (*clusterCenterY)[cN])*(myMeanY - (*clusterCenterY)[cN]) + (myMeanZ - (*clusterCenterZ)[cN])*(myMeanZ - (*clusterCenterZ)[cN]); //distance from posible new clust max to old clust max
+ //ROS_ERROR_STREAM("Dentro!! ");
+ //ROS_ERROR_STREAM("distToOther " <<distToOther);
+ if(distToOther < (bandwidth/2)*(bandwidth/2)) //if its within bandwidth/2 merge new and old
+ {
+ mergeWith = cN;
+ break;
+ }
+ }
+
+ if(mergeWith > -1)
+ {
+ //ROS_ERROR_STREAM("Merging cluster with #"<<mergeWith);
+ //ROS_ERROR_STREAM("NumClust = "<<numClust);
+ //exit(0);
+
+ (*clusterCenterX)[mergeWith] = 0.5*(myMeanX+(*clusterCenterX)[mergeWith]);
+ (*clusterCenterY)[mergeWith] = 0.5*(myMeanY+(*clusterCenterY)[mergeWith]);
+ (*clusterCenterZ)[mergeWith] = 0.5*(myMeanZ+(*clusterCenterZ)[mergeWith]);
+
+ cv::Mat newClusterVotes = cv::Mat(clusterVotes.row(mergeWith)) + thisClusterVotes;
+ newClusterVotes.copyTo(clusterVotes.row(mergeWith));
+
+ //ROS_ERROR_STREAM("clusterVotes = "<<clusterVotes);
+ //exit(0);
+ }
+ else
+ {
+ //ROS_ERROR_STREAM("Creating new cluster");
+ (*clusterCenterX).push_back(myMeanX); //record the mean
+ (*clusterCenterY).push_back(myMeanY);
+ (*clusterCenterZ).push_back(myMeanZ);
+ numClust = numClust+1; //increment clusters
+
+ clusterVotes.push_back(thisClusterVotes); // add the new row for the new cluster
+
+ //ROS_ERROR_STREAM("clusterVotes = "<<clusterVotes);
+ //ROS_ERROR_STREAM("ClusterCenterX = "<<(*clusterCenterX)[numClust-1]);
+ //ROS_ERROR_STREAM("ClusterCenterY = "<<(*clusterCenterY)[numClust-1]);
+ //ROS_ERROR_STREAM("ClusterCenterZ = "<<(*clusterCenterZ)[numClust-1]);
+ //exit(0);
+ }
+ break;
+ }
+ }
+
+ std::vector<int> newInitPtInds; //we can initialize with any of the points not yet visited
+ for(int i=0; i<numPts; i++)
+ if (beenVisitedFlag.at<int>(cv::Point(i, 0)) == 0)
+ newInitPtInds.push_back(i);
+ initPtInds = newInitPtInds;
+ numInitPts = initPtInds.size(); //number of active points in set
+ }
+
+ cv::Mat TclusterVotes = clusterVotes.t();
+ cv::Point minLoc;
+ cv::Point maxLoc;
+ double min, max;
+ for(int i=0; i<TclusterVotes.rows; i++)
+ {
+ cv::minMaxLoc(cv::Mat(TclusterVotes.row(i)), &min, &max, &minLoc, &maxLoc);
+ (*point2Clusters).push_back(maxLoc.x);
+ }
+
+ //ROS_ERROR_STREAM("Number of clusters " <<numClust<<" and "<<(*clusterCenterX).size());
+ //ROS_ERROR_STREAM("Tclusters rows " <<TclusterVotes.rows<<" cols "<<TclusterVotes.cols);
+}
+
+void colorMeanShiftFilt(double *newPixelsPtr, const double *pixelsPtr, int mPixels, int maxPixelDim, const double *imagePtr, int numRows, int numCols, float spaceDivider, char *kernelFun, int maxIterNum, float tolFun)
+{
+ // Copy the original pixel values to the new pixel matrix
+ memcpy(newPixelsPtr, pixelsPtr, maxPixelDim*mPixels*sizeof(double));
+
+ // Copy the original image to a new image
+ double *newImagePtr=new double[mPixels];
+ memcpy(newImagePtr, imagePtr, mPixels*sizeof(double));
+
+ // Help variables
+ double x,y,val;
+ int neighX, neighY;
+ bool bRepeat=false;
+
+ double *delta = (double *) calloc(maxPixelDim,sizeof(double));
+ double *dX = (double *) calloc(maxPixelDim,sizeof(double));
+ double *neighVec = (double *) calloc(maxPixelDim,sizeof(double));
+ double deltaGrad2;
+
+ // Create an array that indicates if we reached the end of the optimization for each pixel
+ double *optimizedPointArray=new double[mPixels];
+
+ for (int iterNum=0; iterNum<maxIterNum; iterNum++)
+ {
+ bRepeat=false;
+
+ cv::Mat XX=cv::Mat(numRows, numCols, CV_64F, newPixelsPtr);
+ cv::Mat YY=cv::Mat(numRows, numCols, CV_64F, newPixelsPtr+mPixels);
+ cv::Mat MM=cv::Mat(numRows, numCols, CV_64F, newPixelsPtr+2*mPixels);
+
+ for (int pointNum=0; pointNum<mPixels; pointNum++)
+ {
+ if (optimizedPointArray[pointNum]==1) // The optimization for this pixel has reached the threshold
+ continue;
+
+ x=newPixelsPtr[_2D_to_linear(0, pointNum, mPixels)];
+ y=newPixelsPtr[_2D_to_linear(1, pointNum, mPixels)];
+ val=newPixelsPtr[_2D_to_linear(2, pointNum, mPixels)];
+
+ //ROS_ERROR("y = %d, x = %d, pointNum = %d, spaceDivider = %g", (int)floor(y), (int)floor(x), pointNum, spaceDivider);
+ double sumF;
+
+ int x_coord = max(0, floor(x*spaceDivider)-1-spaceDivider);
+ int y_coord = max(0, floor(y*spaceDivider)-1-spaceDivider);
+ int width = min(2*spaceDivider+1, numCols-x_coord);
+ int height= min(2*spaceDivider+1, numRows-y_coord);
+
+ cv::Mat submatXX = XX(cv::Rect(x_coord, y_coord, width, height)).clone();
+ cv::Mat submatYY = YY(cv::Rect(x_coord, y_coord, width, height)).clone();
+ cv::Mat submatMM = MM(cv::Rect(x_coord, y_coord, width, height)).clone();
+
+ cv::Mat delta_XX, delta_YY, delta_MM, dist2_matrix, kres_matrix;
+ delta_XX = x - submatXX;
+ delta_YY = y - submatYY;
+ delta_MM = val - submatMM;
+ dist2_matrix = delta_XX.mul(delta_XX) + delta_YY.mul(delta_YY) + delta_MM.mul(delta_MM);
+ cv::exp(dist2_matrix/(-2.0), kres_matrix); //kres =(exp(-dist2/2.0));
+
+ dX[0] = cv::sum(kres_matrix.mul(submatXX))[0];//cv::sum() returns a cvScalar, to get the value we need .val(0)
+ dX[1] = cv::sum(kres_matrix.mul(submatYY))[0];
+ dX[2] = cv::sum(kres_matrix.mul(submatMM))[0];
+ sumF = cv::sum(kres_matrix)[0];
+
+ // Find the corresponding row and column of the point
+
+ dX[0]=(sumF==0)?0:dX[0]/sumF; //check for NaNs
+ dX[1]=(sumF==0)?0:dX[1]/sumF; //check for NaNs
+ dX[2]=(sumF==0)?0:dX[2]/sumF; //check for NaNs
+
+ double tmp1=newPixelsPtr[_2D_to_linear(0, pointNum, mPixels)]-dX[0];
+ double tmp2=newPixelsPtr[_2D_to_linear(1, pointNum, mPixels)]-dX[1];
+ double tmp3=newPixelsPtr[_2D_to_linear(2, pointNum, mPixels)]-dX[2];
+
+ deltaGrad2=tmp1*tmp1+tmp2*tmp2+tmp3*tmp3;
+ newPixelsPtr[_2D_to_linear(0, pointNum, mPixels)]=dX[0];
+ newPixelsPtr[_2D_to_linear(1, pointNum, mPixels)]=dX[1];
+ newPixelsPtr[_2D_to_linear(2, pointNum, mPixels)]=dX[2];
+
+ newImagePtr[pointNum]=dX[2];
+
+ if (deltaGrad2<tolFun)
+ optimizedPointArray[pointNum]=1;
+ else
+ bRepeat=true;
+
+ } // end pointNum loop
+
+ if (bRepeat==false)
+ break;
+
+ } // end for(iterNum) loop
+
+
+ // CLEAN-UP
+ delete[] newImagePtr;
+ delete[] optimizedPointArray;
+ delete(delta);
+ delete(dX);
+ delete(neighVec);
+}
+
+
+
diff --git a/RWR/src/mean-shift/dvs_meanshift/src/image_reconst.cpp b/RWR/src/mean-shift/dvs_meanshift/src/image_reconst.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..1d58720914bc49f62bbc0ce2dc8895e7d4052710
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/src/image_reconst.cpp
@@ -0,0 +1,568 @@
+/*
+ * image_reconst.cpp
+ *
+ * Created on: Nov 3, 2016
+ * Author: fran
+ */
+
+/*
+######################### Poisson Image Editing ############################
+
+Copyright (C) 2012 Siddharth Kherada
+Copyright (C) 2006-2012 Natural User Interface Group
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details. "
+
+##############################################################################
+*/
+
+#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <complex>
+#include "image_reconstruct/image_reconst.h"
+#include <math.h>
+#include <omp.h>
+
+//using namespace std;
+//using namespace cv;
+
+#define pi 3.1416
+
+#define ATF at<float>
+#define AT3F at<Vec3f>
+#define ATB at<uchar>
+#define AT3B at<Vec3b>
+
+/////////////////////Poisson Equation Solver by FFT/////////////////////
+void dst(double *btest, double *bfinal, int h, int w)
+{
+
+ unsigned long int idx;
+
+ cv::Mat temp = cv::Mat(2 * h + 2, 1, CV_32F);
+ cv::Mat res = cv::Mat(h, 1, CV_32F);
+
+ int p = 0;
+
+ for (int i = 0; i<w; i++)
+ {
+ temp.ATF(0, 0) = 0.0;
+
+ for (int j = 0, r = 1; j<h; j++, r++)
+ {
+ idx = j*w + i;
+ temp.ATF(r, 0) = (float)btest[idx];
+ }
+
+ temp.ATF(h + 1, 0) = 0.0;
+
+ for (int j = h - 1, r = h + 2; j >= 0; j--, r++)
+ {
+ idx = j*w + i;
+ temp.ATF(r, 0) = (float)(-1 * btest[idx]);
+ }
+
+ cv::Mat planes[] = { cv::Mat_<float>(temp), cv::Mat::zeros(temp.size(), CV_32F) };
+
+ cv::Mat complex1;
+ merge(planes, 2, complex1);
+
+ dft(complex1, complex1, 0, 0);
+
+ cv::Mat planes1[] = { cv::Mat::zeros(complex1.size(), CV_32F), cv::Mat::zeros(complex1.size(), CV_32F) };
+
+ split(complex1, planes1);
+
+ std::complex<double> two_i = std::sqrt(std::complex<double>(-1));
+
+ double fac = -2 * imag(two_i);
+
+ for (int c = 1, z = 0; c<h + 1; c++, z++)
+ {
+ res.ATF(z, 0) = (float)(planes1[1].ATF(c, 0) / fac);
+ }
+
+ for (int q = 0, z = 0; q<h; q++, z++)
+ {
+ idx = q*w + p;
+ bfinal[idx] = res.ATF(z, 0);
+ }
+ p++;
+ }
+
+}
+
+void idst(double *btest, double *bfinal, int h, int w)
+{
+ int nn = h + 1;
+ dst(btest, bfinal, h, w);
+ #pragma omp parallel for
+ for (int i = 0; i<h; i++)
+ for (int j = 0; j<w; j++)
+ {
+ unsigned long int idx = i*w + j;
+ bfinal[idx] = (double)(2 * bfinal[idx]) / nn;
+ }
+
+}
+
+void transpose(double *mat, double *mat_t, int h, int w)
+{
+
+ cv::Mat tmp = cv::Mat(h, w, CV_32FC1);
+ int p = 0;
+
+ #pragma omp parallel for
+ for (int i = 0; i < h; i++)
+ {
+ for (int j = 0; j < w; j++)
+ {
+
+ unsigned long int idx = i*(w)+j;
+ tmp.ATF(i, j) = (float)(mat[idx]);
+ }
+ }
+ cv::Mat tmp_t = tmp.t();
+
+ unsigned long int idx;
+ for (int i = 0; i < tmp_t.size().height; i++)
+ for (int j = 0; j<tmp_t.size().width; j++)
+ {
+ idx = i*tmp_t.size().width + j;
+ mat_t[idx] = tmp_t.ATF(i, j);
+ }
+
+}
+
+
+void getGradientX(cv::Mat &img, cv::Mat &gx)
+{
+ int height = img.rows;
+ int width = img.cols;
+ cv::Mat cat = repeat(img, 1, 2);
+ /*cat.col(width) = 0;*/
+ cv::Mat roimat = cat(cv::Rect(1, 0, width, height));
+ gx = roimat - img;
+ gx.col(width - 1) = 0;
+}
+
+// calculate vertical gradient, gy(i, j) = img(i, j+1) - img(i, j)
+void getGradientY(cv::Mat &img, cv::Mat &gy)
+{
+ int height = img.rows;
+ int width = img.cols;
+ cv::Mat cat = repeat(img, 2, 1);
+ /*cat.row(height) = 0;*/
+ cv::Mat roimat = cat(cv::Rect(0, 1, width, height));
+ gy = roimat - img;
+ gy.row(height - 1) = 0;
+}
+
+// calculate horizontal gradient, gxx(i+1, j) = gx(i+1, j) - gx(i, j)
+void lapx(cv::Mat &gx, cv::Mat &gxx)
+{
+ int height = gx.rows;
+ int width = gx.cols;
+ cv::Mat cat = repeat(gx, 1, 2);
+ /*cat.col(width - 1) = 0;*/
+ cv::Mat roi = cat(cv::Rect(width - 1, 0, width, height));
+ gxx = gx - roi;
+ gxx.col(0) = 0;
+}
+
+// calculate vertical gradient, gyy(i, j+1) = gy(i, j+1) - gy(i, j)
+void lapy(cv::Mat &gy, cv::Mat &gyy)
+{
+ int height = gy.rows;
+ int width = gy.cols;
+ cv::Mat cat = repeat(gy, 2, 1);
+ /*cat.row(height - 1) = 0;*/
+ cv::Mat roi = cat(cv::Rect(0, height - 1, width, height));
+ gyy = gy - roi;
+ gyy.row(0) = 0;
+}
+
+
+
+void poisson_solver(cv::Mat &img, cv::Mat &gxx, cv::Mat &gyy, cv::Mat &result){
+
+ int w = img.cols;
+ int h = img.rows;
+ int channel = img.channels();
+
+ unsigned long int idx;
+
+ cv::Mat lap = gxx + gyy;
+
+ cv::Mat bound(img);
+ bound(cv::Rect(1, 1, w - 2, h - 2)) = 0;
+ double *dir_boundary = new double[h*w];
+
+#pragma omp parallel for
+ for (int i = 1; i < h - 1; i++)
+ for (int j = 1; j < w - 1; j++)
+ {
+ unsigned long int idx = i*w + j;
+ if ( i == 1 || i == h - 2 || j == 1 || j == w - 2 )
+ dir_boundary[idx] = (int)bound.ATF(i, (j + 1)) + (int)bound.ATF(i, (j - 1)) + (int)bound.ATF(i - 1, j) + (int)bound.ATF(i + 1, j);
+ else dir_boundary[idx] = 0;
+
+ }
+
+
+ cv::Mat diff(h, w, CV_32FC1);
+
+#pragma omp parallel for
+ for (int i = 0; i<h; i++)
+ {
+ for (int j = 0; j<w; j++)
+ {
+ unsigned long int idx = i*w + j;
+ diff.ATF(i, j) = (float)(lap.ATF(i, j) - dir_boundary[idx]);
+ }
+ }
+
+ double *btemp = new double[(h - 2)*(w - 2)];
+#pragma omp parallel for
+ for (int i = 0; i < h - 2; i++)
+ {
+ for (int j = 0; j < w - 2; j++)
+ {
+ unsigned long int idx = i*(w - 2) + j;
+ btemp[idx] = diff.ATF(i + 1, j + 1);
+
+ }
+ }
+
+ double *bfinal = new double[(h - 2)*(w - 2)];
+ double *bfinal_t = new double[(h - 2)*(w - 2)];
+ double *denom = new double[(h - 2)*(w - 2)];
+ double *fres = new double[(h - 2)*(w - 2)];
+ double *fres_t = new double[(h - 2)*(w - 2)];
+
+
+ dst(btemp, bfinal, h - 2, w - 2);
+
+ transpose(bfinal, bfinal_t, h - 2, w - 2);
+
+ dst(bfinal_t, bfinal, w - 2, h - 2);
+
+ transpose(bfinal, bfinal_t, w - 2, h - 2);
+
+ int cx = 1;
+ int cy = 1;
+
+ for (int i = 0; i < w - 2; i++, cy++)
+ {
+ for (int j = 0, cx = 1; j < h - 2; j++, cx++)
+ {
+ idx = j*(w - 2) + i;
+ denom[idx] = (float)2 * cos(pi*cy / ((double)(w - 1))) - 2 + 2 * cos(pi*cx / ((double)(h - 1))) - 2;
+
+ }
+ }
+
+ for (idx = 0; (int)idx < (w - 2)*(h - 2); idx++)
+ {
+ bfinal_t[idx] = bfinal_t[idx] / denom[idx];
+ }
+
+
+ idst(bfinal_t, fres, h - 2, w - 2);
+
+ transpose(fres, fres_t, h - 2, w - 2);
+
+ idst(fres_t, fres, w - 2, h - 2);
+
+ transpose(fres, fres_t, w - 2, h - 2);
+
+
+ img.convertTo(result, CV_8UC1);
+
+
+ cv::Mat tmp = cv::Mat(h-2, w-2, CV_64F, fres_t);
+ double min, max;
+ cv::minMaxLoc(tmp, &min, &max);
+
+
+ #pragma omp parallel for
+ for (int i = 0; i < h - 2; i++)
+ {
+ for (int j = 0; j < w - 2; j++)
+ {
+ unsigned long int idx = i*(w - 2) + j;
+ //if (fres_t[idx] < 0.0)
+ // result.ATB(i+1, j+1) = 0;
+ //else if (fres_t[idx] > 255.0)
+ // result.ATB(i+1, j+1) = 255;
+ //else
+ // result.ATB(i+1, j+1) = (int)fres_t[idx];
+ result.ATB(i+1,j+1) = (int) (255*(fres_t[idx]-min)/(max-min));
+ }
+ }
+
+}
+
+void poisson_solver_jacobi(cv::Mat &img, cv::Mat &gxx, cv::Mat &gyy, cv::Mat &result) {
+ int w = img.cols;
+ int h = img.rows;
+ int channel = img.channels();
+
+ unsigned long int idx;
+
+ cv::Mat lap = gxx + gyy;
+
+ cv::Mat bound(img);
+ bound(cv::Rect(1, 1, w - 2, h - 2)) = 0;
+ double *dir_boundary = new double[h*w];
+
+ #pragma omp parallel for
+ for (int i = 1; i < h - 1; i++)
+ for (int j = 1; j < w - 1; j++)
+ {
+ idx = i*w + j;
+ if (i == 1 || i == h - 2 || j == 1 || j == w - 2)
+ dir_boundary[idx] = (int)bound.ATF(i, (j + 1)) + (int)bound.ATF(i, (j - 1)) + (int)bound.ATF(i - 1, j) + (int)bound.ATF(i + 1, j);
+ else dir_boundary[idx] = 0;
+
+ }
+
+
+ cv::Mat diff(h, w, CV_32FC1);
+ #pragma omp parallel for
+ for (int i = 0; i < h; i++)
+ {
+ for (int j = 0; j < w; j++)
+ {
+ idx = i*w + j;
+ diff.ATF(i, j) = (float)(-lap.ATF(i, j) + dir_boundary[idx]);
+ }
+ }
+
+ double *gtest = new double[(h - 2)*(w - 2)];
+ #pragma omp parallel for
+ for (int i = 0; i < h - 2; i++)
+ {
+ for (int j = 0; j < w - 2; j++)
+ {
+ idx = i*(w - 2) + j;
+ gtest[idx] = diff.ATF(i + 1, j + 1);
+
+ }
+ }
+ //Iteration begins
+ cv::Mat U = cv::Mat::zeros(img.size(), CV_32FC3);
+ int k = 0;
+ while (k <= 3000){
+ #pragma omp parallel for
+ for (int i = 1; i < h - 1; i++)
+ {
+ for (int j = 1; j < w - 1; j++)
+ {
+ U.ATF(i, j) = (float)((U.ATF(i + 1, j) + U.ATF(i, j + 1) + U.ATF(i - 1, j) + U.ATF(i, j - 1) + diff.ATF(i, j)) / 4.0);
+ }
+ }
+ k++;
+ }
+
+ img.convertTo(result, CV_8UC1);
+ #pragma omp parallel for
+ for (int i = 1; i < h - 1; i++)
+ {
+ for (int j = 1; j < w - 1; j++)
+ {
+ if (U.ATF(i,j) < 0.0)
+ result.ATB(i , j ) = 0;
+ else if (U.ATF(i, j) > 255.0)
+ result.ATB(i, j) = 255;
+ else
+ result.ATB(i , j ) = (int)U.ATF(i, j);
+ }
+ }
+}
+
+
+//IplImage versions
+
+void lapx( const IplImage *img, IplImage *gxx)
+{
+ int w = img->width;
+ int h = img->height;
+ int channel = img->nChannels;
+
+ cvZero( gxx );
+ for(int i=0;i<h;i++)
+ for(int j=0;j<w-1;j++)
+ for(int c=0;c<channel;++c)
+ {
+ CV_IMAGE_ELEM(gxx,float,i,(j+1)*channel+c) =
+ (float)CV_IMAGE_ELEM(img,float,i,(j+1)*channel+c) - (float)CV_IMAGE_ELEM(img,float,i,j*channel+c);
+ }
+}
+void lapy( const IplImage *img, IplImage *gyy)
+{
+ int w = img->width;
+ int h = img->height;
+ int channel = img->nChannels;
+
+ cvZero( gyy );
+ for(int i=0;i<h-1;i++)
+ for(int j=0;j<w;j++)
+ for(int c=0;c<channel;++c)
+ {
+ CV_IMAGE_ELEM(gyy,float,i+1,j*channel+c) =
+ (float)CV_IMAGE_ELEM(img,float,(i+1),j*channel+c) - (float)CV_IMAGE_ELEM(img,float,i,j*channel+c);
+
+ }
+}
+
+void poisson_solver(const IplImage *img, IplImage *gxx , IplImage *gyy, cv::Mat &result)
+{
+
+ int w = img->width;
+ int h = img->height;
+ int channel = img->nChannels;
+
+ unsigned long int idx,idx1;
+
+ IplImage *lap = cvCreateImage(cvGetSize(img), 32, 1);
+
+ for(int i =0;i<h;i++)
+ for(int j=0;j<w;j++)
+ CV_IMAGE_ELEM(lap,float,i,j)=CV_IMAGE_ELEM(gyy,float,i,j)+CV_IMAGE_ELEM(gxx,float,i,j);
+
+ //cv::Mat bound(img);
+ cv::Mat bound = cv::cvarrToMat(img);
+
+ for(int i =1;i<h-1;i++)
+ for(int j=1;j<w-1;j++)
+ {
+ bound.at<uchar>(i,j) = 0.0;
+ }
+
+ double *f_bp = new double[h*w];
+
+
+ for(int i =1;i<h-1;i++)
+ for(int j=1;j<w-1;j++)
+ {
+ idx=i*w + j;
+ f_bp[idx] = -4*(int)bound.at<uchar>(i,j) + (int)bound.at<uchar>(i,(j+1)) + (int)bound.at<uchar>(i,(j-1))
+ + (int)bound.at<uchar>(i-1,j) + (int)bound.at<uchar>(i+1,j);
+ }
+
+
+ cv::Mat diff = cv::Mat(h,w,CV_32FC1);
+ for(int i =0;i<h;i++)
+ {
+ for(int j=0;j<w;j++)
+ {
+ idx = i*w+j;
+ diff.at<float>(i,j) = (CV_IMAGE_ELEM(lap,float,i,j) - f_bp[idx]);
+ }
+ }
+ double *gtest = new double[(h-2)*(w-2)];
+ for(int i = 0 ; i < h-2;i++)
+ {
+ for(int j = 0 ; j < w-2; j++)
+ {
+ idx = i*(w-2) + j;
+ gtest[idx] = diff.at<float>(i+1,j+1);
+
+ }
+ }
+ ///////////////////////////////////////////////////// Find DST /////////////////////////////////////////////////////
+
+ double *gfinal = new double[(h-2)*(w-2)];
+ double *gfinal_t = new double[(h-2)*(w-2)];
+ double *denom = new double[(h-2)*(w-2)];
+ double *f3 = new double[(h-2)*(w-2)];
+ double *f3_t = new double[(h-2)*(w-2)];
+ double *img_d = new double[(h)*(w)];
+
+ dst(gtest,gfinal,h-2,w-2);
+
+ transpose(gfinal,gfinal_t,h-2,w-2);
+
+ dst(gfinal_t,gfinal,w-2,h-2);
+
+ transpose(gfinal,gfinal_t,w-2,h-2);
+
+ int cx=1;
+ int cy=1;
+
+ for(int i = 0 ; i < w-2;i++,cy++)
+ {
+ for(int j = 0,cx = 1; j < h-2; j++,cx++)
+ {
+ idx = j*(w-2) + i;
+ denom[idx] = (float) 2*cos(pi*cy/( (double) (w-1))) - 2 + 2*cos(pi*cx/((double) (h-1))) - 2;
+
+ }
+ }
+
+ for(idx = 0 ; idx < (w-2)*(h-2) ;idx++)
+ {
+ gfinal_t[idx] = gfinal_t[idx]/denom[idx];
+ }
+
+
+ idst(gfinal_t,f3,h-2,w-2);
+
+ transpose(f3,f3_t,h-2,w-2);
+
+ idst(f3_t,f3,w-2,h-2);
+
+ transpose(f3,f3_t,w-2,h-2);
+
+ for(int i = 0 ; i < h;i++)
+ {
+ for(int j = 0 ; j < w; j++)
+ {
+ idx = i*w + j;
+ img_d[idx] = (double)CV_IMAGE_ELEM(img,uchar,i,j);
+ }
+ }
+ for(int i = 1 ; i < h-1;i++)
+ {
+ for(int j = 1 ; j < w-1; j++)
+ {
+ idx = i*w + j;
+ img_d[idx] = 0.0;
+ }
+ }
+ int id1,id2;
+ for(int i = 1,id1=0 ; i < h-1;i++,id1++)
+ {
+ for(int j = 1,id2=0 ; j < w-1; j++,id2++)
+ {
+ idx = i*w + j;
+ idx1= id1*(w-2) + id2;
+ img_d[idx] = f3_t[idx1];
+ }
+ }
+
+ for(int i = 0 ; i < h;i++)
+ {
+ for(int j = 0 ; j < w; j++)
+ {
+ idx = i*w + j;
+ if(img_d[idx] < 0.0)
+ result.at<uchar>(i,j) = 0;
+ else if(img_d[idx] > 255.0)
+ result.at<uchar>(i,j) = 255.0;
+ else
+ result.at<uchar>(i,j) = img_d[idx];
+ }
+ }
+
+}
diff --git a/RWR/src/mean-shift/dvs_meanshift/src/meanshift.cpp b/RWR/src/mean-shift/dvs_meanshift/src/meanshift.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..faca3149e05e8e6941e8be34192e121b0dc4376d
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/src/meanshift.cpp
@@ -0,0 +1,1606 @@
+
+#include "dvs_meanshift/meanshift.h"
+#include "dvs_meanshift/color_meanshift.h"
+#include "image_reconstruct/image_reconst.h"
+#include <time.h>
+
+//remove this include, just for debugging
+#include "graph3d/pnmfile.h"
+#include <std_msgs/Float32.h>
+
+#define min(x,y) ((x)<(y)?(x):(y))
+
+double epsilon = 1E-10;
+int counterGlobal = 0;
+const int MAX_DISTANCE = 500; //actually the real distance is sqrt(MAX_DISTANCE)
+
+bool firstevent = true;
+double firsttimestamp;
+
+//std::ofstream foutX("/home/fran/0_tmp/kk/shapes_rotation_totalevents.txt");
+//std::ofstream fouttotalproc("/home/fran/0_tmp/kk/shapes_rotation_totalprocessing_f.txt");
+//std::ofstream foutX("/home/fran/0_tmp/kk/shapes_rotation_2_totalevents.txt");
+//std::ofstream fouttotalproc("/home/fran/0_tmp/kk/shapes_rotation_2_totalprocessing_f.txt");
+//std::ofstream foutX("/home/fran/0_tmp/kk/shapes_translation_totalevents.txt");
+//std::ofstream fouttotalproc("/home/fran/0_tmp/kk/shapes_translation_totalprocessing_f.txt");
+//std::ofstream foutX("/home/fran/0_tmp/kk/shapes_translation_2_totalevents.txt");
+//std::ofstream fouttotalproc("/home/fran/0_tmp/kk/shapes_translation_2_totalprocessing_f.txt");
+//std::ofstream foutX("/home/fran/0_tmp/kk/shapes_6dof_totalevents.txt");
+//std::ofstream fouttotalproc("/home/fran/0_tmp/kk/shapes_6dof_totalprocessing_f.txt");
+//std::ofstream foutX("/home/fran/0_tmp/kk/shapes_6dof_2_totalevents.txt");
+//std::ofstream fouttotalproc("/home/fran/0_tmp/kk/shapes_6dof_2_totalprocessing_f.txt");
+//std::ofstream foutnumclust("/home/fran/0_tmp/kk/shapes_translation_numclusters_f.txt");
+//std::ofstream foutnumclust("/home/fran/0_tmp/kk/shapes_rotation_numclusters_f.txt");
+//std::ofstream foutnumclust("/home/fran/0_tmp/kk/shapes_6dof_numclusters_f.txt");
+//std::ofstream foutnumclust("/home/fran/0_tmp/kk/shapes_6dof_2_numclusters_f.txt");
+//std::ofstream foutnumclust("/home/fran/0_tmp/kk/shapes_rotation_2_numclusters_f.txt");
+//std::ofstream foutnumclust("/home/fran/0_tmp/kk/shapes_translation_2_numclusters_f.txt");
+
+#if KALMAN_FILTERING
+//std::ofstream foutX("/home/fran/0_tmp/kk/traj.txt");
+//std::ofstream foutX_estim("/home/fran/0_tmp/kk/filt_traj.txt");
+//std::ofstream foutT("/home/fran/0_tmp/kk/timetraj.txt");
+#endif
+
+namespace dvs_meanshift {
+
+void writeMatToFile(cv::Mat & m, const char* filename)
+{
+ std::ofstream fout(filename);
+ std::cout<<" IM ALIVE?? 1"<<std::endl;
+
+ if(!fout)
+ {
+ std::cout<<"File Not Opened"<<std::endl; return;
+ }
+
+ for(int i=0; i<m.rows; i++)
+ {
+ for(int j=0; j<m.cols; j++)
+ {
+ fout<<m.at<double>(i,j)<<"\t";
+ }
+ fout<<std::endl;
+ }
+ fout.close();
+}
+
+class Parallel_pixel_cosine: public cv::ParallelLoopBody
+{
+
+public:
+
+ Parallel_pixel_cosine(cv::Mat imgg) : img(imgg)
+ {
+ }
+
+ void operator() (const cv::Range &r) const
+ {
+ for(int j=r.start; j<r.end; ++j)
+ {
+ double* current = const_cast<double*>(img.ptr<double>(j));
+ double* last = current + img.cols;
+
+ for (; current != last; ++current)
+ *current = (double) cos((double) *current);
+ }
+ }
+
+private:
+ cv::Mat img;
+};
+
+void meshgrid(const cv::Mat &xgv, const cv::Mat &ygv, cv::Mat1i &X, cv::Mat1i &Y)
+{
+ std::cout<<" IM ALIVE?? 2"<<std::endl;
+ cv::repeat(xgv.reshape(1,1), ygv.total(), 1, X);
+ cv::repeat(ygv.reshape(1,1).t(), 1, xgv.total(), Y);
+
+}
+
+// helper function (maybe that goes somehow easier)
+ void meshgridTest(const cv::Range &xgv, const cv::Range &ygv, cv::Mat1i &X, cv::Mat1i &Y)
+{
+ std::cout<<" IM ALIVE?? 3"<<std::endl;
+ std::vector<int> t_x, t_y;
+ for (int i = xgv.start; i <= xgv.end; i++) t_x.push_back(i);
+ for (int i = ygv.start; i <= ygv.end; i++) t_y.push_back(i);
+
+ meshgrid(cv::Mat(t_x), cv::Mat(t_y), X, Y);
+}
+
+void findClosestCluster(std::vector<int> * clustID, std::vector<double> clustX, std::vector<double> clustY,std::vector<double>clustX_old,std::vector<double> clustY_old)
+{
+
+ //for (int i = 0; i<clustX.size(); i++)
+ // ROS_ERROR_STREAM("ClusterCenter("<<i+1<<",:)=["<<clustX.at(i)<<","<<clustY.at(i)<<"];");
+ //for (int i = 0; i<clustX_old.size(); i++)
+ // ROS_ERROR_STREAM("prev_ClusterCenter("<<i+1<<",:)=["<<clustX_old.at(i)<<","<<clustY_old.at(i)<<"];");
+ std::cout<<" IM ALIVE?? 4"<<std::endl;
+ std::vector<double> distvalue (clustX.size());
+ double tmp;
+
+ for(int i=0; i<clustX.size(); i++)
+ {
+ distvalue[i]=MAX_DISTANCE;
+ (*clustID).push_back(-1); //no match when clustID is -1
+ for(int j=0; j<clustX_old.size(); j++)
+ {
+ tmp= (clustX[i]*DVSW-clustX_old[j]*DVSW)*(clustX[i]*DVSW-clustX_old[j]*DVSW) + (clustY[i]*DVSH-clustY_old[j]*DVSH)*(clustY[i]*DVSH-clustY_old[j]*DVSH);
+ if(tmp<distvalue[i])
+ {
+ distvalue[i]=tmp;
+ (*clustID)[i]=j;
+ //ROS_ERROR_STREAM("Cluster "<<i+1<<" same than old cluster "<<j+1<<". Distance = "<<distvalue[i]);
+ }
+ }
+ }
+}
+
+/*
+void Meanshift::assignClusterColor(std::vector<int> * positionClusterColor, int numClusters, std::vector<int> matches, std::vector<int> oldPositions)
+{
+
+ if (matches.size()==0) //no mask --> assing all new colors
+ {
+ for(int i=0; i<numClusters; i++)
+ {
+ (*positionClusterColor).push_back(lastPositionClusterColor);
+ lastPositionClusterColor = lastPositionClusterColor+1;
+ }
+ }
+ else
+ {
+ for(int i=0; i<numClusters; i++)
+ {
+ if(matches[i]==-1) //No match with previous clusters --> new cluster, new color
+ {
+ (*positionClusterColor).push_back(lastPositionClusterColor);
+ lastPositionClusterColor = lastPositionClusterColor+1;
+ }
+ else //Match with previous cluster --> assign color of previous cluster
+ {
+ (*positionClusterColor).push_back(oldPositions[matches[i]]);
+ }
+ }
+
+ }
+}*/
+
+void Meanshift::assignClusterColor(std::vector<int> * positionClusterColor, int numClusters, std::vector<int> matches, std::vector<int> oldPositions, std::vector<int> activeTrajectories)
+{
+ std::cout<<" IM ALIVE?? 5"<<std::endl;
+ if (matches.size()==0) //no mask --> assing all new colors
+ {
+ for(int i=0; i<numClusters; i++)
+ {
+ (*positionClusterColor).push_back(lastPositionClusterColor);
+ lastPositionClusterColor = lastPositionClusterColor+1;
+ }
+ }
+ else
+ {
+ for(int i=0; i<numClusters; i++)
+ {
+ if(matches[i]==-1) //No match with previous clusters --> new cluster, new color
+ {
+ //Check if the new position is being used
+ while ( activeTrajectories.end() != find (activeTrajectories.begin(), activeTrajectories.end(), (lastPositionClusterColor & (MAX_NUM_CLUSTERS-1))))
+ lastPositionClusterColor++;
+
+ (*positionClusterColor).push_back(lastPositionClusterColor);
+ lastPositionClusterColor = lastPositionClusterColor+1;
+ }
+ else //Match with previous cluster --> assign color of previous cluster
+ {
+ (*positionClusterColor).push_back(oldPositions[matches[i]]);
+ }
+ }
+
+ }
+}
+
+Meanshift::Meanshift(ros::NodeHandle & nh, ros::NodeHandle nh_private) : nh_(nh)
+{
+ //std::cout<<" IM ALIVE?? 6"<<std::endl;
+ used_last_image_ = false;
+
+ // get parameters of display method
+ std::string display_method_str;
+ nh_private.param<std::string>("display_method", display_method_str, "");
+ display_method_ = (display_method_str == std::string("grayscale")) ? GRAYSCALE : RED_BLUE;
+
+ // setup subscribers and publishers
+ //event_sub_ = nh_.subscribe("events", 1, &Meanshift::eventsCallback, this);
+ event_sub_ = nh_.subscribe("events", 1, &Meanshift::eventsCallback_simple, this);
+ camera_info_sub_ = nh_.subscribe("camera_info", 1, &Meanshift::cameraInfoCallback, this);
+
+ image_transport::ImageTransport it_(nh_);
+
+ image_pub_ = it_.advertise("dvs_rendering", 1);
+ std::cout<<" IM ALIVE?? 6"<<std::endl;
+ image_segmentation_pub_ = it_.advertise("dvs_segmentation", 1);
+ std::cout<<" shit cunt"<<std::endl;
+
+
+ //DEBUGGING
+ image_debug1_pub_ = it_.advertise("dvs_debug1", 1);
+ image_debug2_pub_ = it_.advertise("dvs_debug2", 1);
+ //NOT DEBUGGING
+
+ //Initializing params for color meanshift
+ //computing image calibration
+ numRows=DVSH; numCols=DVSW;
+ mPixels=numRows*numCols; maxPixelDim=3;
+
+ spaceDivider=7; timeDivider = 5;
+ maxIterNum=1;
+ tolFun=0.0001;
+ kernelFun[0] = 'g'; kernelFun[1] = 'a'; kernelFun[2] = 'u'; kernelFun[3] = 's';
+ kernelFun[4] = 's'; kernelFun[5] = 'i'; kernelFun[6] = 'a'; kernelFun[7] = 'n';
+
+ //Initialize pointers
+ outputFeatures = NULL; imagePtr = NULL; pixelsPtr = NULL;
+ initializationPtr=new double[maxPixelDim*mPixels/4];
+
+ /*for (int i = 0; i < numRows; i++)
+ for (int j = 0; j < numCols; j++)
+ {
+ initializationPtr[i*numCols + j]=j/spaceDivider;//x
+ initializationPtr[mPixels+i*numCols+j]=i/spaceDivider;//y
+ initializationPtr[2*mPixels+i*numCols+j]=0;//img
+
+ }*/
+
+ //Write xposition, yposition, and image in the same vector
+ for (int i = 0; i < numRows/2; i++)
+ for (int j = 0; j < numCols/2; j++)
+ {
+ initializationPtr[i*numCols/2 + j]=j/spaceDivider;//x
+ initializationPtr[mPixels/4+i*numCols/2+j]=i/spaceDivider;//y
+ initializationPtr[2*mPixels/4+i*numCols/2+j]=0;//img
+
+ }
+
+ //Initialize params for graph3d segmentation
+ sigma = 0.75;
+ k = 500;
+ min_region = (int)(numRows*numCols*0.01);
+ num_components = 0;
+
+ //Initialization of random seed
+ srand(time(NULL));//Random seed initialization
+ //asm("rdtsc\n"
+ // "mov edi, eax\n"
+ // "call srand");
+
+ //Create first the RGB values and then assign it to the clusters
+ //(hopefully, this will keep some stability in the cluster colors along consecutive frames)
+ //Assume we won't have more than 50 clusters
+ for(int i=0; i<MAX_NUM_CLUSTERS; i++)
+ {
+ RGBColors.push_back(cv::Vec3b((uchar)random(), (uchar)random(), (uchar)random()));
+ //counterTrajectories.push_back(0);//initialize counter trajectories to all zeros
+ }
+ counterTrajectories=std::vector<int>(MAX_NUM_CLUSTERS,0);
+
+ lastPositionClusterColor=0;
+
+ //Initializing trajectories
+ trajectories = cv::Mat(numRows, numCols, CV_8UC3);
+ trajectories = cv::Scalar(128,128,128);
+
+ //Initialize trajectory matrix
+ allTrajectories = new std::vector<cv::Point>[MAX_NUM_CLUSTERS];
+ for(int i = 0; i < MAX_NUM_CLUSTERS; i++)
+ {
+ allTrajectories[i] = std::vector<cv::Point>(MAX_NUM_TRAJECTORY_POINTS);
+ }
+ prev_activeTrajectories=std::vector<int>(MAX_NUM_CLUSTERS,0);
+
+ //Init Kalman filter
+ //createKalmanFilter();
+ vector_of_kf=std::vector<cv::KalmanFilter>(MAX_NUM_CLUSTERS);
+ vector_of_meas=std::vector<cv::Mat>(MAX_NUM_CLUSTERS);
+ vector_of_state=std::vector<cv::Mat>(MAX_NUM_CLUSTERS);
+ for(int i=0; i<vector_of_kf.size(); i++)
+ createKalmanFilter(&(vector_of_kf[i]), &(vector_of_state[i]), &(vector_of_meas[i]));
+
+ selectedTrajectory = -1;
+ //foundBlobs = false;
+ vector_of_foundBlobs=std::vector<bool>(MAX_NUM_CLUSTERS,false);
+ notFoundBlobsCount = 0;
+ //foundTrajectory=false;
+ foundTrajectory = std::vector<bool>(MAX_NUM_CLUSTERS, false);
+ //ticks = 0;
+ vector_of_ticks=std::vector<double> (MAX_NUM_CLUSTERS,0.0);
+
+ //BG activity filtering
+ BGAFframe = cv::Mat(numRows, numCols, CV_32FC1);
+ BGAFframe =cv::Scalar(0.);
+}
+
+Meanshift::~Meanshift()
+{
+ delete [] initializationPtr;
+
+#if KALMAN_FILTERING
+ //foutX.close();
+ //foutX_estim.close();
+ //foutT.close();
+#endif
+ image_pub_.shutdown();
+ image_segmentation_pub_.shutdown();
+ image_debug1_pub_.shutdown();
+}
+
+void Meanshift::cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg)
+{
+ std::cout<<" IM ALIVE?? 7"<<std::endl;
+ camera_matrix_ = cv::Mat(3, 3, CV_64F);
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ camera_matrix_.at<double>(cv::Point(i, j)) = msg->K[i+j*3];
+
+ dist_coeffs_ = cv::Mat(msg->D.size(), 1, CV_64F);
+ for (int i = 0; i < msg->D.size(); i++)
+ dist_coeffs_.at<double>(i) = msg->D[i];
+}
+
+
+void Meanshift::createKalmanFilter(cv::KalmanFilter *kf, cv::Mat *state, cv::Mat *meas)
+{
+ std::cout<<" IM ALIVE?? 8"<<std::endl;
+ /*
+ * This implementation takes 6 states position, velocity, and dimensions of the bounding box
+ int stateSize = 6;
+ int measSize = 4;
+ int contrSize = 0;
+
+ unsigned int type = CV_32F;
+
+ //cv::KalmanFilter kf(stateSize, measSize, contrSize, type);
+ kf.init(stateSize, measSize, contrSize, type);
+
+ cv::Mat state(stateSize, 1, type); // [x,y,v_x,v_y,w,h]
+ cv::Mat meas(measSize, 1, type); // [z_x,z_y,z_w,z_h]
+ // [E_x,E_y,E_v_x,E_v_y,E_w,E_h]
+
+ // Transition State Matrix A
+ // Note: set dT at each processing step!
+ // [ 1 0 dT 0 0 0 ]
+ // [ 0 1 0 dT 0 0 ]
+ // [ 0 0 1 0 0 0 ]
+ // [ 0 0 0 1 0 0 ]
+ // [ 0 0 0 0 1 0 ]
+ // [ 0 0 0 0 0 1 ]
+ cv::setIdentity(kf.transitionMatrix);
+
+ // Measure Matrix H
+ // [ 1 0 0 0 0 0 ]
+ // [ 0 1 0 0 0 0 ]
+ // [ 0 0 0 0 1 0 ]
+ // [ 0 0 0 0 0 1 ]
+ kf.measurementMatrix = cv::Mat::zeros(measSize, stateSize, type);
+ kf.measurementMatrix.at<float>(0) = 1.0f;
+ kf.measurementMatrix.at<float>(7) = 1.0f;
+ kf.measurementMatrix.at<float>(16) = 1.0f;
+ kf.measurementMatrix.at<float>(23) = 1.0f;
+
+ // Process Noise Covariance Matrix Q
+ // [ Ex 0 0 0 0 0 ]
+ // [ 0 Ey 0 0 0 0 ]
+ // [ 0 0 Ev_x 0 0 0 ]
+ // [ 0 0 0 Ev_y 0 0 ]
+ // [ 0 0 0 0 Ew 0 ]
+ // [ 0 0 0 0 0 Eh ]
+ //cv::setIdentity(kf.processNoiseCov, cv::Scalar(1e-2));
+ kf.processNoiseCov.at<float>(0) = 1e-2;
+ kf.processNoiseCov.at<float>(7) = 1e-2;
+ kf.processNoiseCov.at<float>(14) = 5.0f;
+ kf.processNoiseCov.at<float>(21) = 5.0f;
+ kf.processNoiseCov.at<float>(28) = 1e-2;
+ kf.processNoiseCov.at<float>(35) = 1e-2;
+
+ // Measures Noise Covariance Matrix R
+ cv::setIdentity(kf.measurementNoiseCov, cv::Scalar(1e-1));
+ */
+
+
+ /*
+ * This implementation is only for one kalman filter
+ */
+ int stateSize = 4;
+ int measSize = 2;
+ int contrSize = 0;
+
+ unsigned int type = CV_32F;
+
+ kf->init(stateSize, measSize, contrSize, type);
+
+ //cv::Mat state(stateSize, 1, type); // [x,y,v_x,v_y]
+ //cv::Mat meas(measSize, 1, type); // [z_x,z_y]
+ state->create(stateSize,1,type);
+ meas->create(measSize,1,type);
+ // [E_x,E_y,E_v_x,E_v_y]
+
+
+ // Transition State Matrix A
+ // Note: set dT at each processing step!
+ // [ 1 0 dT 0 ]
+ // [ 0 1 0 dT]
+ // [ 0 0 1 0 ]
+ // [ 0 0 0 1 ]
+ cv::setIdentity(kf->transitionMatrix);
+
+ // Measure Matrix H
+ // [ 1 0 0 0 ]
+ // [ 0 1 0 0 ]
+ kf->measurementMatrix = cv::Mat::zeros(measSize, stateSize, type);
+ kf->measurementMatrix.at<float>(0) = 1.0f;
+ kf->measurementMatrix.at<float>(5) = 1.0f;
+
+ // Process Noise Covariance Matrix Q
+ // [ Ex 0 0 0 ]
+ // [ 0 Ey 0 0 ]
+ // [ 0 0 Ev_x 0 ]
+ // [ 0 0 0 Ev_y ]
+ //cv::setIdentity(kf.processNoiseCov, cv::Scalar(1e-2));
+ kf->processNoiseCov.at<float>(0) = 1e-2;//original values
+ kf->processNoiseCov.at<float>(5) = 1e-2;
+ kf->processNoiseCov.at<float>(10) = 5.0f;
+ kf->processNoiseCov.at<float>(15) = 5.0f;
+
+ kf->processNoiseCov.at<float>(0) = 1e-2;
+ kf->processNoiseCov.at<float>(5) = 1e-2;
+ kf->processNoiseCov.at<float>(10) = 7.0f;
+ kf->processNoiseCov.at<float>(15) = 7.0f;
+
+ // Measures Noise Covariance Matrix R
+ cv::setIdentity(kf->measurementNoiseCov, cv::Scalar(1e-1));
+}
+
+
+
+void Meanshift::eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg)
+{
+ std::cout<<" IM ALIVE?? 9"<<std::endl;
+ // only create image if at least one subscriber
+ if (image_pub_.getNumSubscribers() > 0)
+ {
+ cv_bridge::CvImage cv_image;
+
+ if (display_method_ == RED_BLUE)
+ {
+ cv_image.encoding = "bgr8";
+
+ if (last_image_.rows == msg->height && last_image_.cols == msg->width)
+ {
+ last_image_.copyTo(cv_image.image);
+ used_last_image_ = true;
+ }
+ else
+ {
+ cv_image.image = cv::Mat(msg->height, msg->width, CV_8UC3);
+ cv_image.image = cv::Scalar(128, 128, 128);
+ }
+
+ for (int i = 0; i < msg->events.size(); ++i)
+ {
+ const int x = msg->events[i].x;
+ const int y = msg->events[i].y;
+
+ cv_image.image.at<cv::Vec3b>(cv::Point(x, y)) = (
+ msg->events[i].polarity == true ? cv::Vec3b(255, 0, 0) : cv::Vec3b(0, 0, 255));
+ }
+ }
+ else
+ {
+ cv_image.encoding = "mono8";
+ cv_image.image = cv::Mat(msg->height, msg->width, CV_8U);
+ cv_image.image = cv::Scalar(128);
+
+ cv::Mat on_events = cv::Mat(msg->height, msg->width, CV_8U);
+ on_events = cv::Scalar(0);
+
+ cv::Mat off_events = cv::Mat(msg->height, msg->width, CV_8U);
+ off_events = cv::Scalar(0);
+
+ // count events per pixels with polarity
+ for (int i = 0; i < msg->events.size(); ++i)
+ {
+ const int x = msg->events[i].x;
+ const int y = msg->events[i].y;
+
+ if (msg->events[i].polarity == 1)
+ on_events.at<uint8_t>(cv::Point(x, y))++;
+ else
+ off_events.at<uint8_t>(cv::Point(x, y))++;
+ }
+
+ // scale image
+ cv::normalize(on_events, on_events, 0, 128, cv::NORM_MINMAX, CV_8UC1);
+ cv::normalize(off_events, off_events, 0, 127, cv::NORM_MINMAX, CV_8UC1);
+
+ cv_image.image += on_events;
+ cv_image.image -= off_events;
+ }
+ image_pub_.publish(cv_image.toImageMsg());
+ }
+
+ if (image_segmentation_pub_.getNumSubscribers() > 0)
+ {
+ cv_bridge::CvImage cv_segments;
+ //cv_bridge::CvImage cv_debug1;
+ //cv_bridge::CvImage cv_debug2;
+
+
+ //Doing color meanshift
+ cv::Mat timestamp_matrix = cv::Mat(numRows, numCols, CV_64F);
+ timestamp_matrix = cv::Scalar(0.);
+ cv::Mat timestamp_matrix_out;
+
+ //Reading time of first event and saving it
+ //ROS_ERROR("Reading values from timestamp_matrix_out: %d", (int) msg->events.size());
+ uint64_t first_timestamp = msg->events[0].ts.toNSec();
+ // count events per pixels with polarity
+ for (int i = 0; i < msg->events.size(); ++i)
+ {
+ const int x = msg->events[i].x;
+ const int y = msg->events[i].y;
+
+ double event_timestamp = (0.001*(double)(msg->events[i].ts.toNSec()-first_timestamp));//now in usecs
+
+ if (msg->events[i].polarity == 1)
+ timestamp_matrix.at<double>(cv::Point(x, y))= event_timestamp*0.001;
+ else
+ timestamp_matrix.at<double>(cv::Point(x, y))=-event_timestamp*0.001;
+ }
+
+ //First create the interface and
+ //cv_debug1.image =result_image;
+ //cv::Mat img_hist_equalized;
+ //cv::equalizeHist(result_image, img_hist_equalized); //equalize the histogram
+ //cv_debug1.image = img_hist_equalized;
+
+
+
+ //POISSON SOLVER IMPLEMENTATION
+ /*----------------------------------------------------------------------------* /
+ cv::Mat debugIm1= cv::Mat(numRows, numCols, CV_8UC1);
+
+ double min, max;
+ cv::minMaxLoc(timestamp_matrix, &min, &max);
+
+ cv::normalize(timestamp_matrix, debugIm1, 0, 255, cv::NORM_MINMAX, CV_8UC1);
+
+ // Here, we are assuming that our image is a gradient already (gx and gy)
+ cv::Mat gxx = cv::Mat(numRows, numCols, CV_64F);
+ cv::Mat gyy = cv::Mat(numRows, numCols, CV_64F);
+
+ cv::Mat tmp;
+ debugIm1.convertTo(tmp, CV_64F);
+ tmp = (tmp/255)*(max-min)+min;
+
+
+ lapx(tmp, gxx);
+ lapy(tmp, gyy);
+
+ cv::Mat lapfnc = cv::Mat(numRows, numCols, CV_64F);
+ cv::Mat fcos = cv::Mat(numRows, numCols, CV_64F);
+ cv::Mat denom = cv::Mat(numRows, numCols, CV_64F);
+ cv::Mat output = cv::Mat(numRows, numCols, CV_64F);
+ lapfnc = gxx+gyy;
+ cv::dct(lapfnc, fcos);
+
+ cv::Mat1i XX, YY;
+ cv::Mat XX_bis, YY_bis;
+ meshgridTest(cv::Range(0,numCols-1), cv::Range(0, numRows-1), XX, YY);
+
+ XX.convertTo(XX_bis, CV_64F); YY.convertTo(YY_bis, CV_64F);
+ XX_bis = CV_PI*XX_bis/numCols; YY_bis=CV_PI*YY_bis/numRows;
+
+ parallel_for_(cv::Range(0,XX_bis.rows), Parallel_pixel_cosine(XX_bis));
+ parallel_for_(cv::Range(0,YY_bis.rows), Parallel_pixel_cosine(YY_bis));
+
+ denom = 2*XX_bis-2 + 2*YY_bis-2;
+
+ double first_elem =fcos.at<double>(cv::Point(0, 0));
+ cv::divide(fcos, denom, fcos);
+ fcos.at<double>(cv::Point(0, 0))=first_elem; // remove the Inf
+
+ cv::idct(fcos, output);
+ double min_bis, max_bis;
+ cv::minMaxLoc(output, &min_bis, &max_bis);
+ output = output - min_bis;
+ cv::Mat result_image, output_norm;
+ cv::normalize(output, output_norm, 0, 255, cv::NORM_MINMAX, CV_8UC1);
+
+ cv::medianBlur(output_norm,result_image,3);
+ /*----------------------------------------------------------------------------*/
+ //Alternatively, one can use only the time
+ cv::Mat result_image;
+ cv::normalize(timestamp_matrix, result_image, 0, 255, cv::NORM_MINMAX, CV_8UC1);
+ /*----------------------------------------------------------------------------*/
+
+ //cv::Mat kk1;
+ //result_image.convertTo(kk1, CV_64F);
+ //writeMatToFile(kk1,"/home/fran/kk1.txt");
+
+ //cv_debug1.encoding = "mono8";
+ //cv_debug1.image = result_image;
+ //image_debug1_pub_.publish(cv_debug1.toImageMsg());
+
+ cv::Mat temporalIm, temporalImSubsampled;
+ result_image.convertTo(temporalIm, CV_64F);
+ temporalIm = temporalIm/timeDivider;
+ cv::pyrDown(temporalIm, temporalImSubsampled, cv::Size(numCols/2, numRows/2));
+ //Initialize positions (only once)
+ outputFeatures=new double[maxPixelDim*mPixels/4];
+ pixelsPtr=new double[maxPixelDim*mPixels/4];
+ imagePtr=new double[mPixels/4];
+
+ memcpy(pixelsPtr, initializationPtr, maxPixelDim*mPixels/4*sizeof(double)); //copy event the 0s image
+ memcpy(pixelsPtr+2*mPixels/4, (void*)(temporalImSubsampled.data), mPixels/4*sizeof(double)); //copy event the 0s image
+ memcpy(imagePtr, (void*)(temporalImSubsampled.data), mPixels/4*sizeof(double)); //copy event the 0s image
+
+ //extract the features
+ colorMeanShiftFilt(outputFeatures, pixelsPtr, mPixels/4, maxPixelDim, imagePtr, \
+ numRows/2, numCols/2, spaceDivider, kernelFun, maxIterNum, tolFun);
+
+ cv::Mat meanshiftIm;
+ cv::Mat meanshiftIm_small = cv::Mat(numRows/2, numCols/2, CV_64F, outputFeatures+2*mPixels/4);
+ meanshiftIm_small = meanshiftIm_small*timeDivider;
+ cv::pyrUp(meanshiftIm_small, meanshiftIm, cv::Size(numCols, numRows));
+
+ //cv::Mat kk = cv::Mat(numRows, numCols, CV_64F, imagePtr);
+ //kk = kk*timeDivider;
+ //writeMatToFile(kk,"/home/fran/imagePtr.txt");
+
+ //do the segmentation
+// cv::Mat filtered_im = cv::Mat(numRows, numCols, CV_64F, outputFeatures+2*mPixels);
+// filtered_im = filtered_im*timeDivider;
+ cv::Mat meanshiftImNormalized;
+ //meanshift_im.convertTo(meanshift_im_normalized,CV_8UC1);
+ cv::normalize(meanshiftIm, meanshiftImNormalized, 0, 255, cv::NORM_MINMAX, CV_8UC1);
+
+ //cv_debug2.encoding = "mono8";
+ //cv_debug2.image = meanshift_im_normalized;
+ //image_debug2_pub_.publish(cv_debug2.toImageMsg());
+
+ //meanshift_im_normalized= cv::imread("/home/fran/mug.pgm", CV_LOAD_IMAGE_GRAYSCALE);
+
+ //tmp has the values that are non-zero
+
+ cv_segments.encoding = "bgr8";
+ cv_segments.image =cv::Mat(numRows, numCols, CV_8UC3);
+ //cv_segments.image =cv::Mat(128, 128, CV_8UC3);
+ im2segment(meanshiftImNormalized, cv_segments.image, sigma, k, min_region, &num_components);
+ image_segmentation_pub_.publish(cv_segments.toImageMsg());
+
+ //cv::Mat mask;
+ //mask = (abs(timestamp_matrix) > 1E-6); // Puts 255 wherever it is true
+ //mask.convertTo(mask, CV_64F);
+ //writeMatToFile(mask,"/home/fran/mask.txt");
+
+ //cv::Mat finalOutput;
+ //cv::bitwise_and(cv_segments.image, cv::Scalar(255,255,255), finalOutput, mask);
+ //cv_segments.image = finalOutput;
+ //image_debug2_pub_.publish(cv_segments.toImageMsg());
+ //-------------------------------------------------------------------------------------
+
+ delete[] imagePtr;
+ delete [] outputFeatures;
+ delete [] pixelsPtr;
+ }
+}
+
+/*void Meanshift::eventsCallback_simple(const dvs_msgs::EventArray::ConstPtr& msg)
+{
+ // only create image if at least one subscriber
+ if (image_pub_.getNumSubscribers() > 0)
+ {
+ cv_bridge::CvImage cv_image;
+
+ if (display_method_ == RED_BLUE)
+ {
+ cv_image.encoding = "bgr8";
+
+ if (last_image_.rows == msg->height && last_image_.cols == msg->width)
+ {
+ last_image_.copyTo(cv_image.image);
+ used_last_image_ = true;
+ }
+ else
+ {
+ cv_image.image = cv::Mat(msg->height, msg->width, CV_8UC3);
+ cv_image.image = cv::Scalar(128, 128, 128);
+ }
+
+ for (int i = 0; i < msg->events.size(); ++i)
+ {
+ const int x = msg->events[i].x;
+ const int y = msg->events[i].y;
+
+ cv_image.image.at<cv::Vec3b>(cv::Point(x, y)) = (
+ msg->events[i].polarity == true ? cv::Vec3b(255, 0, 0) : cv::Vec3b(0, 0, 255));
+ }
+ }
+ else
+ {
+ cv_image.encoding = "mono8";
+ cv_image.image = cv::Mat(msg->height, msg->width, CV_8U);
+ cv_image.image = cv::Scalar(128);
+
+ cv::Mat on_events = cv::Mat(msg->height, msg->width, CV_8U);
+ on_events = cv::Scalar(0);
+
+ cv::Mat off_events = cv::Mat(msg->height, msg->width, CV_8U);
+ off_events = cv::Scalar(0);
+
+ // count events per pixels with polarity
+ for (int i = 0; i < msg->events.size(); ++i)
+ {
+ const int x = msg->events[i].x;
+ const int y = msg->events[i].y;
+
+ if (msg->events[i].polarity == 1)
+ on_events.at<uint8_t>(cv::Point(x, y))++;
+ else
+ off_events.at<uint8_t>(cv::Point(x, y))++;
+ }
+
+ // scale image
+ cv::normalize(on_events, on_events, 0, 128, cv::NORM_MINMAX, CV_8UC1);
+ cv::normalize(off_events, off_events, 0, 127, cv::NORM_MINMAX, CV_8UC1);
+
+ cv_image.image += on_events;
+ cv_image.image -= off_events;
+ }
+ image_pub_.publish(cv_image.toImageMsg());
+ }
+
+ if (image_segmentation_pub_.getNumSubscribers() > 0)
+ {
+ cv_bridge::CvImage cv_segments;
+
+ //Doing color meanshift
+ cv::Mat diffTimestampMatrix = cv::Mat(numRows, numCols, CV_64F, cv::Scalar::all(0.));
+ cv::Mat timestampMatrix = cv::Mat(numRows, numCols, CV_64F, cv::Scalar::all(0.));
+
+ uint64_t first_timestamp = msg->events[0].ts.toNSec();
+ double final_timestamp = (1E-6*(double)(msg->events[(msg->events.size())-1].ts.toNSec()-first_timestamp));
+
+ // count events per pixels with polarity
+ cv::Mat data=cv::Mat(3, msg->events.size(), CV_64F, cv::Scalar::all(0.));
+ int counter = 0;
+
+ //std::ofstream foutX("/home/fran/xpos.txt");
+ //std::ofstream foutY("/home/fran/ypos.txt");
+ //std::ofstream foutTime("/home/fran/timestamp.txt");
+
+ for (int i = 0; i < msg->events.size(); i++)
+ {
+ const int x = msg->events[i].x;
+ const int y = msg->events[i].y;
+
+ double event_timestamp = (1E-6*(double)(msg->events[i].ts.toNSec()-first_timestamp));//now in usecs
+
+ //if (timestampMatrix.at<double>(cv::Point(x,y))> epsilon)
+ // diffTimestampMatrix.at<double>(cv::Point(x, y)) = event_timestamp -timestampMatrix.at<double>(cv::Point(x, y));
+ //timestampMatrix.at<double>(cv::Point(x, y))= event_timestamp;
+
+ if (event_timestamp < 15)
+ {
+ data.at<double>(cv::Point(i, 0))= (double)x/numCols;
+ data.at<double>(cv::Point(i, 1))= (double)y/numRows;
+ data.at<double>(cv::Point(i, 2))= event_timestamp/final_timestamp;//normalized
+ counter++;
+ }
+ else
+ {
+ //ROS_ERROR_STREAM("I'm using "<<counter<<" out of "<<msg->events.size());
+ break;
+ }
+ //foutX<<x<<"\t"; foutY<<y<<"\t"; foutTime<<event_timestamp<<"\t";
+ }
+ //foutX.close(); foutY.close(); foutTime.close();
+
+/*
+ cv::Mat data=cv::Mat(3, 66, CV_64F, cv::Scalar::all(0.));
+ int counter = 0;
+ std::ifstream foutX("/home/fran/xpos.txt");
+ std::ifstream foutY("/home/fran/ypos.txt");
+ std::ifstream foutTime("/home/fran/timestamp.txt");
+ final_timestamp = 9.963;
+ for (int i = 0; i < 66; i++)
+ {
+ double x, y, event_timestamp;
+ foutX>>x; foutY>>y; foutTime>>event_timestamp;
+ data.at<double>(cv::Point(i, 0))= x/numCols;
+ data.at<double>(cv::Point(i, 1))= y/numRows;
+ data.at<double>(cv::Point(i, 2))= event_timestamp/final_timestamp;
+ }
+ foutX.close(); foutY.close(); foutTime.close();
+* /
+
+ //----------------------------------------------------------------------------
+ //Alternatively, one can use only the time
+ //cv::Mat result_image;
+ //cv::normalize(diffTimestampMatrix, result_image, 0, 255, cv::NORM_MINMAX, CV_8UC1);
+ //----------------------------------------------------------------------------
+
+ cv::Mat clusterCenters;
+ cv::Mat segmentation=cv::Mat(numRows, numCols, CV_8UC3);
+ segmentation = cv::Scalar(128,128,128);
+
+ std::vector<double> clustCentX, clustCentY, clustCentZ;
+ std::vector<int> point2Clusters;
+ double bandwidth = 0.2;
+ std::vector<cv::Vec3b> RGBColors;
+ cv::RNG rng(12345);
+
+ //ROS_ERROR_STREAM("Just before meanshift "<<msg->events.size());
+ meanshiftCluster_Gaussian(data, &clustCentX, &clustCentY, &clustCentZ, &point2Clusters, bandwidth);
+
+ //Now, draw the different segments with a color
+ for(int i=0; i<clustCentX.size(); i++)
+ RGBColors.push_back(cv::Vec3b((uchar)random(), (uchar)random(), (uchar)random()));
+
+ counter =0;
+ for (int i = 0; i < msg->events.size(); i++)
+ {
+ const int x = msg->events[i].x;
+ const int y = msg->events[i].y;
+ double ts = (1E-6*(double)(msg->events[i].ts.toNSec()-first_timestamp));//now in usecs
+
+ if(ts<15)
+ {
+ segmentation.at<cv::Vec3b>(cv::Point(x,y))=RGBColors[point2Clusters[i]];
+ counter++;
+ }
+ else
+ {
+ break;
+ }
+ //ROS_ERROR_STREAM("segmentation["<<x<<","<<y<<"] = "<<RGBColors[point2Clusters[i]]);
+ }
+
+ cv_segments.encoding = "bgr8";
+ cv_segments.image =segmentation;
+ image_segmentation_pub_.publish(cv_segments.toImageMsg());
+ //std::cin.ignore();
+
+ //Saving RGB image
+ //if (counterGlobal>20)
+ //{
+ // char filename[80];
+ // sprintf(filename,"/home/fran/RGB_%0d.png", counterGlobal+1);
+ // cv::imwrite(filename , segmentation);
+ // exit(0);
+ //}
+ //counterGlobal++;
+ //-------------------------------------------------------------------------------------
+
+ }
+}*/
+
+void Meanshift::eventsCallback_simple(const dvs_msgs::EventArray::ConstPtr& msg)
+{
+ //std::cout<<" IM ALIVE?? 10"<<std::endl;
+ // only create image if at least one subscriber
+ if (image_pub_.getNumSubscribers() > 0)
+ {
+ std::cout<<" Wanky shit demon"<<std::endl;
+ cv_bridge::CvImage cv_image;
+
+ if (display_method_ == RED_BLUE)
+ {
+ cv_image.encoding = "bgr8";
+
+ if (last_image_.rows == msg->height && last_image_.cols == msg->width)
+ {
+ last_image_.copyTo(cv_image.image);
+ used_last_image_ = true;
+ }
+ else
+ {
+ cv_image.image = cv::Mat(msg->height, msg->width, CV_8UC3);
+ cv_image.image = cv::Scalar(128, 128, 128);
+ }
+
+ for (int i = 0; i < msg->events.size(); ++i)
+ {
+ const int x = msg->events[i].x;
+ const int y = msg->events[i].y;
+
+ cv_image.image.at<cv::Vec3b>(cv::Point(x, y)) = (
+ msg->events[i].polarity == true ? cv::Vec3b(255, 0, 0) : cv::Vec3b(0, 0, 255));
+ }
+
+ //Write the total number of events
+ //foutX<<msg->events.size()<<std::endl;
+ }
+ else
+ {
+ std::cout<<" Wanky shit demon 2"<<std::endl;
+ cv_image.encoding = "mono8";
+ cv_image.image = cv::Mat(msg->height, msg->width, CV_8U);
+ cv_image.image = cv::Scalar(128);
+
+ cv::Mat on_events = cv::Mat(msg->height, msg->width, CV_8U);
+ on_events = cv::Scalar(0);
+
+ cv::Mat off_events = cv::Mat(msg->height, msg->width, CV_8U);
+ off_events = cv::Scalar(0);
+
+ // count events per pixels with polarity
+ for (int i = 0; i < msg->events.size(); ++i)
+ {
+ const int x = msg->events[i].x;
+ const int y = msg->events[i].y;
+
+ if (msg->events[i].polarity == 1)
+ on_events.at<uint8_t>(cv::Point(x, y))++;
+ else
+ off_events.at<uint8_t>(cv::Point(x, y))++;
+ }
+
+ // scale image
+ cv::normalize(on_events, on_events, 0, 128, cv::NORM_MINMAX, CV_8UC1);
+ cv::normalize(off_events, off_events, 0, 127, cv::NORM_MINMAX, CV_8UC1);
+
+ cv_image.image += on_events;
+ cv_image.image -= off_events;
+ }
+ image_pub_.publish(cv_image.toImageMsg());
+ }
+
+/*
+ // only create image if at least one subscriber
+ if (image_debug1_pub_.getNumSubscribers() > 0)
+ {
+ cv_bridge::CvImage cv_image;
+
+ if (display_method_ == RED_BLUE)
+ {
+ cv_image.encoding = "bgr8";
+
+ if (last_image_.rows == msg->height && last_image_.cols == msg->width)
+ {
+ last_image_.copyTo(cv_image.image);
+ used_last_image_ = true;
+ }
+ else
+ {
+ cv_image.image = cv::Mat(msg->height, msg->width, CV_8UC3);
+ cv_image.image = cv::Scalar(128, 128, 128);
+ }
+
+ if(firstevent)
+ {
+ firsttimestamp = (1E-6*(double)(msg->events[0].ts.toNSec()));
+ firstevent = false;
+ }
+
+ double maxTs;
+ int posx, posy;
+ double usTime = 16.0;
+ double ts;
+ for (int i = 0; i < msg->events.size(); ++i)
+ {
+ const int x = msg->events[i].x;
+ const int y = msg->events[i].y;
+ ts = (1E-6*(double)(msg->events[i].ts.toNSec())) - firsttimestamp;
+#if BG_FILTERING
+ //BGAFframe.at<float>(cv::Point(x,y))=(float)(1E-6*(double)(msg->events[i].ts.toNSec()));
+ BGAFframe.at<float>(cv::Point(x,y))=0.;
+ maxTs = -1;
+ //ROS_ERROR_STREAM("NEW EVENT "<<x<<","<<y<<":"<<ts);
+ for(int ii=-1; ii<=1; ii++)
+ for(int jj=-1; jj<=1; jj++)
+ {
+ posx = x + ii;
+ posy = y + jj;
+ if(posx<0)
+ posx = 0;
+ if(posy<0)
+ posy=0;
+ if(posx>numRows-1)
+ posx = numRows-1;
+ if(posy>numCols-1)
+ posy = numCols-1;
+
+ if(BGAFframe.at<float>(cv::Point(posx,posy)) > maxTs)
+ maxTs = BGAFframe.at<float>(cv::Point(posx,posy));
+
+ //ROS_ERROR_STREAM(posx<<","<<posy<<":"<<BGAFframe.at<float>(cv::Point(posx,posy)));
+ }
+ //ROS_ERROR_STREAM("maxTs: "<<maxTs);
+ BGAFframe.at<float>(cv::Point(x,y))=ts;
+ //ROS_ERROR_STREAM(BGAFframe.at<float>(cv::Point(x,y)) - maxTs);
+
+ if(BGAFframe.at<float>(cv::Point(x,y)) >= (maxTs + usTime))
+ {
+ continue;
+ //ROS_ERROR_STREAM("HERE");
+ }
+
+#endif
+ cv_image.image.at<cv::Vec3b>(cv::Point(x, y)) = (
+ msg->events[i].polarity == true ? cv::Vec3b(255, 0, 0) : cv::Vec3b(0, 0, 255));
+ }
+ //ROS_ERROR_STREAM("DONE--------------------------------------------");
+
+ //Write the total number of events
+ //foutX<<msg->events.size()<<std::endl;
+ }
+ image_debug1_pub_.publish(cv_image.toImageMsg());
+ }
+*/
+
+
+ if (image_segmentation_pub_.getNumSubscribers() > 0)
+ {
+ //std::cout<<" Wanky shit demon 3"<<std::endl;
+ cv_bridge::CvImage cv_segments;
+
+ //Doing color meanshift
+ //cv::Mat diffTimestampMatrix = cv::Mat(numRows, numCols, CV_64F, cv::Scalar::all(0.));
+ //cv::Mat timestampMatrix = cv::Mat(numRows, numCols, CV_64F, cv::Scalar::all(0.));
+
+ uint64_t first_timestamp = msg->events[0].ts.toNSec();
+ double final_timestamp = (1E-6*(double)(msg->events[(msg->events.size())-1].ts.toNSec()-first_timestamp));
+ std::vector<bool> activeEvents(msg->events.size());
+
+ int counterIn = 0;
+ int counterOut = 0;
+
+ int beginEvent = 0;
+ int packet;
+ if(DVSW==240)
+ packet = 1800;
+ else
+ packet = 1500;
+ //ROS_ERROR_STREAM("Before while");
+
+
+ while(beginEvent < msg->events.size())
+ {
+ std::cout<<" Wanky shit demon 4"<<std::endl;
+ //if(msg->events.size()>15e3)
+ //ROS_ERROR_STREAM("BEGIN packet "<<beginEvent);
+ //SELECT SMALL PACKETS OF MAXIMUM 1500 events
+ counterIn = 0;
+ counterOut = 0;
+ //ROS_ERROR_STREAM("Computing events ["<<beginEvent<<","<<min(beginEvent+packet, msg->events.size())<<"]");
+ //ROS_ERROR_STREAM("SIZE of current package is "<<msg->events.size()<<"];");
+
+ // count events per pixels with polarity
+ cv::Mat data=cv::Mat(3, min(packet, msg->events.size()-beginEvent), CV_64F, cv::Scalar::all(0.));
+
+ //std::ofstream foutX("/home/fran/xpos.txt");
+ //std::ofstream foutY("/home/fran/ypos.txt");
+ //std::ofstream foutTime("/home/fran/timestamp.txt");
+
+ //Filter events
+ //cv::Mat BGAFframe = cv::Mat(numRows, numCols, CV_32FC1);
+ //frame =cv::Scalar(-1);
+
+ if(firstevent)
+ {
+ firsttimestamp = (1E-6*(double)(msg->events[10].ts.toNSec()));
+ firstevent = false;
+ }
+
+ double maxTs;
+ int posx, posy;
+ double usTime = 10.0;
+ double ts;
+
+
+ int size = msg->events.size();
+ for (int i = beginEvent; i < min(beginEvent+packet, msg->events.size()); i++)
+ {
+
+ //SELECT SMALL PACKETS OF MAXIMUM 1000 events
+ const int x = msg->events[counterIn].x;
+ const int y = msg->events[counterIn].y;
+
+ double event_timestamp = (1E-6*(double)(msg->events[counterIn].ts.toNSec()-first_timestamp));//now in usecs
+
+ ts = (1E-6*(double)(msg->events[i].ts.toNSec())) - firsttimestamp;
+#if BG_FILTERING
+ //BGAFframe.at<float>(cv::Point(x,y))=(float)(1E-6*(double)(msg->events[i].ts.toNSec()));
+ BGAFframe.at<float>(cv::Point(x,y))=0.;
+ maxTs = -1;
+ //calculate maximum in a 3x3 neighborhood
+ for(int ii=-1; ii<=1; ii++)
+ for(int jj=-1; jj<=1; jj++)
+ {
+ posx = x + ii;
+ posy = y + jj;
+ if(posx<0)
+ posx = 0;
+ if(posy<0)
+ posy=0;
+ if(posx>numRows-1)
+ posx = numRows-1;
+ if(posy>numCols-1)
+ posy = numCols-1;
+
+ if(BGAFframe.at<float>(cv::Point(posx,posy)) > maxTs)
+ maxTs = BGAFframe.at<float>(cv::Point(posx,posy));
+ }
+
+ BGAFframe.at<float>(cv::Point(x,y))=ts;
+ //if nothing happened in usTime, remove the event
+ if(BGAFframe.at<float>(cv::Point(x,y)) >= (maxTs + usTime))
+ {
+ activeEvents.at(counterIn)=false;
+ counterIn++;
+ }
+ else
+ {
+#endif
+ std::cout<<" Wanky shit demon 6: " << i << "size of message: " << size <<std::endl;
+ data.at<double>(cv::Point(counterOut, 0))= (double)x/numCols;
+ data.at<double>(cv::Point(counterOut, 1))= (double)y/numRows;
+ //data.at<double>(cv::Point(counter, 2))= event_timestamp/final_timestamp;//normalized
+ double tau = 10000;
+ data.at<double>(cv::Point(counterOut, 2))= exp(-(final_timestamp-event_timestamp)/tau);//normalized
+ activeEvents.at(counterIn)=true;
+ counterIn++;
+ counterOut++;
+#if BG_FILTERING
+ }
+#endif
+
+std::cout<<" Wanky shit demon 5: " << i << "counterOut: " << counterIn <<std::endl;
+
+ //foutX<<x<<"\t"; foutY<<y<<"\t"; foutTime<<event_timestamp<<"\t";
+ }
+ std::cout<<"Wank on the shit"<<std::endl;
+ double last_timestamp = (1E-6*(double)(msg->events[counterIn-1].ts.toNSec()));//now in usecs
+ //foutX.close(); foutY.close(); foutTime.close();
+
+ cv::Mat clusterCenters;
+ cv::Mat segmentation=cv::Mat(numRows, numCols, CV_8UC3);
+ segmentation = cv::Scalar(128,128,128);
+
+ cv::Mat traj = cv::Mat(numRows, numCols, CV_8UC3);
+ traj = cv::Scalar(128,128,128);
+
+ std::vector<double> clustCentX, clustCentY, clustCentZ;
+ std::vector<int> point2Clusters;
+ std::vector<int> positionClusterColor;
+ std::vector<int> assign_matches;
+
+
+ //double bandwidth = 0.05;
+ double bandwidth = 0.15;
+ //double bandwidth = 0.15;
+ //double bandwidth = 0.20;
+ //double bandwidth = 0.25;
+ //double bandwidth = 0.50;
+ //cv::RNG rng(12345);
+
+/* if(counterGlobal==0)
+ {
+ //clock_t start, end;
+ //double elapsed;
+ //start = clock();
+ //clustCentZ_old = clustCentZ;
+
+ meanshiftCluster_Gaussian(data, &clustCentX, &clustCentY, &clustCentZ, &point2Clusters, bandwidth);
+
+ assignClusterColor(&positionClusterColor, clustCentX.size(), assign_matches, prev_positionClusterColor); //assign new colors to clusters
+
+ prev_clustCentX = clustCentX;
+ prev_clustCentY = clustCentY;
+ prev_positionClusterColor = positionClusterColor;
+ //std::vector<int> checkvector (clustCentX.size(), -1);
+ //ROS_ERROR_STREAM("Total number of clusters is "<<clustCentX.size());
+
+
+ //end = clock();
+ //elapsed = ((double) (end - start)) / CLOCKS_PER_SEC;
+ //ROS_ERROR_STREAM("Num. packets = "<<data.cols<<" elapsed time = "<<elapsed<<". Time/packet = "<<elapsed/data.cols);
+
+ //Moved to initialization
+ //Now, draw the different segments with a color
+ //for(int i=0; i<clustCentX.size(); i++)
+ // RGBColors.push_back(cv::Vec3b((uchar)random(), (uchar)random(), (uchar)random()));
+
+ counter =0;
+ for (int i = beginEvent; i < min(beginEvent+packet, msg->events.size()); i++)
+ {
+ const int x = msg->events[counter].x;
+ const int y = msg->events[counter].y;
+ double ts = (1E-6*(double)(msg->events[counter].ts.toNSec()-first_timestamp));//now in usecs
+
+ //if(ts<15)
+ //{
+ //segmentation.at<cv::Vec3b>(cv::Point(x,y))=RGBColors[point2Clusters[counter]];
+ //segmentation.at<cv::Vec3b>(cv::Point(x,y))=RGBColors[(positionClusterColor[point2Clusters[counter]])%MAX_NUM_CLUSTERS];
+ segmentation.at<cv::Vec3b>(cv::Point(x,y))=RGBColors[(positionClusterColor[point2Clusters[counter]])&MAX_NUM_CLUSTERS]; //cheaper than % (mod operation)
+ //}
+ //else
+ // break;
+ counter++;
+ }
+ }
+ else
+ {
+
+*/
+
+ //fouttotalproc<<min(packet, msg->events.size()-beginEvent)<<std::endl;
+std::cout<<"Wank on the shit 2"<<std::endl;
+ meanshiftCluster_Gaussian(data, &clustCentX, &clustCentY, &clustCentZ, &point2Clusters, bandwidth);
+std::cout<<"Wank on the shit 3"<<std::endl;
+ //Assign new color or use color from previous clusters?
+ //if(counterGlobal>0)//not the first time
+ findClosestCluster(&assign_matches, clustCentX, clustCentY, prev_clustCentX, prev_clustCentY); //no match when clustID is -1
+std::cout<<"Wank on the shit 4"<<std::endl;
+ //assignClusterColor(&positionClusterColor, clustCentX.size(), assign_matches, prev_positionClusterColor); //assign new colors to clusters
+ assignClusterColor(&positionClusterColor, clustCentX.size(), assign_matches, prev_positionClusterColor, prev_activeTrajectories); //assign new colors to clusters
+std::cout<<"Wank on the shit 5"<<std::endl;
+ //foutnumclust<<clustCentX.size()<<std::endl;
+
+ int tmpColorPos;
+ float estimClustCenterX=-1., estimClustCenterY=-1.;
+ //std::vector<int> activeTrajectories(MAX_NUM_CLUSTERS,0);
+ std::vector<int> activeTrajectories;
+
+ if(point2Clusters.size()>400) //update positions only when there is enough change (>25%)
+ {
+ std::cout<<" Wanky shit demon 6"<<std::endl;
+ for(int i=0; i<clustCentX.size(); i++)
+ {
+ estimClustCenterX=-1., estimClustCenterY=-1.;
+
+ tmpColorPos = (positionClusterColor[i])&(MAX_NUM_CLUSTERS-1);
+
+ //Check if the new position is being used
+ /*int idx = 0;
+ for (int idx=0; idx<prev_activeTrajectories.size(); idx++)
+ if(tmpColorPos == prev_activeTrajectories[idx])
+ break;*/
+ std::vector<int>::iterator it;
+ it = find (prev_activeTrajectories.begin(), prev_activeTrajectories.end(), tmpColorPos);
+
+ if(it==prev_activeTrajectories.end())//if the element tmpcolorpos is not found in the vector
+ {
+ //Initialize the trajectory (just the counter)
+ //allTrajectories[tmpColorPos] = std::vector<cv::Point>(MAX_NUM_TRAJECTORY_POINTS);
+ counterTrajectories[tmpColorPos]=0;
+
+#if KALMAN_FILTERING
+ foundTrajectory[tmpColorPos] = false;
+ createKalmanFilter(&(vector_of_kf[tmpColorPos]), &(vector_of_state[tmpColorPos]), &(vector_of_meas[tmpColorPos]));
+ vector_of_foundBlobs[tmpColorPos]=false;
+ vector_of_ticks[tmpColorPos]=0.0;
+#endif
+ }
+
+ /*
+ if(prev_activeTrajectories[tmpColorPos]==0) //TODO: when the cluster is tracked all the time, it is active when I go back with &MAX_NUM_CLUSTERS
+ {
+ //Initialize the trajectory (just the counter)
+ //allTrajectories[tmpColorPos] = std::vector<cv::Point>(MAX_NUM_TRAJECTORY_POINTS);
+ counterTrajectories[tmpColorPos]=0;
+ }*/
+
+#if KALMAN_FILTERING
+ if(counterTrajectories[tmpColorPos]>25 & !foundTrajectory[tmpColorPos])
+ {
+ //selectedTrajectory = tmpColorPos;
+ foundTrajectory[tmpColorPos] = true;
+ }
+ //if(foundTrajectory[tmpColorPos] & selectedTrajectory == tmpColorPos) //More than 25 points in the trajectory, start w/ KF
+ if(foundTrajectory[tmpColorPos]) //More than 25 points in the trajectory, start w/ KF
+ {
+ //double precTick = ticks;
+ //ticks = (double) cv::getTickCount();
+ //double dT = (ticks - precTick) / cv::getTickFrequency(); //seconds
+
+ /* In case we use only 1 kalman filter
+ if((last_timestamp - ticks) > 1)
+ {
+ double precTick = ticks;
+ ticks = last_timestamp;
+ double dT = (ticks - precTick)/1000; //seconds
+
+ if (foundBlobs)
+ {
+ // >>>> Matrix A
+ kf.transitionMatrix.at<float>(2) = dT;
+ kf.transitionMatrix.at<float>(7) = dT;
+ // <<<< Matrix A
+ state = kf.predict();
+ estimClustCenterX = state.at<float>(0);
+ estimClustCenterY = state.at<float>(1);
+ }
+
+ meas.at<float>(0) = clustCentX[i]*DVSW;
+ meas.at<float>(1) = clustCentY[i]*DVSH; //[z_x, z_y]
+
+ if (!foundBlobs) // First detection!
+ {
+ // >>>> Initialization
+ kf.errorCovPre.at<float>(0) = 1; // px
+ kf.errorCovPre.at<float>(5) = 1; // px
+ kf.errorCovPre.at<float>(10) = 2;
+ kf.errorCovPre.at<float>(15) = 2;
+
+ state.at<float>(0) = meas.at<float>(0);
+ state.at<float>(1) = meas.at<float>(1);
+ state.at<float>(2) = 0;
+ state.at<float>(3) = 0; //[z_x, z_y, v_x, v_y]
+ // <<<< Initialization
+
+ kf.statePost = state;
+
+ foundBlobs = true;
+ }
+ else
+ kf.correct(meas); // Kalman Correction
+
+ if(estimClustCenterX>=0.) //initialized to -1
+ {
+ //ROS_ERROR_STREAM("Estimated point = ["<<estimClustCenterX<<", "<<estimClustCenterY<<"];");
+ //ROS_ERROR_STREAM("Measured point = ["<<clustCentX[i]<<", "<<clustCentY[i]<<"];");
+ cv::circle(trajectories, cv::Point(clustCentX[i]*DVSW, clustCentY[i]*DVSH), 2, cv::Scalar( 0, 0, 255 ),-1,8);
+ cv::circle(trajectories, cv::Point(estimClustCenterX, estimClustCenterY), 2, cv::Scalar( 255, 0, 0),-1,8);
+ foutX<<clustCentX[i]*DVSW<<" "<<clustCentY[i]*DVSH<<std::endl;
+ foutX_estim<<estimClustCenterX<<" "<<estimClustCenterY<<std::endl;
+ foutT<<dT<<std::endl;
+ }
+ }
+ */
+
+ if((last_timestamp - vector_of_ticks[tmpColorPos]) > 1)
+ {
+ double precTick = vector_of_ticks[tmpColorPos];
+ vector_of_ticks[tmpColorPos] = last_timestamp;
+ double dT = (vector_of_ticks[tmpColorPos] - precTick)/1000; //seconds
+ //if (foundBlobs)
+ if(vector_of_foundBlobs[tmpColorPos])
+ {
+ // >>>> Matrix A
+ vector_of_kf[tmpColorPos].transitionMatrix.at<float>(2) = dT;
+ vector_of_kf[tmpColorPos].transitionMatrix.at<float>(7) = dT;
+ // <<<< Matrix A
+ vector_of_state[tmpColorPos] = vector_of_kf[tmpColorPos].predict();
+ estimClustCenterX = vector_of_state[tmpColorPos].at<float>(0);
+ estimClustCenterY = vector_of_state[tmpColorPos].at<float>(1);
+ }
+ vector_of_meas[tmpColorPos].at<float>(0) = clustCentX[i]*DVSW;
+ vector_of_meas[tmpColorPos].at<float>(1) = clustCentY[i]*DVSH; //[z_x, z_y]
+
+
+ //if (!foundBlobs) // First detection!
+ if(!vector_of_foundBlobs[tmpColorPos])
+ {
+ // >>>> Initialization
+ vector_of_kf[tmpColorPos].errorCovPre.at<float>(0) = 1; // px
+ vector_of_kf[tmpColorPos].errorCovPre.at<float>(5) = 1; // px
+ vector_of_kf[tmpColorPos].errorCovPre.at<float>(10) = 2;
+ vector_of_kf[tmpColorPos].errorCovPre.at<float>(15) = 2;
+
+ vector_of_state[tmpColorPos].at<float>(0) = vector_of_meas[tmpColorPos].at<float>(0);
+ vector_of_state[tmpColorPos].at<float>(1) = vector_of_meas[tmpColorPos].at<float>(1);
+ vector_of_state[tmpColorPos].at<float>(2) = 0;
+ vector_of_state[tmpColorPos].at<float>(3) = 0; //[z_x, z_y, v_x, v_y]
+ // <<<< Initialization
+
+ vector_of_kf[tmpColorPos].statePost = vector_of_state[tmpColorPos];
+
+ //foundBlobs = true;
+ vector_of_foundBlobs[tmpColorPos]=true;
+ }
+ else
+ vector_of_kf[tmpColorPos].correct(vector_of_meas[tmpColorPos]); // Kalman Correction
+
+ if(estimClustCenterX>=0.) //initialized to -1
+ {
+ //ROS_ERROR_STREAM("Estimated difference = ["<<abs(estimClustCenterX/DVSW -clustCentX[i]) <<", "<<abs(estimClustCenterY/DVSH-clustCentY[i])<<"];");
+ //ROS_ERROR_STREAM("Estimated point = ["<<estimClustCenterX<<", "<<estimClustCenterY<<"];");
+ //ROS_ERROR_STREAM("Measured point = ["<<clustCentX[i]<<", "<<clustCentY[i]<<"];");
+ //cv::circle(trajectories, cv::Point(clustCentX[i]*DVSW, clustCentY[i]*DVSH), 2, cv::Scalar( 0, 0, 255 ),-1,8);
+ //cv::circle(trajectories, cv::Point(estimClustCenterX, estimClustCenterY), 2, cv::Scalar( 255, 0, 0),-1,8);
+
+ //foutX<<tmpColorPos<<" "<<clustCentX[i]*DVSW<<" "<<clustCentY[i]*DVSH<<" "<<estimClustCenterX<<" "<<estimClustCenterY<<std::endl;
+
+ //foutX_estim<<estimClustCenterX<<" "<<estimClustCenterY<<std::endl;
+ //foutT<<dT<<std::endl;
+ //foutX<<"Estimated difference = ["<<abs(estimClustCenterX/DVSW -clustCentX[i]) <<", "<<abs(estimClustCenterY/DVSH-clustCentY[i])<<"];"<<std::endl;
+
+ // !!!!!!CAREFUL ****************************************************************************************
+ //ACTIVATE THIS !!!!!
+ //clustCentX[i] = estimClustCenterX/DVSW;
+ //clustCentY[i] = estimClustCenterY/DVSH;
+ // CAREFUL ****************************************************************************************
+ }
+ }
+ }
+#endif
+
+ cv::Point end(clustCentX[i]*DVSW, clustCentY[i]*DVSH);
+/*
+#if KALMAN_FILTERING
+ //frame, event, colorCluster, X,Y, filtX, filtY
+ foutX<<counterGlobal+1<<" "<<(double)(1E-6*(msg->events[beginEvent].ts.toNSec()-msg->events[0].ts.toNSec()))<<" " <<tmpColorPos<<" "<<clustCentX[i]*DVSW<<" "<<clustCentY[i]*DVSH<<" "<<estimClustCenterX<<" "<<estimClustCenterY<<std::endl;
+#endif
+*/
+
+ allTrajectories[tmpColorPos][(counterTrajectories[tmpColorPos]) & (MAX_NUM_TRAJECTORY_POINTS-1)]=end;
+ counterTrajectories[tmpColorPos]++;
+ //activeTrajectories[tmpColorPos]=1;
+ activeTrajectories.push_back(tmpColorPos);
+ }
+
+ prev_clustCentX = clustCentX;
+ prev_clustCentY = clustCentY;
+ prev_positionClusterColor = positionClusterColor;
+ prev_activeTrajectories = activeTrajectories;
+
+ trajectories = cv::Scalar(128,128,128);
+ int first=1;
+ for(int i=0; i<activeTrajectories.size(); i++)
+ {
+ int tmpval = activeTrajectories[i];
+ if (counterTrajectories[tmpval]>1)
+ {
+ if(counterTrajectories[tmpval]<=MAX_NUM_TRAJECTORY_POINTS)
+ {
+ for(int j=1; j<counterTrajectories[tmpval]; j++) //instead of % I use &(power of 2 -1): it is more efficient
+ cv::line( trajectories, allTrajectories[tmpval][j-1], allTrajectories[tmpval][j], cv::Scalar( RGBColors[tmpval].val[0], RGBColors[tmpval].val[1], RGBColors[tmpval].val[2]), 2, 1);
+ }
+ else
+ {
+ int j=1;
+ int pos = counterTrajectories[tmpval];
+ while(j<MAX_NUM_TRAJECTORY_POINTS)
+ {
+ cv::line( trajectories, allTrajectories[tmpval][pos & (MAX_NUM_TRAJECTORY_POINTS-1)], allTrajectories[tmpval][(pos+1)&(MAX_NUM_TRAJECTORY_POINTS-1)], cv::Scalar( RGBColors[tmpval].val[0], RGBColors[tmpval].val[1], RGBColors[tmpval].val[2]), 2, 1);
+ pos = pos+1;
+ j++;
+ }
+ }
+ }
+
+ }
+ }
+
+ trajectories.copyTo(segmentation); //Always keep trajectories
+
+/*
+#if TEST
+ char filename[80];
+ sprintf(filename,"/home/fran/0_tmp/kk/RGB_%0d.png", counterGlobal+1);
+ cv::imwrite(filename , segmentation);
+
+ sprintf(filename,"/home/fran/0_tmp/kk/RGB_%0d.txt", counterGlobal+1);
+ std::ofstream foutallT(filename);
+
+ for(int k=0; k<activeTrajectories.size(); k++)
+ if(counterTrajectories[activeTrajectories[k]]>0)
+ {
+ foutallT<<"ClusterID "<<activeTrajectories[k]<<" with "<<counterTrajectories[activeTrajectories[k]]<<" elems: = [";
+ for(int j=0; j<counterTrajectories[activeTrajectories[k]]; j++) //instead of % I use &(power of 2 -1): it is more efficient
+ {
+ foutallT<<"("<<allTrajectories[activeTrajectories[k]][j].x<<", "<<allTrajectories[activeTrajectories[k]][j].y<<"), ";
+ }
+ foutallT<<"];\n";
+ }
+
+ foutallT.close();
+#endif
+*/
+
+#if TEST
+ char filename1[80];
+ sprintf(filename1,"/home/chris/0_tmp/kk/rgb_%0d.txt", counterGlobal+1);
+ std::ofstream foutX(filename1);
+#endif
+ counterIn =0;
+ counterOut=0;
+ for (int i = beginEvent; i < min(beginEvent+packet, msg->events.size()); i++)
+ {
+ if(activeEvents.at(counterIn)) //Label it only if it is not noise
+ {
+ const int x = msg->events[counterIn].x;
+ const int y = msg->events[counterIn].y;
+ double ts = (1E-6*(double)(msg->events[counterIn].ts.toNSec()-first_timestamp));//now in usecs
+
+ //if(ts<15) //This is just for painting, we are processing all events
+ //{
+ //segmentation.at<cv::Vec3b>(cv::Point(x,y))=RGBColors[(positionClusterColor[point2Clusters[counter]])%MAX_NUM_CLUSTERS];
+ segmentation.at<cv::Vec3b>(cv::Point(x,y))=RGBColors[(positionClusterColor[point2Clusters[counterOut]])&(MAX_NUM_CLUSTERS-1)]; //cheaper than % (mod operation)
+ #if TEST
+ //foutX<<x<<" "<<y<<" "<<ts<<" "<<msg->events[counter].polarity<<'\n';
+ foutX<<x<<'\t'<<'\t'<<y<<'\t'<<ts<<'\t'<<(int)(msg->events[counter].polarity)<<'\t'<<(int)((positionClusterColor[point2Clusters[counter]])&(MAX_NUM_CLUSTERS-1))<<'\n';
+ #endif
+ counterOut++;
+ }
+ //}
+ //else
+ //{
+ // break;
+ //}
+ counterIn++;
+ }
+
+
+ //char filename2[80];
+ //sprintf(filename2,"/home/chris/0_tmp/kk/rgb_%0d.png", counterGlobal+1);
+ //cv::imwrite(filename2, segmentation);
+
+
+// }
+
+
+ cv_segments.encoding = "bgr8";
+ cv_segments.image = segmentation;
+ std::cout<<" Wanky shit demon 7"<<std::endl;
+ image_segmentation_pub_.publish(cv_segments.toImageMsg());
+ std::cout<<" Wanky shit demon 8"<<std::endl;
+ //std::cin.ignore();
+
+ beginEvent +=packet;
+
+//#if TEST
+ //char filename[80];
+ //sprintf(filename,"/home/fran/0_tmp/kk/rgb_%0d.jpg", counterGlobal+1);
+ //cv::imwrite(filename , segmentation);
+ //sprintf(filename,"/home/fran/0_tmp/kk/cc_%0d.txt", counterGlobal+1);
+ //std::ofstream foutcc(filename);
+ //for (int i = 0; i<clustCentX.size(); i++)
+ // foutcc<<"ClusterCenter("<<i+1<<",:)=["<<clustCentX.at(i)*DVSW<<","<<clustCentY.at(i)*DVSH<<"];"<<std::endl;
+ //foutcc.close();
+//#endif
+ //Saving RGB image
+/*
+ if (counterGlobal>0)
+ {
+ //std::cin.ignore();
+ //for (int i = 0; i<clustCentX.size(); i++)
+ // ROS_ERROR_STREAM("ClusterCenter("<<i+1<<",:)=["<<clustCentX.at(i)<<","<<clustCentY.at(i)<<","<<clustCentZ.at(i)<<"];");
+
+ //std::cout<<"clustperevent=["<<point2Clusters[0];
+ //for (int i = 1; i<point2Clusters.size(); i++)
+ // std::cout<<","<<point2Clusters[i];
+ //std::cout<<"];"<<std::endl;
+
+ char filename[80];
+ sprintf(filename,"/home/fran/0_tmp/kk/RGB_%0d.png", counterGlobal+1);
+ cv::imwrite(filename , segmentation);
+ //exit(0);
+
+ //char filename[80];
+ //sprintf(filename,"/home/fran/0_tmp/kk/RGB_%0d.txt", counterGlobal+1);
+ //std::ofstream foutX(filename);
+ //foutX<<counter<<"\n";
+ //foutX.close();
+ }
+*/
+
+
+ /*if(msg->events.size()>15e3)
+ {
+ char filename[80];
+ sprintf(filename,"/home/fran/0_tmp/kk/RGB_%0d.png", counterGlobal+1);
+ cv::imwrite(filename , segmentation);
+ exit(0);
+ }*/
+ counterGlobal++;
+ }
+ }
+}
+
+
+
+
+} // namespace
diff --git a/RWR/src/mean-shift/dvs_meanshift/src/meanshift_node.cpp b/RWR/src/mean-shift/dvs_meanshift/src/meanshift_node.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..879b219b3532f680d1f1d3c5af0841e88bca02c1
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/src/meanshift_node.cpp
@@ -0,0 +1,17 @@
+
+
+#include "dvs_meanshift/meanshift.h"
+
+int main(int argc, char* argv[])
+{
+ ros::init(argc, argv, "dvs_meanshift");
+
+ ros::NodeHandle nh;
+ ros::NodeHandle nh_private("~");
+
+ dvs_meanshift::Meanshift meanshift(nh, nh_private);
+
+ ros::spin();
+
+ return 0;
+}
diff --git a/RWR/src/mean-shift/dvs_meanshift/src/segment.cpp b/RWR/src/mean-shift/dvs_meanshift/src/segment.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..8d1d30b230d18c5235e77ecb73f6d3076dc30af7
--- /dev/null
+++ b/RWR/src/mean-shift/dvs_meanshift/src/segment.cpp
@@ -0,0 +1,171 @@
+/*
+Copyright (C) 2006 Pedro Felzenszwalb
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation; either version 2 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+*/
+
+#include <cstdio>
+#include <cstdlib>
+#include "graph3d/segment.h"
+#include "graph3d/image.h"
+#include "graph3d/misc.h"
+#include "graph3d/pnmfile.h"
+#include "graph3d/segment-image.h"
+
+
+#include <cstdio>
+#include <cstdlib>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include "graph3d/image.h"
+#include "graph3d/misc.h"
+#include "graph3d/pnmfile.h"
+#include "graph3d/segment-image.h"
+
+/*
+int main(int argc, char **argv) {
+ if (argc != 6) {
+ fprintf(stderr, "usage: %s sigma k min input(ppm) output(ppm)\n", argv[0]);
+ return 1;
+ }
+
+ float sigma = atof(argv[1]);
+ float k = atof(argv[2]);
+ int min_size = atoi(argv[3]);
+
+ printf("loading input image.\n");
+ image<rgb> *input = loadPPM(argv[4]);
+
+ printf("processing\n");
+ int num_ccs;
+ image<rgb> *seg = segment_image(input, sigma, k, min_size, &num_ccs);
+ savePPM(seg, argv[5]);
+
+
+ printf("got %d components\n", num_ccs);
+ printf("done! uff...thats hard work.\n");
+
+ return 0;
+}*/
+
+
+
+//void imadjust(const cv::Mat & src, cv::Mat & dst, int tol = 1, cv::Vec2i in = cv::Vec2i(0, 255), cv::Vec2i out = cv::Vec2i(0, 255))
+void imadjust(const cv::Mat & src, cv::Mat & dst)
+{
+ // src : input CV_8UC1 image
+ // dst : output CV_8UC1 imge
+ // tol : tolerance, from 0 to 100.
+ // in : src image bounds
+ // out : dst image buonds
+
+ int in[2]; in[0]=0; in[1]=255;
+ int out[2]; out[0]=0; out[1]=255;
+ int tol =1;
+
+
+
+ dst = src.clone();
+
+ tol = std::max(0, std::min(100, tol));
+
+ if (tol > 0)
+ {
+ // Compute in and out limits
+
+ // Histogram
+ std::vector<int> hist(256, 0);
+ for (int r = 0; r < src.rows; ++r) {
+ for (int c = 0; c < src.cols; ++c) {
+ hist[src.at<uint8_t>(cv::Point(r,c))]++;
+ }
+ }
+
+ // Cumulative histogram
+ std::vector<int> cum = hist;
+ for (int i = 1; i < hist.size(); ++i) {
+ cum[i] = cum[i - 1] + hist[i];
+ }
+
+ // Compute bounds
+ int total = src.rows * src.cols;
+ int low_bound = total * tol / 100;
+ int upp_bound = total * (100-tol) / 100;
+ in[0] = distance(cum.begin(), lower_bound(cum.begin(), cum.end(), low_bound));
+ in[1] = distance(cum.begin(), lower_bound(cum.begin(), cum.end(), upp_bound));
+
+ }
+
+ // Stretching
+ float scale = float(out[1] - out[0]) / float(in[1] - in[0]);
+ for (int r = 0; r < dst.rows; ++r)
+ {
+ for (int c = 0; c < dst.cols; ++c)
+ {
+ int vs = std::max(src.at<uint8_t>(cv::Point(r, c)) - in[0], 0);
+
+
+ int vd = std::min(int(vs * scale + 0.5f) + out[0], out[1]);
+ dst.at<uint8_t>(cv::Point(r, c)) = cv::saturate_cast<uchar>(vd);
+ }
+ }
+}
+
+//This function is just an interface to call the segment_image function
+void im2segment(cv::Mat input, cv::Mat cv_image_output, float sigma, float k, int min_size, int *num_ccs)
+{
+
+ image<rgb> *inputIm=new image<rgb>(input.cols, input.rows);
+
+
+ for (int y = 0; y < input.rows; y++) {
+ for (int x = 0; x < input.cols; x++) {
+ imRef(inputIm, x, y).r = input.at<uint8_t>(cv::Point(x, y));
+ imRef(inputIm, x, y).g = input.at<uint8_t>(cv::Point(x, y));
+ imRef(inputIm, x, y).b = input.at<uint8_t>(cv::Point(x, y));
+ }
+ }
+
+ std::cout<<"I'm here!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"<<std::endl;
+ savePPM(inputIm, "/home/chris/input_image.ppm");
+
+ image<rgb> *seg = segment_image(inputIm, sigma, k, min_size, num_ccs);
+
+ //cv_image_output.encoding = "bgr8";
+ //cv_image_output.image =cv::Mat(input.rows, input.cols, CV_8UC3);
+
+ for (int y = 0; y < input.rows; y++) {
+ for (int x = 0; x < input.cols; x++) {
+ //output.at<uint8_t>(cv::Point(x, y))=imRef(seg, x, y);
+ cv_image_output.at<cv::Vec3b>(cv::Point(x, y)) = ( \
+ cv::Vec3b(imRef(seg,x,y).b, imRef(seg,x,y).g, imRef(seg,x,y).r));
+ }
+ }
+
+
+ /*image<uchar> *inputTmp=new image<uchar>(input.cols, input.rows);;
+ for (int y = 0; y < input.rows; y++) {
+ for (int x = 0; x < input.cols; x++) {
+ imRef(inputTmp, x, y) = input.at<uint8_t>(cv::Point(x, y));
+ }
+ }
+ savePGM(inputTmp, "/home/fran/input_image.ppm");
+ */
+
+ //printf("got %d components\n", num_ccs);
+ //printf("done! uff...thats hard work.\n");
+}
+
diff --git a/RWR/src/mean-shift/rpg_dvs_ros b/RWR/src/mean-shift/rpg_dvs_ros
new file mode 160000
index 0000000000000000000000000000000000000000..95f08d565c3115b181a4763e1e350f4df0375c4b
--- /dev/null
+++ b/RWR/src/mean-shift/rpg_dvs_ros
@@ -0,0 +1 @@
+Subproject commit 95f08d565c3115b181a4763e1e350f4df0375c4b