void ICPSlowdometry::initICPModel(unsigned short * depth,
const float depthCutoff,
const Eigen::Matrix4f & modelPose)
{
depth_tmp[0].upload(depth, sizeof(unsigned short) * width, height, width);
for(int i = 1; i < NUM_PYRS; ++i)
{
pyrDown(depth_tmp[i - 1], depth_tmp[i]);
}
for(int i = 0; i < NUM_PYRS; ++i)
{
createVMap(intr(i), depth_tmp[i], vmaps_g_prev_[i], depthCutoff);
createNMap(vmaps_g_prev_[i], nmaps_g_prev_[i]);
}
cudaDeviceSynchronize();
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcam = modelPose.topLeftCorner(3, 3);
Eigen::Vector3f tcam = modelPose.topRightCorner(3, 1);
Mat33 & device_Rcam = device_cast<Mat33>(Rcam);
float3& device_tcam = device_cast<float3>(tcam);
for(int i = 0; i < NUM_PYRS; ++i)
{
tranformMaps(vmaps_g_prev_[i], nmaps_g_prev_[i], device_Rcam, device_tcam, vmaps_g_prev_[i], nmaps_g_prev_[i]);
}
cudaDeviceSynchronize();
}
void ICPOdometry::initICPModel(const DeviceArray2D<unsigned short>& depth,
const float depthCutoff,
const Eigen::Matrix4f & modelPose)
{
depth_tmp[0] = depth;
for(int i = 1; i < NUM_PYRS; ++i)
{
pyrDown(depth_tmp[i - 1], depth_tmp[i]);
}
for(int i = 0; i < NUM_PYRS; ++i)
{
createVMap(intr(i), depth_tmp[i], vmaps_g_prev_[i], depthCutoff);
createNMap(vmaps_g_prev_[i], nmaps_g_prev_[i]);
}
cudaDeviceSynchronize();
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcam = modelPose.topLeftCorner(3, 3);
Eigen::Vector3f tcam = modelPose.topRightCorner(3, 1);
Mat33 & device_Rcam = device_cast<Mat33>(Rcam);
float3& device_tcam = device_cast<float3>(tcam);
if (modelPose != Eigen::Matrix4f::Identity())
for(int i = 0; i < NUM_PYRS; ++i)
tranformMaps(vmaps_g_prev_[i], nmaps_g_prev_[i], device_Rcam, device_tcam, vmaps_g_prev_[i], nmaps_g_prev_[i]);
cudaDeviceSynchronize();
}
void RGBDOdometry::initICPModel(GPUTexture * predictedVertices,
GPUTexture * predictedNormals,
const float depthCutoff,
const Eigen::Matrix4f & modelPose)
{
cudaArray * textPtr;
cudaGraphicsMapResources(1, &predictedVertices->cudaRes);
cudaGraphicsSubResourceGetMappedArray(&textPtr, predictedVertices->cudaRes, 0, 0);
cudaMemcpyFromArray(vmaps_tmp.ptr(), textPtr, 0, 0, vmaps_tmp.sizeBytes(), cudaMemcpyDeviceToDevice);
cudaGraphicsUnmapResources(1, &predictedVertices->cudaRes);
cudaGraphicsMapResources(1, &predictedNormals->cudaRes);
cudaGraphicsSubResourceGetMappedArray(&textPtr, predictedNormals->cudaRes, 0, 0);
cudaMemcpyFromArray(nmaps_tmp.ptr(), textPtr, 0, 0, nmaps_tmp.sizeBytes(), cudaMemcpyDeviceToDevice);
cudaGraphicsUnmapResources(1, &predictedNormals->cudaRes);
copyMaps(vmaps_tmp, nmaps_tmp, vmaps_g_prev_[0], nmaps_g_prev_[0]);
for(int i = 1; i < NUM_PYRS; ++i)
{
resizeVMap(vmaps_g_prev_[i - 1], vmaps_g_prev_[i]);
resizeNMap(nmaps_g_prev_[i - 1], nmaps_g_prev_[i]);
}
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcam = modelPose.topLeftCorner(3, 3);
Eigen::Vector3f tcam = modelPose.topRightCorner(3, 1);
mat33 device_Rcam = Rcam;
float3 device_tcam = *reinterpret_cast<float3*>(tcam.data());
for(int i = 0; i < NUM_PYRS; ++i)
{
tranformMaps(vmaps_g_prev_[i], nmaps_g_prev_[i], device_Rcam, device_tcam, vmaps_g_prev_[i], nmaps_g_prev_[i]);
}
cudaDeviceSynchronize();
}
