/*! Create a view onto one quadrant of this matrix.
\param ud If 0 then select upper rows, else lower rows
\param lr If 0 then select left columns, else right columns */
mat_t quad(unsigned ud, unsigned lr)
{
return mat_t(
ud ? (rows/2) : (rows-rows/2),
lr ? (cols/2) : (cols-cols/2),
stride,
mat+ud*(stride*rows/2)+lr*(cols/2)
);
}
bool CameraPoseFinderICP::estimateCameraPose(const DepthFrameData& depth_frame,const ColorFrameData& color_frame)
{
if(depth_frame.frameId()==0)
{
return true;
}
//prepare datas
cudaDownSampleNewVertices();
cudaDownSampleNewNormals();
cudaDownSampleModelVertices();
cudaDownSampleModelNormals();
Mat44 cur_transform=_pose;
Mat44 last_transform_inv=_pose.getInverse();
Eigen::Matrix<float, 6, 1, 0, 6, 1> delta_dof;
for(int l=_iter_nums.size()-1;l>=0;l--)
{
int it_nums=_iter_nums[l];
while(it_nums--)
{
// findCorresSet(l,cur_transform,last_transform_inv);
if(false==minimizePointToPlaneErrFunc(l,delta_dof,cur_transform,last_transform_inv)) return false;
Eigen::Matrix4f t = Eigen::Matrix4f::Identity();
if (false==vector6ToTransformMatrix(delta_dof, t) )
{
cout<<"camera shaking detected"<<endl;
return false;
}
//eigen matrix to mat44
Eigen::Matrix4f tt = t.transpose();
Mat44 mat_t(tt.data());
cur_transform = mat_t*cur_transform;
}
}
_pose=cur_transform;
return true;
}
