Eigen::Matrix4d getTransformationFromPose(boost::filesystem::path& pose)
{
std::ifstream poseIn(pose.c_str());
if(poseIn.good())
{
double rPosTheta[3];
double rPos[3];
double alignxf[16];
poseIn >> rPos[0] >> rPos[1] >> rPos[2];
poseIn >> rPosTheta[0] >> rPosTheta[1] >> rPosTheta[2];
rPosTheta[0] *= 0.0174533;
rPosTheta[1] *= 0.0174533;
rPosTheta[2] *= 0.0174533;
double sx = sin(rPosTheta[0]);
double cx = cos(rPosTheta[0]);
double sy = sin(rPosTheta[1]);
double cy = cos(rPosTheta[1]);
double sz = sin(rPosTheta[2]);
double cz = cos(rPosTheta[2]);
alignxf[0] = cy*cz;
alignxf[1] = sx*sy*cz + cx*sz;
alignxf[2] = -cx*sy*cz + sx*sz;
alignxf[3] = 0.0;
alignxf[4] = -cy*sz;
alignxf[5] = -sx*sy*sz + cx*cz;
alignxf[6] = cx*sy*sz + sx*cz;
alignxf[7] = 0.0;
alignxf[8] = sy;
alignxf[9] = -sx*cy;
alignxf[10] = cx*cy;
alignxf[11] = 0.0;
alignxf[12] = rPos[0];
alignxf[13] = rPos[1];
alignxf[14] = rPos[2];
alignxf[15] = 1;
return buildTransformation(alignxf);
}
AffineFromProjRectificator::AffineFromProjRectificator(vector<pair<VectorXd, VectorXd>> parallelPairs)
{
buildTransformation(parallelPairs);
}