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(); }