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();
}
cudaError_t cudaMemcpy3Dfix(const struct cudaMemcpy3DParms *param) {
const cudaMemcpy3DParms& p = *param;
// Use cudaMemcpy3D for 3D only
// But it does not handle 2D or 1D copies well
if (1<p.extent.depth) {
return cudaMemcpy3D( &p );
} else if (1<p.extent.height) {
// 2D copy
// Arraycopy
if (0 != p.srcArray && 0 == p.dstArray) {
return cudaMemcpy2DFromArray(p.dstPtr.ptr, p.dstPtr.pitch, p.srcArray, p.srcPos.x, p.srcPos.y, p.extent.width, p.extent.height, p.kind);
} else if(0 == p.srcArray && 0 != p.dstArray) {
return cudaMemcpy2DToArray(p.dstArray, p.dstPos.x, p.dstPos.y, p.srcPtr.ptr, p.srcPtr.pitch, p.extent.width, p.extent.height, p.kind);
} else if(0 != p.srcArray && 0 != p.dstArray) {
return cudaMemcpy2DArrayToArray( p.dstArray, p.dstPos.x, p.dstPos.y, p.srcArray, p.srcPos.x, p.srcPos.y, p.extent.width, p.extent.height, p.kind);
} else {
return cudaMemcpy2D( p.dstPtr.ptr, p.dstPtr.pitch, p.srcPtr.ptr, p.srcPtr.pitch, p.extent.width, p.extent.height, p.kind );
}
} else {
// 1D copy
// p.extent.width should not include pitch
EXCEPTION_ASSERT( p.extent.width == p.dstPtr.xsize );
EXCEPTION_ASSERT( p.extent.width == p.srcPtr.xsize );
// Arraycopy
if (0 != p.srcArray && 0 == p.dstArray) {
return cudaMemcpyFromArray(p.dstPtr.ptr, p.srcArray, p.srcPos.x, p.srcPos.y, p.extent.width, p.kind);
} else if(0 == p.srcArray && 0 != p.dstArray) {
return cudaMemcpyToArray(p.dstArray, p.dstPos.x, p.dstPos.y, p.srcPtr.ptr, p.extent.width, p.kind);
} else if(0 != p.srcArray && 0 != p.dstArray) {
return cudaMemcpyArrayToArray(p.dstArray, p.dstPos.x, p.dstPos.y, p.srcArray, p.srcPos.x, p.srcPos.y, p.extent.width, p.kind);
} else {
return cudaMemcpy( p.dstPtr.ptr, p.srcPtr.ptr, p.extent.width, p.kind );
}
}
}
void RGBDOdometry::initICP(GPUTexture * predictedVertices, GPUTexture * predictedNormals, const float depthCutoff)
{
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_curr_[0], nmaps_curr_[0]);
for(int i = 1; i < NUM_PYRS; ++i)
{
resizeVMap(vmaps_curr_[i - 1], vmaps_curr_[i]);
resizeNMap(nmaps_curr_[i - 1], nmaps_curr_[i]);
}
cudaDeviceSynchronize();
}
SEXP R_auto_cudaMemcpyFromArray(SEXP r_dst, SEXP r_src, SEXP r_wOffset, SEXP r_hOffset, SEXP r_count, SEXP r_kind)
{
SEXP r_ans = R_NilValue;
void * dst = GET_REF(r_dst, void );
cudaArray_const_t src = (cudaArray_const_t) getRReference(r_src);
size_t wOffset = REAL(r_wOffset)[0];
size_t hOffset = REAL(r_hOffset)[0];
size_t count = REAL(r_count)[0];
enum cudaMemcpyKind kind = (enum cudaMemcpyKind) INTEGER(r_kind)[0];
cudaError_t ans;
ans = cudaMemcpyFromArray(dst, src, wOffset, hOffset, count, kind);
r_ans = Renum_convert_cudaError_t(ans) ;
return(r_ans);
}
cudaError_t WINAPI wine_cudaMemcpyFromArray( void *dst, const struct cudaArray *src, size_t wOffset, size_t hOffset, size_t count, enum cudaMemcpyKind kind ) {
WINE_TRACE("\n");
return cudaMemcpyFromArray( dst, src, wOffset, hOffset, count, kind );
}
