rmap_utils.h
#ifndef UTILS_RANGE_MAP_H
#define UTILS_RANGE_MAP_H
#include <stdlib.h>
#include <string>
#include <stdio.h>
#include <vector>
#include <sstream>
#include <fstream>
#include <iostream>
#include "opencv2\opencv.hpp"
class rmap_utils
{
public:
rmap_utils::rmap_utils(){}
rmap_utils::~rmap_utils(){}
int loadRmap(const std::string &path, float *& xMap, float *& yMap, float *& zMap, unsigned char *&rgbMap, int &height, int &width);
cv::Mat visualize_depth_image(float *img, int width, int height, bool visualize, std::string wName = "rangeMap");
cv::Mat visualize_rgb_image(unsigned char *img, int width, int height, bool visualize, std::string wName = "rangeMap");
cv::Mat visualize_normalized_rgb_image(float *img, int width, int height, bool visualize, std::string wName = "rangeMap");
float compute_rangeMap_resolution(float *xMap, float *yMap, float *zMap, int width, int height);
};
#endif
rmap_utils.cpp
#include "rmap_utils.h"
void Tokenize(const std::string &line, std::vector<std::string> &tokens, const std::string &delimiters = " ", const bool skipEmptyCells = true, const int maxNumTokens = std::numeric_limits<int>::max())
{
tokens.clear();
int nTokens = 0;
std::string::size_type pos = 0;
std::string::size_type lastPos = 0;
if(skipEmptyCells)
{
// Skip delimiters at beginning.
lastPos = line.find_first_not_of(delimiters, 0);
// Find first "non-delimiter".
pos = line.find_first_of(delimiters, lastPos);
while ( (std::string::npos != pos || std::string::npos != lastPos) && (nTokens < maxNumTokens) )
{
// Found a token, add it to the vector.
tokens.push_back(line.substr(lastPos, pos - lastPos));
nTokens++;
// Skip delimiters
lastPos = line.find_first_not_of(delimiters, pos);
// Find next "non-delimiter"
pos = line.find_first_of(delimiters, lastPos);
}
}
else
{
while ( (std::string::npos != pos) && (nTokens < maxNumTokens) )
{
pos = line.find_first_of(delimiters, lastPos);
tokens.push_back(line.substr(lastPos, pos - lastPos));
nTokens++;
lastPos = pos+1;
}
}
}
template <typename T> void Read_Array(std::ifstream &filev, T* &aData, unsigned long int &nData, const bool binaryFile = true)
{
if (!filev.is_open() )
{
std::cout << "ERROR (Read_Array): file is not open";
getchar();
exit(-1);
}
if(binaryFile)
{
//read number of elements
filev.read((char*)&nData, sizeof(nData) );
aData = new T[nData];
//read data
filev.read((char*)aData, sizeof(T)*nData );
if(filev.gcount() != (sizeof(T)*nData) )
{
std::cout << "ERROR (Read_Array): filev.gcount() != (sizeof(T)*nData) " << filev.gcount() << " " << (sizeof(T)*nData) << std::endl;
std::cout << "Are you sure you opened the file in binary mode?";
getchar();
exit(-1);
}
if(!filev.good())
{
std::cout << "ERROR (Read_Array): !filev.good() [eof fail bad] [" << filev.eof() << " " << filev.fail() << " " << filev.bad() << "]" << std::endl;
std::cout << "Are you sure you opened the file in binary mode?";
getchar();
exit(-1);
}
}
else
{
//read number of elements
std::string line;
std::getline(filev, line);
filev >> nData;
aData = new T[nData];
//read data
T* ptrData = aData;
for(unsigned long int da=0; da<nData; da++)
{
filev >> *ptrData;
ptrData++;
}
std::getline(filev, line);
if(!filev.good())
{
std::cout << "ERROR (Read_Array): !filev.good() [eof fail bad] [" << filev.eof() << " " << filev.fail() << " " << filev.bad() << "]" << std::endl;
getchar();
exit(-1);
}
}
}
template <typename T> void Read_Vector(std::ifstream &filev, std::vector<T> &vData, const bool binaryFile = true)
{
if (!filev.is_open() )
{
std::cout << "ERROR (Read_Vector): file is not open";
getchar();
exit(-1);
}
unsigned long int nData = 0;
if(binaryFile)
{
//read number of elements
filev.read((char*)&nData, sizeof(nData) );
vData.resize((size_t)nData);
//read data
filev.read((char*)(&vData[0]), sizeof(T)*nData );
if(filev.gcount() != (sizeof(T)*nData) )
{
std::cout << "ERROR (Read_Vector): filev.gcount() != (sizeof(T)*nData) " << filev.gcount() << " " << (sizeof(T)*nData) << std::endl;
std::cout << "Are you sure you opened the file in binary mode?";
getchar();
exit(-1);
}
if(!filev.good())
{
std::cout << "ERROR (Read_Vector): !filev.good() [eof fail bad] [" << filev.eof() << " " << filev.fail() << " " << filev.bad() << "]" << std::endl;
std::cout << "Are you sure you opened the file in binary mode?";
getchar();
exit(-1);
}
}
else
{
//read number of elements
std::string line;
std::getline(filev, line);
filev >> nData;
vData.resize((size_t)nData);
//read data
T* ptrData = &vData[0];
for(unsigned long int da=0; da<nData; da++)
{
filev >> (*ptrData);
ptrData++;
}
std::getline(filev, line);
if(!filev.good())
{
std::cout << "ERROR (Read_Vector): !filev.good() [eof fail bad] [" << filev.eof() << " " << filev.fail() << " " << filev.bad() << "]" << std::endl;
getchar();
exit(-1);
}
}
}
template<typename T> T LexicalCast(const std::string& s)
{
std::stringstream ss(s);
T result;
if ((ss >> result).fail() || !(ss >> std::ws).eof())
{
//throw std::bad_cast();
std::cout << "ERROR:Impossible to cast " << s;
getchar();
exit(-1);
}
return result;
}
//passi i puntatori vuoti e lui elaborando li riempe per come ti servono
int loadRmap(const std::string &path, float *& xMap, float *& yMap, float *& zMap, unsigned char *&rgbMap, int &height, int &width)
{
bool binaryFile = true;
std::ifstream in(path.c_str(), std::ios::binary|std::ios::in);
if(!in.is_open())
{
std::cout << "ERROR (RangeMap::Load_RMap): Problems opening file";
getchar();
exit(-1);
}
std::string line;
std::vector<std::string> tokens;
int nValidPoints = 0;
bool texture = false;
int nChannels = 1;
double resolution;
//read header
while (!in.eof())
{
getline (in, line);
// Ignore empty lines
if (line == "")
continue;
// Tokenize the line
Tokenize(line, tokens, "\t\r " );
// first line
if (tokens[0] == "RMap")
continue;
// version
if (tokens[0] == "version")
{
continue;
}
// height
if (tokens[0] == "height")
{
height = LexicalCast<int>(tokens[1]);
continue;
}
// width
if (tokens[0] == "width")
{
width = LexicalCast<int>(tokens[1]);
continue;
}
// nValidPoints
if (tokens[0] == "nValidPoints")
{
nValidPoints = LexicalCast<int>(tokens[1]);
continue;
}
// resolution
if (tokens[0] == "resolution")
{
resolution = LexicalCast<double>(tokens[1]);
continue;
}
// texture
if (tokens[0] == "texture")
{
texture = true;
if (tokens[1] == "GrayScale")
{
nChannels = 1;
}
else if (tokens[1] == "BGR")
{
nChannels = 3;
}
else
{
std::cout << "ERROR (RangeMap::Load_RMap): tokens[1] != \"GrayScale\" and tokens[1] != \"BGR\" ";
getchar();
exit(-1);
}
continue;
}
// end_header
if (tokens[0] == "headerEnd")
{
break;
}
}
if(in.eof())
{
in.close();
std::cout << "ERROR (RangeMap::Load_RMap): end_header tag not reached";
getchar();
exit(-1);
}
//read valid point map
bool* validMap = NULL;
unsigned long int nTotalPoints = 0;
Read_Array(in, validMap, nTotalPoints, binaryFile);
if(nTotalPoints != width*height)
{
in.close();
std::cout << "ERROR (RangeMap::Load_RMap): nTotalPoints != m_width*m_height " << nTotalPoints << " != " << width << " * " << height << std::endl;
getchar();
exit(-1);
}
//read maps
std::vector<float> xVals;
std::vector<float> yVals;
std::vector<float> zVals;
Read_Vector(in, xVals, binaryFile);
Read_Vector(in, yVals, binaryFile);
Read_Vector(in, zVals, binaryFile);
if(xVals.size() != nValidPoints)
{
in.close();
std::cout << "ERROR (RangeMap::Load_RMap): vMap_X.size() != nValidPoints " << xVals.size() << " != " << nValidPoints << std::endl;
getchar();
exit(-1);
}
if(yVals.size() != nValidPoints)
{
in.close();
std::cout << "ERROR (RangeMap::Load_RMap): vMap_Y.size() != nValidPoints " << yVals.size() << " != " << nValidPoints << std::endl;
getchar();
exit(-1);
}
if(zVals.size() != nValidPoints)
{
in.close();
std::cout << "ERROR (RangeMap::Load_RMap): vMap_Z.size() != nValidPoints " << zVals.size() << " != " << nValidPoints << std::endl;
getchar();
exit(-1);
}
//if(xMap)
//{
// delete[] xMap;
//}
xMap = new float[width*height];
/*if(yVals)
{
delete[] yVals;
}*/
yMap = new float[width*height];
/*if(zVals)
{
delete[] zVals;
}*/
zMap = new float[width*height];
float* ptrvMap_X = &xVals[0];
float* ptrvMap_Y = &yVals[0];
float* ptrvMap_Z = &zVals[0];
bool* ptrValidMap = validMap;
float* ptrXmap = xMap;
float* ptrYmap = yMap;
float* ptrZmap = zMap;
for(unsigned long int po=0; po<nTotalPoints; po++)
{
if(*ptrValidMap)
{
*ptrXmap = *(ptrvMap_X++);
*ptrYmap = *(ptrvMap_Y++);
*ptrZmap = *(ptrvMap_Z++);
}
else
{
*ptrZmap = std::numeric_limits<float>::quiet_NaN();
}
ptrXmap++;
ptrYmap++;
ptrZmap++;
ptrValidMap++;
}
delete[] validMap;
//read texture
if(texture)
{
IplImage* m_texture = cvCreateImage(cvSize(width, height), 8, nChannels);
rgbMap = new unsigned char[width*height*3];
in.read( (char*)(m_texture->imageData), height*m_texture->widthStep );
if(in.gcount() != height*m_texture->widthStep)
{
std::cout << "ERROR (RangeMap::Load_RMap): in.gcount() != m_height*m_texture->widthStep " << in.gcount() << " " << height*m_texture->widthStep;
getchar();
exit(-1);
}
if(!in.good())
{
std::cout << "ERROR (RangeMap::Load_RMap): !in.good() in reading m_texture [eof fail bad] [" << in.eof() << " " << in.fail() << " " << in.bad() << "]";
getchar();
exit(-1);
}
for (int j=0; j<height; j++)
{
for ( int i=0; i<width; i++)
{
if ( nChannels == 3)
{
rgbMap[ j*width*3+i*3 ] = ((unsigned char *)(m_texture->imageData))[ j * m_texture->widthStep + i*3];
rgbMap[ j*width*3+i*3 +1 ] = ((unsigned char *)(m_texture->imageData))[ j * m_texture->widthStep + i*3 +1];
rgbMap[ j*width*3+i*3 +2 ] = ((unsigned char *)(m_texture->imageData))[ j * m_texture->widthStep + i*3 +2];
}
else
{
rgbMap[ j*width*3+i*3+2 ] = ((unsigned char *)(m_texture->imageData))[ j * m_texture->widthStep + i];
rgbMap[ j*width*3+i*3+1 ] = ((unsigned char *)(m_texture->imageData))[ j * m_texture->widthStep + i];
rgbMap[ j*width*3+i*3 ] = ((unsigned char *)(m_texture->imageData))[ j * m_texture->widthStep + i];
}
}
}
/*cvNamedWindow("cicciux", 0);
cvShowImage("cicciux", m_texture);
cvWaitKey(0);*/
cvReleaseImage(&m_texture);
}
else
{
rgbMap = NULL;
}
in.close();
return nValidPoints;
}
cv::Mat visualize_depth_image(float *img, int width, int height, bool visualize, std::string wName)
{
cv::Mat mat(height, width, CV_8UC3);
float dmax = -1;
float dmin = std::numeric_limits<float>::max();
for ( int j=0; j<height; j++)
{
for ( int i=0; i<width; i++)
{
if ( img[j*width+i] == img[j*width+i] )
{
if ( img[j*width+i] > dmax )
dmax = img[j*width+i];
if (img[j*width+i]<dmin)
dmin = img[j*width+i];
}
}
}
for ( int j=0; j<height; j++)
{
for ( int i=0; i<width; i++)
{
if ( img[j*width+i] == img[j*width+i] )
{
unsigned char pixel = cv::saturate_cast<unsigned char>( ((img[j*width+i] - dmin)* 255.0f) / (dmax-dmin) ) ;
mat.at<cv::Vec3b>(j,i)[0] = pixel;
mat.at<cv::Vec3b>(j,i)[1] = pixel;
mat.at<cv::Vec3b>(j,i)[2] = pixel;
}
else
{
mat.at<cv::Vec3b>(j,i)[0] = 0;
mat.at<cv::Vec3b>(j,i)[1] = 0;
mat.at<cv::Vec3b>(j,i)[2] = 255;
}
}
}
if ( visualize)
{
cv::imshow(wName, mat);
cv::waitKey(0);
}
return mat;
}
cv::Mat visualize_rgb_image(unsigned char *img, int width, int height, bool visualize, std::string wName)
{
cv::Mat mat(height, width, CV_8UC3);
for ( int j=0; j<height; j++)
{
for ( int i=0; i<width; i++)
{
for ( int c=0; c<3; c++)
{
mat.at<cv::Vec3b>(j,i)[c] = img[j*width*3 + i*3 + c];
}
}
}
if ( visualize )
{
cv::imshow(wName, mat);
cv::waitKey(0);
}
return mat;
}
cv::Mat visualize_normalized_rgb_image(float *img, int width, int height, bool visualize, std::string wName)
{
cv::Mat mat(height, width, CV_8UC3);
for ( int j=0; j<height; j++)
{
for ( int i=0; i<width; i++)
{
for ( int c=0; c<3; c++)
{
mat.at<cv::Vec3b>(j,i)[c] = static_cast<unsigned char>(std::floor(img[j*width*3 + i*3 + c] * 255));
}
}
}
if ( visualize)
{
cv::imshow(wName, mat);
cv::waitKey(0);
}
return mat;
}
float compute_rangeMap_resolution(float *xMap, float *yMap, float *zMap, int width, int height)
{
float res = 0.0f;
int nCount = 0;
for ( int j=0; j<height-1; j++)
{
for ( int i=0; i<width-1; i++)
{
if ( zMap[j*width+i] == zMap[j*width+i] )
{
if ( zMap[j*width+i+1] == zMap[j*width+i+1] )
{
nCount ++ ;
res += (zMap[j*width+i]-zMap[j*width+i+1])*(zMap[j*width+i]-zMap[j*width+i+1]) + (yMap[j*width+i]-yMap[j*width+i+1])*(yMap[j*width+i]-yMap[j*width+i+1]) + (xMap[j*width+i]-xMap[j*width+i+1])*(xMap[j*width+i]-xMap[j*width+i+1]);
}
if ( zMap[(j+1)*width+i] == zMap[(j+1)*width+i] )
{
nCount ++ ;
res += (zMap[j*width+i]-zMap[(j+1)*width+i])*(zMap[j*width+i]-zMap[(j+1)*width+i]) + (yMap[j*width+i]-yMap[(j+1)*width+i])*(yMap[j*width+i]-yMap[(j+1)*width+i]) + (xMap[j*width+i]-xMap[(j+1)*width+i])*(xMap[j*width+i]-xMap[(j+1)*width+i]);
}
}
}
}
res /= nCount;
res = sqrt(res);
return res;
}
Error List: error LNK2019: unresolved external symbol "public: int __cdecl rmap_utils::loadRmap(class std::basic_string,class std::allocator > const &,float * &,float * &,float * &,unsigned char * &,int &,int &)" (?loadRmap@rmap_utils@@QEAAHAEBV?$basic_string@DU?$char_traits@D@std@@V?$allocator@D@2@@std@@AEAPEAM11AEAPEAEAEAH3@Z) referenced in function main
