[Jderobot-admin] jderobot-r1246 - in trunk/src/stable/libs: . depthLib

frivas en jderobot.org frivas en jderobot.org
Mar Nov 11 22:13:43 CET 2014


Author: frivas
Date: 2014-11-11 22:13:43 +0100 (Tue, 11 Nov 2014)
New Revision: 1246

Added:
   trunk/src/stable/libs/depthLib/
   trunk/src/stable/libs/depthLib/CMakeLists.txt
   trunk/src/stable/libs/depthLib/DepthFilter.cpp
   trunk/src/stable/libs/depthLib/DepthFilter.h
   trunk/src/stable/libs/depthLib/DepthSampler.cpp
   trunk/src/stable/libs/depthLib/DepthSampler.h
Log:
[#286] included depthLib library


Added: trunk/src/stable/libs/depthLib/CMakeLists.txt
===================================================================
--- trunk/src/stable/libs/depthLib/CMakeLists.txt	                        (rev 0)
+++ trunk/src/stable/libs/depthLib/CMakeLists.txt	2014-11-11 21:13:43 UTC (rev 1246)
@@ -0,0 +1,15 @@
+include_directories(${SLICE_DIR}) # Aquí se alojan las cabeceras de las interfaces ICE en C++
+include_directories(${INTERFACES_CPP_DIR}) # Aquí se alojan las cabeceras de las interfaces ICE en C++
+include_directories(${LIBS_DIR}) # Aquí se alojan las cabeceras de las interfaces ICE en C++
+
+
+include_directories( ${LIBS_DIR}/)
+
+add_library (depthLib STATIC DepthFilter.cpp  DepthFilter.h  DepthSampler.cpp  DepthSampler.h)
+TARGET_LINK_LIBRARIES(depthLib ${Boost_LIBRARIES})
+
+add_library (depthLibshare SHARED DepthFilter.cpp  DepthFilter.h  DepthSampler.cpp  DepthSampler.h)
+TARGET_LINK_LIBRARIES(depthLibshare ${Boost_LIBRARIES})
+
+
+set_target_properties(depthLibshare PROPERTIES OUTPUT_NAME depthLib)

Added: trunk/src/stable/libs/depthLib/DepthFilter.cpp
===================================================================
--- trunk/src/stable/libs/depthLib/DepthFilter.cpp	                        (rev 0)
+++ trunk/src/stable/libs/depthLib/DepthFilter.cpp	2014-11-11 21:13:43 UTC (rev 1246)
@@ -0,0 +1,354 @@
+/*
+ * DepthFilter.cpp
+ *
+ *  Created on: 04/01/2014
+ *      Author: frivas
+ */
+
+#include "DepthFilter.h"
+
+namespace jderobot {
+
+DepthFilter::DepthFilter(){
+	//default parameters
+	this->type=3;
+	this->buffer.resize(0);
+	this->buffSize=7;
+	this->erodeSize=0;
+	this->threshold=10;
+}
+
+DepthFilter::DepthFilter(int type, int buffSize, int erodeSize, int threshold) {
+	this->type=type;
+	this->buffSize=buffSize;
+	this->buffer.resize(0);
+	this->erodeSize=erodeSize;
+	this->threshold=threshold;
+}
+
+DepthFilter::~DepthFilter() {
+	// TODO Auto-generated destructor stub
+}
+//filtrado bilateral, elimina ruido y mantiene los bordes
+void
+DepthFilter::filterDakkak(cv::Mat imageIn, cv::Mat& imageOut){
+
+	int M=50;
+	int MaxIter=100;
+	int K=40;
+
+	/*for (int m=0; m<M; m++){
+		for (int x=0; x< imageIn.cols ; x++){//x++){
+			for (int y=0; y<imageIn.rows; y++){//y++){
+				int maxIter=0;
+				for (int k=1; k<K; k++){
+					if (maxIter > MaxIter)
+						break;
+					maxIter++;
+
+
+				}
+			}
+	}*/
+	std::vector<cv::Mat> layers;
+	cv::split(imageIn, layers);
+	cv::bilateralFilter(layers[0], imageOut, 20, 21, 3);
+	cv::cvtColor(imageOut,imageOut,CV_GRAY2RGB);
+}
+
+void
+DepthFilter::filterMeanNonMotion3Channels(cv::Mat imageIn, cv::Mat& imageOut){
+	cv::Mat localSource,meanNonZeroImage;
+
+	cv::Mat zeros = cv::Mat(imageIn.rows,imageIn.cols, CV_8UC3, cv::Scalar(0,0,0));
+	cv::Mat ones= cv::Mat(imageIn.rows,imageIn.cols, CV_8UC3, cv::Scalar(1,1,1));
+
+	imageIn.copyTo(localSource);
+	this->m.lock();
+	/*for (int i=0; i< this->erodeSize;i++){
+		cv::erode(localSource,localSource,cv::Mat());
+	}
+	for (int i=0; i< this->erodeSize;i++){
+		cv::dilate(localSource,localSource,cv::Mat());
+	}*/
+
+	this->buffer.push_back(localSource);
+	while (this->buffer.size()>this->buffSize)
+		this->buffer.pop_front();
+	cv::Mat onesAcc; //acumulado de unos en al buffer
+	cv::Mat resultAcc; //resultado acumulado
+
+	zeros.copyTo(imageOut);
+	zeros.copyTo(onesAcc);
+	zeros.copyTo(resultAcc);
+
+	resultAcc.convertTo(resultAcc,CV_32FC3);
+
+	for ( std::list<cv::Mat>::iterator it=this->buffer.begin(); it!= this->buffer.end();it++){
+		cv::Mat localOnes;
+
+		cv::Mat localItFloat;
+		it->convertTo(localItFloat,CV_32FC3);
+		resultAcc=resultAcc+localItFloat;
+		ones.copyTo(localOnes,*it);
+		onesAcc=onesAcc+localOnes;
+	}
+	onesAcc.convertTo(onesAcc,CV_32FC3);
+	resultAcc=resultAcc/onesAcc;
+	resultAcc.copyTo(meanNonZeroImage);
+
+
+
+	zeros.copyTo(onesAcc);
+	zeros.copyTo(resultAcc);
+	resultAcc.convertTo(resultAcc,CV_32FC3);
+	cv::Mat accAndMask;
+	cv::Mat localDiff, localMask;
+	for ( std::list<cv::Mat>::iterator it=this->buffer.begin(); it!= this->buffer.end();it++){
+		if (it==this->buffer.begin())
+			it->copyTo(accAndMask);
+
+		cv::Mat localOnes;
+		cv::Mat localItFloat;
+		it->convertTo(localItFloat,CV_32FC3);
+		cv::absdiff(localItFloat,meanNonZeroImage,localDiff);
+		localDiff.convertTo(localDiff,CV_8UC3);
+		cv::threshold(localDiff,localMask,this->threshold, 255, CV_THRESH_BINARY_INV);
+		resultAcc=resultAcc+localItFloat;
+		localMask.convertTo(localMask,CV_8UC3);
+		ones.copyTo(localOnes,localMask);
+		onesAcc=onesAcc+localOnes;
+		accAndMask=accAndMask & localMask;
+	}
+	onesAcc.convertTo(onesAcc,CV_32FC3);
+	resultAcc=resultAcc/onesAcc;
+	resultAcc.convertTo(resultAcc,CV_8UC3);
+
+	resultAcc.copyTo(imageOut, accAndMask);
+	zeros.copyTo(ones, accAndMask);
+
+	imageIn.copyTo(imageOut,ones);
+
+	//copio la parte sin movimiento del promedio de imágenes y la parte de movimiento de la última captura
+
+
+		meanNonZeroImage.convertTo(this->globalMeanImage,CV_8UC3);
+		localDiff.convertTo(this->globalDiffImage,CV_8UC3);
+
+		accAndMask.copyTo(this->globalFirstMask);
+		ones.copyTo(this->globalSecondMask);
+	this->m.unlock();
+}
+
+//filtra únicamente las zonas de la imagen donde no hay movimiento
+void DepthFilter::filterMeanNonMotion1Channels(cv::Mat imageIn, cv::Mat& imageOut){
+	cv::Mat localSource,meanNonZeroImage;
+
+	cv::Mat zeros3c = cv::Mat(imageIn.rows,imageIn.cols, CV_8UC3, cv::Scalar(0,0,0));
+	cv::Mat ones3c= cv::Mat(imageIn.rows,imageIn.cols, CV_8UC3, cv::Scalar(1,1,1));
+	cv::Mat zeros1c = cv::Mat(imageIn.rows,imageIn.cols, CV_8UC1, cv::Scalar(0));
+	cv::Mat ones1c= cv::Mat(imageIn.rows,imageIn.cols, CV_8UC1, cv::Scalar(1));
+
+	imageIn.copyTo(localSource);
+
+	std::vector<cv::Mat> layers;
+	cv::split(localSource, layers);
+
+	/*for (int i=0; i< this->erodeSize;i++){
+		cv::erode(localSource,localSource,cv::Mat());
+	}
+	for (int i=0; i< this->erodeSize;i++){
+		cv::dilate(localSource,localSource,cv::Mat());
+	}*/
+	this->m.lock();
+	this->buffer.push_back(localSource);
+	this->bufferGray.push_back(layers[0]);
+	while (this->buffer.size()>this->buffSize)
+		this->buffer.pop_front();
+	while (this->bufferGray.size()>this->buffSize)
+		this->bufferGray.pop_front();
+	cv::Mat onesAcc; //acumulado de unos en al buffer
+	cv::Mat resultAcc; //resultado acumulado
+
+	zeros3c.copyTo(imageOut);
+	zeros3c.copyTo(onesAcc);
+	zeros3c.copyTo(resultAcc);
+
+	resultAcc.convertTo(resultAcc,CV_32FC1);
+
+	//calculo la imagen promedio para luego hacer la comparación contra todas las imágenes individuales
+	//y obtener las zonas comunes (sin movimiento y que podemos suavizar).
+	for ( std::list<cv::Mat>::iterator it=this->buffer.begin(); it!= this->buffer.end();it++){
+		cv::Mat localOnes;
+
+		cv::Mat localItFloat;
+		it->convertTo(localItFloat,CV_32FC3);
+		resultAcc=resultAcc+localItFloat;
+		ones3c.copyTo(localOnes,*it);
+		onesAcc=onesAcc+localOnes;
+	}
+	onesAcc.convertTo(onesAcc,CV_32FC3);
+	resultAcc=resultAcc/onesAcc;
+	resultAcc.copyTo(meanNonZeroImage); //imagen promedio sin promediar zonas que son iguales a cero
+
+	meanNonZeroImage.convertTo(meanNonZeroImage,CV_8UC3);
+
+	//imagen promedio en escala de grises para el cálculo de las máscaras:
+	std::vector<cv::Mat> layersMean;
+	cv::split(meanNonZeroImage, layersMean);
+
+
+
+	zeros1c.copyTo(onesAcc);
+	zeros1c.copyTo(resultAcc);
+	resultAcc.convertTo(resultAcc,CV_32FC1);
+	cv::Mat accAndMask;
+
+	ones1c.copyTo(accAndMask);
+	cv::Mat localDiff, localMask;
+	//comparamos todas las imagenes con el promedio y obtenemos zonas comunes
+	for ( std::list<cv::Mat>::iterator it=this->bufferGray.begin(); it!= this->bufferGray.end();it++){
+
+		cv::Mat localOnes;
+		cv::absdiff(*it,layersMean[0],localDiff);
+		localDiff.convertTo(localDiff,CV_8UC1);
+		cv::threshold(localDiff,localMask,this->threshold, 255, CV_THRESH_BINARY_INV);
+		localMask.convertTo(localMask,CV_8UC1);
+		ones1c.copyTo(localOnes,localMask);
+		accAndMask=accAndMask & localMask;
+	}
+
+	onesAcc.convertTo(onesAcc,CV_32FC3);
+	zeros3c.copyTo(ones3c, accAndMask);
+	for (int i=0; i< this->erodeSize;i++){
+		cv::erode(accAndMask,accAndMask,cv::Mat());
+		//cv::erode(ones3c,ones3c,cv::Mat());
+
+	}
+	for (int i=0; i< this->erodeSize;i++){
+		cv::dilate(accAndMask,accAndMask,cv::Mat());
+		//cv::dilate(ones3c,ones3c,cv::Mat());
+
+	}
+
+	meanNonZeroImage.copyTo(imageOut, accAndMask);
+	imageIn.copyTo(imageOut,ones3c);
+
+	//copio la parte sin movimiento del promedio de imágenes y la parte de movimiento de la última captura
+
+
+		meanNonZeroImage.copyTo(this->globalMeanImage);
+		cv::cvtColor(localDiff,globalDiffImage,CV_GRAY2RGB);
+		accAndMask=accAndMask*255;
+		cv::cvtColor(accAndMask,this->globalFirstMask,CV_GRAY2RGB);
+		ones3c=ones3c*255;
+		ones3c.copyTo(this->globalSecondMask);
+	this->m.unlock();
+
+
+
+
+}
+
+//funcion que implementa el filtrado por media con valors no nulos
+void
+DepthFilter::filterMeanNonZero(cv::Mat imageIn, cv::Mat& imageOut){
+	cv::Mat localSource,result;
+
+	cv::Mat zeros = cv::Mat(imageIn.rows,imageIn.cols, CV_8UC3, cv::Scalar(0,0,0));
+	cv::Mat ones= cv::Mat(imageIn.rows,imageIn.cols, CV_8UC3, cv::Scalar(1,1,1));
+
+	imageIn.copyTo(localSource);
+
+	for (int i=0; i< this->erodeSize;i++){
+		cv::erode(localSource,localSource,cv::Mat());
+	}
+	for (int i=0; i< this->erodeSize;i++){
+		cv::dilate(localSource,localSource,cv::Mat());
+	}
+	//localSource.convertTo(localSource,CV_32FC3);
+	this->buffer.push_back(localSource);
+	while (this->buffer.size()>this->buffSize)
+		this->buffer.pop_front();
+	cv::Mat onesAcc; //acumulado de unos en al buffer
+	cv::Mat resultAcc; //resultado acumulado
+
+	zeros.copyTo(onesAcc);
+	zeros.copyTo(resultAcc);
+
+	resultAcc.convertTo(resultAcc,CV_32FC3);
+
+
+	for ( std::list<cv::Mat>::iterator it=this->buffer.begin(); it!= this->buffer.end();it++){
+		cv::Mat localOnes;
+
+		cv::Mat localItFloat;
+		it->convertTo(localItFloat,CV_32FC3);
+
+		resultAcc=resultAcc+localItFloat;
+		std::vector<cv::Mat> layers;
+		//cv::split(*it, layers);
+		ones.copyTo(localOnes,*it);
+		onesAcc=onesAcc+localOnes;
+	}
+	onesAcc.convertTo(onesAcc,CV_32FC3);
+	resultAcc=resultAcc/onesAcc;
+	resultAcc.convertTo(resultAcc,CV_8UC3);
+	resultAcc.copyTo(imageOut);
+
+}
+
+
+void DepthFilter::getMeanImage(cv::Mat& out){
+	this->m.lock();
+	this->globalMeanImage.copyTo(out);
+	this->m.unlock();
+}
+void DepthFilter::getDiffImage(cv::Mat& out){
+	this->m.lock();
+	this->globalDiffImage.copyTo(out);
+	this->m.unlock();
+}
+void DepthFilter::getFistMask(cv::Mat& out){
+	this->m.lock();
+	this->globalFirstMask.copyTo(out);
+	this->m.unlock();
+}
+void DepthFilter::getSecondMask(cv::Mat& out){
+	this->m.lock();
+	this->globalSecondMask.copyTo(out);
+	this->m.unlock();
+}
+
+void DepthFilter::clear(){
+	this->m.lock();
+	this->buffer.resize(0);
+	this->bufferGray.resize(0);
+	this->m.unlock();
+
+};
+
+
+//función iterativa que realimenta el filtrado
+void
+DepthFilter::update(cv::Mat imageIn, cv::Mat& imageOut){
+	switch(type){
+		case 0:
+			filterMeanNonZero(imageIn,imageOut);
+			break;
+		case 1:
+			filterDakkak(imageIn,imageOut);
+			break;
+		case 2:
+			filterMeanNonMotion3Channels(imageIn, imageOut);
+			break;
+		case 3:
+			filterMeanNonMotion1Channels(imageIn, imageOut);
+			break;
+		default:
+			std::cout << "Filter method: " << this->type << "not implemented" << std::endl;
+			break;
+	}
+}
+
+} /* namespace jderobot */

Added: trunk/src/stable/libs/depthLib/DepthFilter.h
===================================================================
--- trunk/src/stable/libs/depthLib/DepthFilter.h	                        (rev 0)
+++ trunk/src/stable/libs/depthLib/DepthFilter.h	2014-11-11 21:13:43 UTC (rev 1246)
@@ -0,0 +1,63 @@
+/*
+ * DepthFilter.h
+ *
+ *  Created on: 04/01/2014
+ *      Author: frivas
+ */
+
+#ifndef DEPTHFILTER_H_
+#define DEPTHFILTER_H_
+
+#include <cv.h>
+#include <highgui.h>
+#include <list>
+#include <IceUtil/Mutex.h>
+
+namespace jderobot {
+
+class DepthFilter {
+public:
+	DepthFilter(int type, int buffSize, int erodeSize, int threshold);
+	DepthFilter();
+	virtual ~DepthFilter();
+	void update(cv::Mat imageIn, cv::Mat& imageOut);
+	int getBufferSize(){return this->buffSize;};
+	void setBufferSize(int size){ this->buffSize=size;};
+	int getErodeSize(){return this->erodeSize;};
+	void setErodeSize(int size){ this->erodeSize=size;};
+	int getThreshold(){return this->threshold;};
+	void setThreshold(int value){this->threshold=value;};
+	void setFilterType(int value){this->type=value;};
+	int getFilterType(){return this->type;};
+	void clear();
+	void getMeanImage(cv::Mat& out);
+	void getDiffImage(cv::Mat& out);
+	void getFistMask(cv::Mat& out);
+	void getSecondMask(cv::Mat& out);
+
+private:
+	int type; //tipo de filtro a aplicar
+	int buffSize; //tamaño del buffer de imágenes
+	std::list<cv::Mat> buffer; //buffer de imágenes
+	std::list<cv::Mat> bufferGray; //buffer de imágenes en escala de grises
+	int erodeSize; //numero de erosiones a realizar en el filtrado
+	int threshold; //umbral para el filtrado
+	IceUtil::Mutex m; //control de datos compartidos
+
+	//callbacks
+	void filterMeanNonZero(cv::Mat imageIn, cv::Mat& imageOut);
+	void filterDakkak(cv::Mat imageIn, cv::Mat& imageOut);
+	void filterMeanNonMotion3Channels(cv::Mat imageIn, cv::Mat& imageOut);
+	void filterMeanNonMotion1Channels(cv::Mat imageIn, cv::Mat& imageOut);
+
+	//tempImages
+	cv::Mat globalMeanImage;
+	cv::Mat globalDiffImage;
+	cv::Mat globalFirstMask;
+	cv::Mat globalSecondMask;
+
+};
+
+} /* namespace jderobot */
+
+#endif /* DEPTHFILTER_H_ */

Added: trunk/src/stable/libs/depthLib/DepthSampler.cpp
===================================================================
--- trunk/src/stable/libs/depthLib/DepthSampler.cpp	                        (rev 0)
+++ trunk/src/stable/libs/depthLib/DepthSampler.cpp	2014-11-11 21:13:43 UTC (rev 1246)
@@ -0,0 +1,170 @@
+/*
+ * DepthSampler.cpp
+ *
+ *  Created on: 08/01/2014
+ *      Author: eldercare
+ */
+
+#include "DepthSampler.h"
+
+namespace jderobot {
+DepthSampler::DepthSampler(int nBins, int maxDistance, int minInd, float step){
+	this->nBins=nBins;
+	this->maxDistance=maxDistance;
+	this->minInd=minInd;
+	this->step=step;
+}
+
+DepthSampler::DepthSampler() {
+	this->nBins=1;
+	this->maxDistance=10000;
+	this->minInd=3;
+	this->step=1;
+}
+
+DepthSampler::~DepthSampler() {
+	// TODO Auto-generated destructor stub
+}
+
+//devuelve en out, los nBins layers con la profundidad discretizada hasta maxDistance
+void DepthSampler::calculateLayers(cv::Mat source, std::vector<cv::Mat>& layers){
+	std::vector<cv::Mat> imgLayers;
+	cv::Mat localSource;
+
+	layers.resize(0);
+
+	source.convertTo(localSource,CV_32FC3);
+	cv::split(localSource, imgLayers);
+	cv::Mat dM(localSource.rows,localSource.cols, CV_32FC1);
+	dM=(imgLayers[1]*256) + imgLayers[2];
+
+	cv::Mat unosc1(localSource.rows, localSource.cols, CV_8UC1, cv::Scalar(255));
+	cv::Mat zerosc1(localSource.rows, localSource.cols, CV_8UC1, cv::Scalar(0));
+
+	IceUtil::Time t=IceUtil::Time::now();
+	for (int i=0; i<nBins; i++){
+		cv::Mat localMask;
+		cv::threshold(dM,localMask,(maxDistance/nBins)*(i+1), 255, CV_THRESH_BINARY_INV);
+		localMask.convertTo(localMask,CV_8UC1);
+		layers.push_back(localMask&unosc1);
+		zerosc1.copyTo(unosc1,localMask);
+	}
+}
+
+//compara el muestreo normal con el muestreo por capas, empezando desde minInd, e incrementando step en cada capa,
+//desde la mas lejana hasta la mas cercana.
+void DepthSampler::evalSample(cv::Mat source, std::vector<cv::Mat> layers, int samplingRate, cv::Mat &outSNormal, cv::Mat &outSLayers){
+	cv::Mat imgNormalSample=cv::Mat(source.rows,source.cols, CV_8UC1, cv::Scalar(0));
+	cv::Mat imgNlayerSample=cv::Mat(source.rows,source.cols, CV_8UC1, cv::Scalar(0));;
+
+	std::vector<int> normalSample;
+	int outlierNormal=0;
+	int ourlierSample=0;
+	std::vector<int> layerSample;
+
+	int nLayers=layers.size();
+	normalSample.resize(nLayers);
+	layerSample.resize(nLayers);
+
+	cv::Mat localSource;
+	std::vector<cv::Mat> tempLayers;
+	source.convertTo(localSource,CV_32FC3);
+	cv::split(localSource, tempLayers);
+	cv::Mat dM(localSource.rows,localSource.cols, CV_32FC1);
+	dM=(tempLayers[1]*256) + tempLayers[2];
+
+
+	for (int i=0; i< nLayers;i++){
+		normalSample[i]=0;
+		layerSample[i]=0;
+	}
+
+	IceUtil::Time n=IceUtil::Time::now();
+	for (int xIm=0; xIm< source.cols; xIm+=samplingRate) {
+		for (int yIm=0; yIm<source.rows ; yIm+=samplingRate) {
+			float d=dM.at<float>(yIm,xIm);
+			if (d != 0){
+				imgNormalSample.at<char>(yIm,xIm)=(char)255;
+				double pos= d/maxDistance;
+				if (pos>1){
+					outlierNormal++;
+				}
+				else{
+					normalSample[floor(pos*nLayers)]=normalSample[floor(pos*nLayers)]++;
+				}
+			}
+		}
+	}
+	std::cout << "Time for normal sampling: " << IceUtil::Time::now().toMilliSeconds() - n.toMilliSeconds() << std::endl;
+	std::cout << "Layer sampling Size : " << std::accumulate(normalSample.begin(), normalSample.end(), 0) << ",  result:" << std::endl;
+	for (std::vector<int>::iterator it= normalSample.begin(); it!= normalSample.end(); it++){
+		std::cout << *it << std::endl;
+	}
+	n=IceUtil::Time::now();
+
+	float localStep=minInd+(step*nLayers);
+	for ( std::vector<cv::Mat>::iterator it= layers.begin(); it != layers.end(); it++){
+		std::cout << "step: " << (int)localStep << std::endl;
+		for (int xIm=0; xIm< source.cols; xIm+=(int)localStep) {
+			for (int yIm=0; yIm<source.rows ; yIm+=(int)localStep) {
+				if ((int)it->at<char>(yIm,xIm) != 0){
+					float d=dM.at<float>(yIm,xIm);
+
+					if (d != 0){
+						imgNlayerSample.at<char>(yIm,xIm)=(char)255;
+						double pos= d/maxDistance;
+						if (pos>1){
+							outlierNormal++;
+						}
+						else{
+							layerSample[floor(pos*nLayers)]=layerSample[floor(pos*nLayers)]++;
+						}
+					}
+				}
+			}
+		}
+
+		localStep=localStep-step;
+	}
+	std::cout << "Time for layers sampling: " << IceUtil::Time::now().toMilliSeconds() - n.toMilliSeconds() << std::endl;
+	std::cout << "Layer sampling Size : " << std::accumulate(layerSample.begin(), layerSample.end(), 0) << ",  result:" << std::endl;
+	for (std::vector<int>::iterator it= layerSample.begin(); it!= layerSample.end(); it++){
+		std::cout << *it << std::endl;
+	}
+
+	/*cv::imshow("normal", imgNormalSample);
+	cv::imshow("layers", imgNlayerSample);
+	cv::waitKey(0);*/
+	imgNormalSample.copyTo(outSNormal);
+	imgNlayerSample.copyTo(outSLayers);
+}
+
+void DepthSampler::sample(cv::Mat source, std::vector<cv::Mat> layers, std::vector<cv::Point2i>& out){
+
+	cv::Mat localSource;
+	std::vector<cv::Mat> tempLayers;
+	source.convertTo(localSource,CV_32FC3);
+	cv::split(localSource, tempLayers);
+	cv::Mat dM(localSource.rows,localSource.cols, CV_32FC1);
+	dM=(tempLayers[1]*256) + tempLayers[2];
+	out.resize(0);
+
+	int nLayers=layers.size();
+	float localStep=minInd+(step*(double)nLayers);
+	for ( std::vector<cv::Mat>::iterator it= layers.begin(); it != layers.end(); it++){
+		for (int xIm=0; xIm< source.cols; xIm+=(int)localStep) {
+			for (int yIm=0; yIm<source.rows ; yIm+=(int)localStep) {
+				if ((int)it->at<char>(yIm,xIm) != 0){
+					float d=dM.at<float>(yIm,xIm);
+					if ((d != 0)&&(d<maxDistance)){
+						out.push_back(cv::Point2i(xIm,yIm));
+					}
+				}
+			}
+		}
+		localStep=localStep-step;
+	}
+}
+
+
+} /* namespace jderobot */

Added: trunk/src/stable/libs/depthLib/DepthSampler.h
===================================================================
--- trunk/src/stable/libs/depthLib/DepthSampler.h	                        (rev 0)
+++ trunk/src/stable/libs/depthLib/DepthSampler.h	2014-11-11 21:13:43 UTC (rev 1246)
@@ -0,0 +1,46 @@
+/*
+ * DepthSampler.h
+ *
+ *  Created on: 08/01/2014
+ *      Author: eldercare
+ */
+
+#ifndef DEPTHSAMPLER_H_
+#define DEPTHSAMPLER_H_
+
+#include <cv.h>
+#include <highgui.h>
+#include <list>
+#include <IceUtil/Mutex.h>
+#include <numeric>
+namespace jderobot {
+
+class DepthSampler {
+public:
+	DepthSampler(int nBins, int maxDistance, int minInd, float step);
+	DepthSampler();
+	virtual ~DepthSampler();
+	void calculateLayers(cv::Mat source, std::vector<cv::Mat>& layers);
+	void evalSample(cv::Mat source, std::vector<cv::Mat> layers, int samplingRate, cv::Mat &outSNormal, cv::Mat &outSLayers);
+	void sample(cv::Mat source, std::vector<cv::Mat> layers, std::vector<cv::Point2i>& out);
+
+	void setnBins(int value){this->nBins=value;};
+	int getnBins(){return this->nBins;};
+	void setMaxDistance(int value){this->maxDistance=value;};
+	int getMaxDistance(){return this->maxDistance;};
+	void setMinInd(int value){this->minInd=value;};
+	int getMinInd(){return this->minInd;};
+	void setStep(double value){this->step=value; std::cout << "SETTING STEP TO: " << value << std::endl;};
+	double getStep(){return this->step;};
+
+
+
+private:
+	int nBins, maxDistance, minInd;
+	double step;
+
+};
+
+} /* namespace jderobot */
+
+#endif /* DEPTHSAMPLER_H_ */



More information about the Jderobot-admin mailing list