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 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 RGBDOdometry::initICP(GPUTexture * filteredDepth, const float depthCutoff)
{
cudaArray * textPtr;
cudaGraphicsMapResources(1, &filteredDepth->cudaRes);
cudaGraphicsSubResourceGetMappedArray(&textPtr, filteredDepth->cudaRes, 0, 0);
cudaMemcpy2DFromArray(depth_tmp[0].ptr(0), depth_tmp[0].step(), textPtr, 0, 0, depth_tmp[0].colsBytes(), depth_tmp[0].rows(), cudaMemcpyDeviceToDevice);
cudaGraphicsUnmapResources(1, &filteredDepth->cudaRes);
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_curr_[i], depthCutoff);
createNMap(vmaps_curr_[i], nmaps_curr_[i]);
}
cudaDeviceSynchronize();
}
void ICPOdometry::initICP(const DeviceArray2D<unsigned short>& depth, const float depthCutoff)
{
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_curr_[i], depthCutoff);
createNMap(vmaps_curr_[i], nmaps_curr_[i]);
}
cudaDeviceSynchronize();
}
void ICPSlowdometry::initICP(unsigned short * depth, const float depthCutoff)
{
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_curr_[i], depthCutoff);
createNMap(vmaps_curr_[i], nmaps_curr_[i]);
}
cudaDeviceSynchronize();
}