int Conversion::convert(const Eigen::MatrixXf & depthMat,
Eigen::MatrixXf & point3D,
double &fx,
double &fy,
double &cx,
double &cy)
{
//max resize
point3D.resize(depthMat.rows()*depthMat.cols(),3);
int index(0);
for (int i=0; i<depthMat.rows();i++)
for (int j=0 ; j<depthMat.cols();j++)
{
float z = depthMat(i,j);
if (fabs( z+ 1.f) > std::numeric_limits<float>::epsilon()
& fabs(z) !=1
& z > std::numeric_limits<float>::epsilon() ){
point3D(index,2) = z;
point3D(index,0) = (i-cx)*point3D(index,2)/fx;
point3D(index,1) = (j-cy)*point3D(index,2)/fy;
index++;
}
}
//min resize
point3D.conservativeResize(index,3);
return 1;
}
Eigen::MatrixXf
readDescrFromFile(const std::string &file, int padding, int rowSize)
{
// check if file exists
boost::filesystem::path path = file;
if ( ! (boost::filesystem::exists ( path ) && boost::filesystem::is_regular_file(path)) )
throw std::runtime_error ("Given file path to read Matrix does not exist!");
std::ifstream in (file.c_str (), std::ifstream::in);
int bufferSize = 819200;
//int bufferSize = rowSize * 10;
char linebuf[bufferSize];
Eigen::MatrixXf matrix;
matrix.resize(0,rowSize);
int j=0;
while(in.getline (linebuf, bufferSize)){
int start_s=clock();
std::string line (linebuf);
std::vector < std::string > strs_2;
boost::split (strs_2, line, boost::is_any_of (" "));
matrix.conservativeResize(matrix.rows()+1,rowSize);
for (int i = 0; i < strs_2.size()-1; i++)
matrix (j, i) = static_cast<float> (atof (strs_2[i].c_str ()));
j++;
int stop_s=clock();
std::cout << "time: " << (stop_s-start_s)/double(CLOCKS_PER_SEC)*1000 << std::endl;
}
return matrix;
}
int Conversion::convert(const vpImage<float> & dmap, Eigen::MatrixXf & point3D,
double fx, double fy, double cx, double cy)
{
int height = dmap.getHeight();
int width = dmap.getWidth();
point3D.resize(height*width,3);
int index=0;
for(int i=0 ; i< height ; i++){
for(int j=0 ; j< width ; j++){
float z =dmap[i][j];
if (fabs(z + 1.f) > std::numeric_limits<float>::epsilon() & z>0 ){
point3D(index,2) = z;
point3D(index,0) = (float)((i-cx)*point3D(index,2)/fx);
point3D(index,1) = (float)((j-cy)*point3D(index,2)/fy);
index++;
}
}
}
// resize the point max to remove the points that have been pruned du to negative z value
point3D.conservativeResize(index,3);
return 1;
}
