void CCubicGrids::extractRTFromBuffer(const cuda::GpuMat& cvgmSumBuf_, Eigen::Matrix3f* peimRw_, Eigen::Vector3f* peivTw_) const{
Mat cvmSumBuf;
cvgmSumBuf_.download(cvmSumBuf);
double* aHostTmp = (double*)cvmSumBuf.data;
//declare A and b
Eigen::Matrix<double, 6, 6, Eigen::RowMajor> A;
Eigen::Matrix<double, 6, 1> b;
//retrieve A and b from cvmSumBuf
short sShift = 0;
for (int i = 0; i < 6; ++i){ // rows
for (int j = i; j < 7; ++j) { // cols + b
double value = aHostTmp[sShift++];
if (j == 6) // vector b
b.data()[i] = value;
else
A.data()[j * 6 + i] = A.data()[i * 6 + j] = value;
}//for each col
}//for each row
//checking nullspace
double dDet = A.determinant();
if (fabs(dDet) < 1e-15 || dDet != dDet){
if (dDet != dDet)
PRINTSTR("Failure -- dDet cannot be qnan. ");
//reset ();
return;
}//if dDet is rational
//float maxc = A.maxCoeff();
Eigen::Matrix<float, 6, 1> result = A.llt().solve(b).cast<float>();
//Eigen::Matrix<float, 6, 1> result = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
float alpha = result(0);
float beta = result(1);
float gamma = result(2);
Eigen::Matrix3f Rinc = (Eigen::Matrix3f)Eigen::AngleAxisf(gamma, Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(beta, Eigen::Vector3f::UnitY()) * Eigen::AngleAxisf(alpha, Eigen::Vector3f::UnitX());
Eigen::Vector3f tinc = result.tail<3>();
//compose
//eivTwCur = Rinc * eivTwCur + tinc;
//eimrmRwCur = Rinc * eimrmRwCur;
Eigen::Vector3f eivTinv = -peimRw_->transpose()* (*peivTw_);
Eigen::Matrix3f eimRinv = peimRw_->transpose();
eivTinv = Rinc * eivTinv + tinc;
eimRinv = Rinc * eimRinv;
*peivTw_ = -eimRinv.transpose() * eivTinv;
*peimRw_ = eimRinv.transpose();
}
inline
Mat::Mat(const cuda::GpuMat& m)
: flags(0), dims(0), rows(0), cols(0), data(0), datastart(0), dataend(0), datalimit(0), allocator(0), u(0), size(&rows)
{
m.download(*this);
}
