int
writeMat2File(const Eigen::MatrixXf &matrix, const std::string &fileName)
{
std::ofstream out( fileName.c_str() );
if (out.is_open())
out << matrix.format(CSVFormat);
else
return 0;
out.close();
return 1;
}
ConverterPlaneFromTo3d::ConverterPlaneFromTo3d(float fx, float fy, float cx, float cy, int height, int width)
{
Eigen::VectorXf colIndicies = Eigen::VectorXf::LinSpaced(Eigen::Sequential, width, 1, (float)width);
Eigen::VectorXf rowIndicies = Eigen::VectorXf::LinSpaced(Eigen::Sequential, height, 1, (float)height);
Eigen::VectorXf onesColSize = Eigen::VectorXf::Ones(width, 1);
Eigen::VectorXf onesRowSize = Eigen::VectorXf::Ones(height, 1);
Eigen::MatrixXf indiciesMatrixAxisX = onesRowSize * colIndicies.transpose(); //row = 1, 2, 3, 4, ..
Eigen::MatrixXf indiciesMatrixAxisY = rowIndicies * onesColSize.transpose();
xAdjustment_ = (indiciesMatrixAxisX.array() - cx) / fx;
yAdjustment_ = (indiciesMatrixAxisY.array() - cy) / fy;
Eigen::IOFormat matlabFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n", "", "", "", "");
std::ostringstream saveFileNameString0;
saveFileNameString0 << "indiciesMatrixAxisX.csv";
std::ofstream matrixFile;
matrixFile.open(saveFileNameString0.str());
if (matrixFile.is_open())
{
matrixFile << indiciesMatrixAxisX.format(matlabFormat);
}
std::ostringstream saveFileNameString;
saveFileNameString << "xAdjustment.csv";
std::ofstream matrixFile1;
matrixFile1.open(saveFileNameString.str());
if (matrixFile1.is_open())
{
matrixFile1 << xAdjustment_.format(matlabFormat);
}
}