[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