You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
3638 lines
132 KiB
3638 lines
132 KiB
////////////////////////////////////////////////////////////////////////////
|
|
// File: ProgramCU.cu
|
|
// Author: Changchang Wu
|
|
// Description : implementation of ProgramCU and all CUDA kernels
|
|
//
|
|
// Copyright (c) 2011 Changchang Wu (ccwu@cs.washington.edu)
|
|
// and the University of Washington at Seattle
|
|
//
|
|
// This library is free software; you can redistribute it and/or
|
|
// modify it under the terms of the GNU General Public
|
|
// License as published by the Free Software Foundation; either
|
|
// Version 3 of the License, or (at your option) any later version.
|
|
//
|
|
// This library is distributed in the hope that it will be useful,
|
|
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
// General Public License for more details.
|
|
//
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
#include <stdio.h>
|
|
#include <float.h>
|
|
#include "CuTexImage.h"
|
|
#include "ProgramCU.h"
|
|
|
|
#define IMUL(X, Y) __mul24(X, Y)
|
|
#define FDIV(X, Y) __fdividef(X, Y)
|
|
#define FDIV2(X, Y) ((X) / (Y))
|
|
#define MAX_BLOCKLEN 65535
|
|
#define MAX_BLOCKLEN_ALIGN 65504
|
|
#define MAX_TEXSIZE (1 << 29)
|
|
#define TEX_TOOBIG4(sz) (sz >> 31)
|
|
#define REDUCTION_NBLOCK 32
|
|
|
|
namespace pba {
|
|
|
|
inline void CuTexImage::BindTexture(textureReference& texRef) {
|
|
size_t sz = GetDataSize();
|
|
if (sz > MAX_TEXSIZE)
|
|
fprintf(stderr, "cudaBindTexture: %lX > %d\n", sz, MAX_TEXSIZE);
|
|
cudaError_t e =
|
|
cudaBindTexture(NULL, &texRef, data(), &texRef.channelDesc, sz);
|
|
}
|
|
|
|
inline void CuTexImage::BindTexture(textureReference& texRef, int offset,
|
|
size_t size) {
|
|
cudaError_t e = cudaBindTexture(NULL, &texRef, (char*)_cuData + offset,
|
|
&texRef.channelDesc, size);
|
|
if (e) fprintf(stderr, "cudaBindTexture: none-zero offset\n");
|
|
}
|
|
|
|
inline void CuTexImage::BindTexture2(textureReference& texRef1,
|
|
textureReference& texRef2) {
|
|
size_t sz = GetDataSize();
|
|
if (sz <= MAX_TEXSIZE) {
|
|
BindTexture(texRef1);
|
|
} else {
|
|
BindTexture(texRef1, 0, MAX_TEXSIZE);
|
|
BindTexture(texRef2, MAX_TEXSIZE, sz - MAX_TEXSIZE);
|
|
}
|
|
}
|
|
|
|
inline void CuTexImage::BindTexture4(textureReference& texRef1,
|
|
textureReference& texRef2,
|
|
textureReference& texRef3,
|
|
textureReference& texRef4) {
|
|
size_t sz = GetDataSize();
|
|
if (sz <= MAX_TEXSIZE) {
|
|
BindTexture(texRef1);
|
|
} else {
|
|
BindTexture(texRef1, 0, MAX_TEXSIZE);
|
|
if (sz <= 2 * MAX_TEXSIZE) {
|
|
BindTexture(texRef2, MAX_TEXSIZE, sz - MAX_TEXSIZE);
|
|
} else {
|
|
BindTexture(texRef2, MAX_TEXSIZE, MAX_TEXSIZE);
|
|
if (sz <= 3 * MAX_TEXSIZE) {
|
|
BindTexture(texRef3, MAX_TEXSIZE * 2, sz - MAX_TEXSIZE * 2);
|
|
} else {
|
|
BindTexture(texRef3, MAX_TEXSIZE * 2, MAX_TEXSIZE);
|
|
BindTexture(texRef4, MAX_TEXSIZE * 3, sz - MAX_TEXSIZE * 3);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
inline int CuTexImage::BindTextureX(textureReference& texRef1,
|
|
textureReference& texRef2,
|
|
textureReference& texRef3,
|
|
textureReference& texRef4, bool force4) {
|
|
size_t szjc = GetDataSize();
|
|
if (TEX_TOOBIG4(szjc)) {
|
|
return 0;
|
|
} else if (force4) {
|
|
BindTexture4(texRef1, texRef2, texRef3, texRef4);
|
|
return 4;
|
|
} else if (szjc > 2 * MAX_TEXSIZE) {
|
|
return 0;
|
|
} else if (szjc > MAX_TEXSIZE) {
|
|
BindTexture2(texRef1, texRef2);
|
|
return 2;
|
|
} else {
|
|
BindTexture(texRef1);
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
void ProgramCU::FinishWorkCUDA() { cudaThreadSynchronize(); }
|
|
|
|
int ProgramCU::CheckErrorCUDA(const char* location) {
|
|
cudaError_t e = cudaGetLastError();
|
|
if (e) {
|
|
if (location) fprintf(stderr, "%s:\t", location);
|
|
fprintf(stderr, "%s(%d)\n", cudaGetErrorString(e), e);
|
|
throw location;
|
|
} else {
|
|
// fprintf(stderr, "%s:\n", location);
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
inline void ProgramCU::GetBlockConfiguration(unsigned int nblock,
|
|
unsigned int& bw,
|
|
unsigned int& bh) {
|
|
if (nblock <= MAX_BLOCKLEN) {
|
|
bw = nblock;
|
|
bh = 1;
|
|
} else {
|
|
bh = (nblock + MAX_BLOCKLEN_ALIGN - 1) / MAX_BLOCKLEN_ALIGN;
|
|
bw = (nblock + bh - 1) / bh;
|
|
bw = ((bw + 31) / 32) * 32;
|
|
bh = (nblock + bw - 1) / bw;
|
|
}
|
|
}
|
|
|
|
void ProgramCU::ClearPreviousError() { cudaGetLastError(); }
|
|
|
|
void ProgramCU::ResetCurrentDevice() {
|
|
int device = 0;
|
|
cudaGetDevice(&device);
|
|
cudaDeviceReset();
|
|
if (device > 0) cudaSetDevice(device);
|
|
}
|
|
|
|
size_t ProgramCU::GetCudaMemoryCap() {
|
|
int device;
|
|
if (cudaGetDevice(&device) != cudaSuccess) return 0;
|
|
cudaDeviceProp prop;
|
|
if (cudaGetDeviceProperties(&prop, device) == cudaSuccess) {
|
|
if (prop.major == 9999 && prop.minor == 9999) return 0;
|
|
return prop.totalGlobalMem;
|
|
} else
|
|
return 0;
|
|
}
|
|
int ProgramCU::SetCudaDevice(int device) {
|
|
int count = 0, device_used;
|
|
if (cudaGetDeviceCount(&count) || count <= 0) {
|
|
ProgramCU::CheckErrorCUDA("CheckCudaDevice");
|
|
return 0;
|
|
} else if (count == 1) {
|
|
cudaDeviceProp deviceProp;
|
|
if (cudaGetDeviceProperties(&deviceProp, 0) != cudaSuccess) {
|
|
fprintf(stderr, "CheckCudaDevice: no device supporting CUDA.\n");
|
|
return 0;
|
|
}
|
|
if (deviceProp.major == 9999 && deviceProp.minor == 9999) {
|
|
fprintf(stderr, "CheckCudaDevice: no device supporting CUDA.\n");
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
if (device > 0 && device < count) {
|
|
cudaSetDevice(device);
|
|
CheckErrorCUDA("cudaSetDevice\n");
|
|
}
|
|
cudaGetDevice(&device_used);
|
|
if (device != device_used)
|
|
fprintf(stderr,
|
|
"ERROR: Cannot set device to %d\n"
|
|
"WARNING: Use device-%d instead (out of %d)\n",
|
|
device, device_used, count);
|
|
return 1;
|
|
}
|
|
|
|
#define WARP_REDUCTION_32(value) \
|
|
__syncthreads(); \
|
|
if (threadIdx.x < 16) value[threadIdx.x] += value[threadIdx.x + 16]; \
|
|
if (threadIdx.x < 8) value[threadIdx.x] += value[threadIdx.x + 8]; \
|
|
if (threadIdx.x < 4) value[threadIdx.x] += value[threadIdx.x + 4]; \
|
|
if (threadIdx.x < 2) value[threadIdx.x] += value[threadIdx.x + 2];
|
|
|
|
#define WARP_REDUCTION_64(value) \
|
|
__syncthreads(); \
|
|
if (threadIdx.x < 32) value[threadIdx.x] += value[threadIdx.x + 32]; \
|
|
WARP_REDUCTION_32(value)
|
|
|
|
#define WARP_REDUCTION_128(value) \
|
|
__syncthreads(); \
|
|
if (threadIdx.x < 64) value[threadIdx.x] += value[threadIdx.x + 64]; \
|
|
WARP_REDUCTION_64(value)
|
|
|
|
#define WARP_REDUCTION_256(value) \
|
|
__syncthreads(); \
|
|
if (threadIdx.x < 128) value[threadIdx.x] += value[threadIdx.x + 128]; \
|
|
WARP_REDUCTION_128(value)
|
|
|
|
__global__ void vector_max_kernel(const float* x, int len, int blen,
|
|
float* result) {
|
|
__shared__ float value[256];
|
|
int bstart = blen * blockIdx.x;
|
|
int start = bstart + threadIdx.x;
|
|
int end = min(len, bstart + blen);
|
|
|
|
float v = 0;
|
|
for (int i = start; i < end; i += blockDim.x) v = max(v, fabs(x[i]));
|
|
value[threadIdx.x] = v;
|
|
// reduce to the first two values
|
|
__syncthreads();
|
|
if (threadIdx.x < 128)
|
|
value[threadIdx.x] = max(value[threadIdx.x], value[threadIdx.x + 128]);
|
|
__syncthreads();
|
|
if (threadIdx.x < 64)
|
|
value[threadIdx.x] = max(value[threadIdx.x], value[threadIdx.x + 64]);
|
|
__syncthreads();
|
|
if (threadIdx.x < 32)
|
|
value[threadIdx.x] = max(value[threadIdx.x], value[threadIdx.x + 32]);
|
|
if (threadIdx.x < 16)
|
|
value[threadIdx.x] = max(value[threadIdx.x], value[threadIdx.x + 16]);
|
|
if (threadIdx.x < 8)
|
|
value[threadIdx.x] = max(value[threadIdx.x], value[threadIdx.x + 8]);
|
|
if (threadIdx.x < 4)
|
|
value[threadIdx.x] = max(value[threadIdx.x], value[threadIdx.x + 4]);
|
|
if (threadIdx.x < 2)
|
|
value[threadIdx.x] = max(value[threadIdx.x], value[threadIdx.x + 2]);
|
|
// write back
|
|
if (threadIdx.x == 0) result[blockIdx.x] = max(value[0], value[1]);
|
|
}
|
|
|
|
float ProgramCU::ComputeVectorMax(CuTexImage& vector, CuTexImage& buf) {
|
|
const unsigned int nblock = 32;
|
|
const unsigned int bsize = 256;
|
|
int len = vector.GetLength();
|
|
int blen = ((len + nblock - 1) / nblock + bsize - 1) / bsize * bsize;
|
|
|
|
////////////////////////////////
|
|
dim3 grid(nblock), block(bsize);
|
|
|
|
/////////////////////////////////
|
|
buf.InitTexture(nblock, 1);
|
|
vector_max_kernel<<<grid, block>>>(vector.data(), len, blen, buf.data());
|
|
ProgramCU::CheckErrorCUDA("ComputeVectorMax");
|
|
|
|
float data[nblock], result = 0;
|
|
buf.CopyToHost(data);
|
|
for (unsigned int i = 0; i < nblock; ++i) result = max(result, data[i]);
|
|
return result;
|
|
}
|
|
|
|
__global__ void vector_norm_kernel(const float* x, int len, int blen,
|
|
float* result) {
|
|
__shared__ float value[256];
|
|
int bstart = blen * blockIdx.x;
|
|
int start = bstart + threadIdx.x;
|
|
int end = min(len, bstart + blen);
|
|
|
|
float v = 0;
|
|
for (int i = start; i < end; i += blockDim.x) {
|
|
float temp = x[i];
|
|
v += (temp * temp);
|
|
}
|
|
value[threadIdx.x] = v;
|
|
// reduce to the first two values
|
|
WARP_REDUCTION_256(value);
|
|
|
|
// write back
|
|
if (threadIdx.x == 0) result[blockIdx.x] = (value[0] + value[1]);
|
|
}
|
|
|
|
double ProgramCU::ComputeVectorNorm(CuTexImage& vector, CuTexImage& buf) {
|
|
const unsigned int nblock = REDUCTION_NBLOCK;
|
|
unsigned int bsize = 256;
|
|
int len = vector.GetLength();
|
|
int blen = ((len + nblock - 1) / nblock + bsize - 1) / bsize * bsize;
|
|
|
|
////////////////////////////////
|
|
dim3 grid(nblock), block(bsize);
|
|
|
|
/////////////////////////////////
|
|
buf.InitTexture(nblock, 1);
|
|
vector_norm_kernel<<<grid, block>>>(vector.data(), len, blen, buf.data());
|
|
ProgramCU::CheckErrorCUDA("ComputeVectorNorm");
|
|
|
|
float data[nblock];
|
|
buf.CopyToHost(data);
|
|
double result = 0;
|
|
for (unsigned int i = 0; i < nblock; ++i) result += data[i];
|
|
return result;
|
|
}
|
|
|
|
__global__ void vector_sum_kernel(const float* x, int len, int blen,
|
|
float* result) {
|
|
__shared__ float value[256];
|
|
int bstart = blen * blockIdx.x;
|
|
int start = bstart + threadIdx.x;
|
|
int end = min(len, bstart + blen);
|
|
float v = 0;
|
|
for (int i = start; i < end; i += blockDim.x) v += x[i];
|
|
|
|
value[threadIdx.x] = v;
|
|
// reduce to the first two values
|
|
WARP_REDUCTION_256(value);
|
|
|
|
// write back
|
|
if (threadIdx.x == 0) result[blockIdx.x] = (value[0] + value[1]);
|
|
}
|
|
|
|
float ProgramCU::ComputeVectorSum(CuTexImage& vector, CuTexImage& buf,
|
|
int skip) {
|
|
const unsigned int nblock = REDUCTION_NBLOCK;
|
|
unsigned int bsize = 256;
|
|
int len = vector.GetLength() - skip;
|
|
int blen = ((len + nblock - 1) / nblock + bsize - 1) / bsize * bsize;
|
|
|
|
////////////////////////////////
|
|
dim3 grid(nblock), block(bsize);
|
|
|
|
/////////////////////////////////
|
|
buf.InitTexture(nblock, 1);
|
|
vector_sum_kernel<<<grid, block>>>((vector.data()) + skip, len, blen,
|
|
buf.data());
|
|
ProgramCU::CheckErrorCUDA("ComputeVectorSum");
|
|
|
|
float data[nblock];
|
|
buf.CopyToHost(data);
|
|
double result = 0;
|
|
for (unsigned int i = 0; i < nblock; ++i) result += data[i];
|
|
return (float)result;
|
|
}
|
|
|
|
__global__ void vector_dotproduct_kernel(const float* a, const float* b,
|
|
int len, int blen, float* result) {
|
|
__shared__ float value[256];
|
|
int bstart = blen * blockIdx.x;
|
|
int start = bstart + threadIdx.x;
|
|
int end = min(len, bstart + blen);
|
|
|
|
float v = 0;
|
|
for (int i = start; i < end; i += blockDim.x) v += (a[i] * b[i]);
|
|
value[threadIdx.x] = v;
|
|
|
|
// reduce to the first two values
|
|
WARP_REDUCTION_256(value);
|
|
|
|
// write back
|
|
if (threadIdx.x == 0) result[blockIdx.x] = (value[0] + value[1]);
|
|
}
|
|
|
|
double ProgramCU::ComputeVectorDot(CuTexImage& vector1, CuTexImage& vector2,
|
|
CuTexImage& buf) {
|
|
const unsigned int nblock = REDUCTION_NBLOCK;
|
|
unsigned int bsize = 256;
|
|
int len = vector1.GetLength();
|
|
int blen = ((len + nblock - 1) / nblock + bsize - 1) / bsize * bsize;
|
|
|
|
////////////////////////////////
|
|
dim3 grid(nblock), block(bsize);
|
|
|
|
/////////////////////////////////
|
|
buf.InitTexture(nblock, 1);
|
|
vector_dotproduct_kernel<<<grid, block>>>(vector1.data(), vector2.data(), len,
|
|
blen, buf.data());
|
|
ProgramCU::CheckErrorCUDA("ComputeVectorDot");
|
|
|
|
float data[nblock];
|
|
buf.CopyToHost(data);
|
|
|
|
double result = 0;
|
|
for (unsigned int i = 0; i < nblock; ++i) result += data[i];
|
|
return result;
|
|
}
|
|
|
|
__global__ void vector_weighted_norm_kernel(const float* vec, const float* w,
|
|
int len, int blen, float* result) {
|
|
__shared__ float value[256];
|
|
int bstart = blen * blockIdx.x;
|
|
int start = bstart + threadIdx.x;
|
|
int end = min(len, bstart + blen);
|
|
|
|
float v = 0;
|
|
for (int i = start; i < end; i += blockDim.x) v += (vec[i] * w[i] * vec[i]);
|
|
value[threadIdx.x] = v;
|
|
|
|
// reduce to the first two values
|
|
WARP_REDUCTION_256(value);
|
|
|
|
// write back
|
|
if (threadIdx.x == 0) result[blockIdx.x] = (value[0] + value[1]);
|
|
}
|
|
|
|
double ProgramCU::ComputeVectorNormW(CuTexImage& vector, CuTexImage& weight,
|
|
CuTexImage& buf) {
|
|
if (weight.IsValid()) {
|
|
const unsigned int nblock = REDUCTION_NBLOCK;
|
|
unsigned int bsize = 256;
|
|
int len = vector.GetLength();
|
|
int blen = ((len + nblock - 1) / nblock + bsize - 1) / bsize * bsize;
|
|
|
|
////////////////////////////////
|
|
dim3 grid(nblock), block(bsize);
|
|
|
|
/////////////////////////////////
|
|
buf.InitTexture(nblock, 1);
|
|
|
|
vector_weighted_norm_kernel<<<grid, block>>>(vector.data(), weight.data(),
|
|
len, blen, buf.data());
|
|
|
|
ProgramCU::CheckErrorCUDA("ComputeVectorNormW");
|
|
|
|
float data[nblock];
|
|
buf.CopyToHost(data);
|
|
|
|
double result = 0;
|
|
for (unsigned int i = 0; i < nblock; ++i) result += data[i];
|
|
return result;
|
|
} else {
|
|
return ComputeVectorNorm(vector, buf);
|
|
}
|
|
}
|
|
// given vector x, y, and a weight a
|
|
// return a * x + y
|
|
__global__ void saxpy_kernel(const float a, const float* x, const float* y,
|
|
float* result, unsigned int len) {
|
|
unsigned int idx = blockIdx.x * blockDim.x + threadIdx.x;
|
|
if (idx < len) result[idx] = a * x[idx] + y[idx];
|
|
}
|
|
|
|
__global__ void saxpy_kernel_large(const float a, const float* x,
|
|
const float* y, float* result,
|
|
unsigned int len, unsigned int rowsz) {
|
|
unsigned int idx = threadIdx.x + blockIdx.x * blockDim.x + blockIdx.y * rowsz;
|
|
if (idx < len) result[idx] = a * x[idx] + y[idx];
|
|
}
|
|
|
|
void ProgramCU::ComputeSAXPY(float a, CuTexImage& texX, CuTexImage& texY,
|
|
CuTexImage& result) {
|
|
unsigned int len = result.GetLength();
|
|
unsigned int bsize = 128;
|
|
unsigned int nblock = (len + bsize - 1) / bsize;
|
|
if (nblock > MAX_BLOCKLEN) {
|
|
unsigned int bw, bh;
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
saxpy_kernel_large<<<grid, block>>>(a, texX.data(), texY.data(),
|
|
result.data(), len, bw * bsize);
|
|
} else {
|
|
dim3 grid(nblock), block(bsize);
|
|
saxpy_kernel<<<grid, block>>>(a, texX.data(), texY.data(), result.data(),
|
|
len);
|
|
}
|
|
ProgramCU::CheckErrorCUDA("ComputeSAXPY");
|
|
}
|
|
|
|
__global__ void sxypz_kernel_large(float a, const float* x, const float* y,
|
|
const float* z, float* result,
|
|
unsigned int len, unsigned int rowsz) {
|
|
unsigned int idx = threadIdx.x + blockIdx.x * blockDim.x + blockIdx.y * rowsz;
|
|
if (idx < len) result[idx] = a * x[idx] * y[idx] + z[idx];
|
|
}
|
|
|
|
void ProgramCU::ComputeSXYPZ(float a, CuTexImage& texX, CuTexImage& texY,
|
|
CuTexImage& texZ, CuTexImage& result) {
|
|
if (texX.IsValid()) {
|
|
unsigned int len = texX.GetLength();
|
|
unsigned int bsize = 128;
|
|
unsigned int nblock = (len + bsize - 1) / bsize;
|
|
unsigned int bw, bh;
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
sxypz_kernel_large<<<grid, block>>>(a, texX.data(), texY.data(),
|
|
texZ.data(), result.data(), len,
|
|
bw * bsize);
|
|
} else {
|
|
ComputeSAXPY(a, texY, texZ, result);
|
|
}
|
|
}
|
|
|
|
__global__ void vxy_kernel(const float* x, float* y, float* result,
|
|
unsigned int len) {
|
|
unsigned int idx = blockIdx.x * blockDim.x + threadIdx.x;
|
|
if (idx < len) result[idx] = x[idx] * y[idx];
|
|
}
|
|
|
|
__global__ void vxy_kernel_large(const float* x, float* y, float* result,
|
|
unsigned int len, unsigned int rowsz) {
|
|
unsigned int idx = blockIdx.x * blockDim.x + threadIdx.x + rowsz * blockIdx.y;
|
|
if (idx < len) result[idx] = x[idx] * y[idx];
|
|
}
|
|
|
|
void ProgramCU::ComputeVXY(CuTexImage& texX, CuTexImage& texY,
|
|
CuTexImage& result, unsigned int part,
|
|
unsigned int skip) {
|
|
unsigned int len = part ? part : texX.GetLength();
|
|
unsigned int bsize = 128;
|
|
unsigned int nblock = (len + bsize - 1) / bsize;
|
|
if (nblock > MAX_BLOCKLEN) {
|
|
unsigned int bw, bh;
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
vxy_kernel_large<<<grid, block>>>(texX.data() + skip, texY.data() + skip,
|
|
result.data() + skip, len, bsize * bw);
|
|
} else {
|
|
dim3 grid(nblock), block(bsize);
|
|
vxy_kernel<<<grid, block>>>(texX.data() + skip, texY.data() + skip,
|
|
result.data() + skip, len);
|
|
}
|
|
ProgramCU::CheckErrorCUDA("ComputeVXY");
|
|
}
|
|
|
|
__global__ void sqrt_kernel_large(float* x, unsigned int len,
|
|
unsigned int rowsz) {
|
|
unsigned int idx = threadIdx.x + blockIdx.x * blockDim.x + blockIdx.y * rowsz;
|
|
if (idx < len) x[idx] = sqrt(x[idx]);
|
|
}
|
|
|
|
void ProgramCU::ComputeSQRT(CuTexImage& tex) {
|
|
unsigned int len = tex.GetLength();
|
|
unsigned int bsize = 128;
|
|
unsigned int nblock = (len + bsize - 1) / bsize;
|
|
unsigned int bw, bh;
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
sqrt_kernel_large<<<grid, block>>>(tex.data(), len, bw * bsize);
|
|
ProgramCU::CheckErrorCUDA("ComputeSQRT");
|
|
}
|
|
|
|
__global__ void rsqrt_kernel_large(float* x, unsigned int len,
|
|
unsigned int rowsz) {
|
|
unsigned int idx = threadIdx.x + blockIdx.x * blockDim.x + blockIdx.y * rowsz;
|
|
if (idx < len) x[idx] = x[idx] > 0 ? rsqrt(x[idx]) : 0;
|
|
}
|
|
|
|
void ProgramCU::ComputeRSQRT(CuTexImage& tex) {
|
|
unsigned int len = tex.GetLength();
|
|
unsigned int bsize = 128;
|
|
unsigned int nblock = (len + bsize - 1) / bsize;
|
|
unsigned int bw, bh;
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
rsqrt_kernel_large<<<grid, block>>>(tex.data(), len, bw * bsize);
|
|
|
|
ProgramCU::CheckErrorCUDA("ComputeRSQRT");
|
|
}
|
|
|
|
__global__ void sax_kernel(const float a, const float* x, float* result,
|
|
unsigned int len) {
|
|
unsigned int idx = blockIdx.x * blockDim.x + threadIdx.x;
|
|
if (idx < len) result[idx] = a * x[idx];
|
|
}
|
|
|
|
__global__ void sax_kernel_large(const float a, const float* x, float* result,
|
|
unsigned int len, unsigned int rowsz) {
|
|
unsigned int idx = blockIdx.x * blockDim.x + threadIdx.x + blockIdx.y * rowsz;
|
|
if (idx < len) result[idx] = a * x[idx];
|
|
}
|
|
|
|
void ProgramCU::ComputeSAX(float a, CuTexImage& texX, CuTexImage& result) {
|
|
unsigned int len = texX.GetLength();
|
|
unsigned int bsize = 128;
|
|
unsigned int nblock = (len + bsize - 1) / bsize;
|
|
|
|
if (nblock > MAX_BLOCKLEN) {
|
|
unsigned int bw, bh;
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
sax_kernel_large<<<grid, block>>>(a, texX.data(), result.data(), len,
|
|
bw * bsize);
|
|
} else {
|
|
dim3 grid(nblock), block(bsize);
|
|
sax_kernel<<<grid, block>>>(a, texX.data(), result.data(), len);
|
|
}
|
|
ProgramCU::CheckErrorCUDA("ComputeSAX");
|
|
}
|
|
|
|
#define JACOBIAN_FRT_KWIDTH 64
|
|
|
|
texture<float4, 1, cudaReadModeElementType> tex_jacobian_cam;
|
|
texture<float4, 1, cudaReadModeElementType> tex_jacobian_pts;
|
|
texture<int2, 1, cudaReadModeElementType> tex_jacobian_idx;
|
|
texture<float2, 1, cudaReadModeElementType> tex_jacobian_meas;
|
|
texture<float4, 1, cudaReadModeElementType> tex_jacobian_sj;
|
|
texture<int, 1, cudaReadModeElementType> tex_jacobian_shuffle;
|
|
|
|
#ifndef PBA_DISABLE_CONST_CAMERA
|
|
#define JACOBIAN_SET_JC_BEGIN if (r3.w == 0.0f) {
|
|
#define JFRT_SET_JC_END \
|
|
} \
|
|
else { \
|
|
jc[jc_pos] = make_float4(0, 0, 0, 0); \
|
|
jc[jc_pos + 1] = make_float4(0, 0, 0, 0); \
|
|
jc[jc_pos + 2] = make_float4(0, 0, 0, 0); \
|
|
jc[jc_pos + 3] = make_float4(0, 0, 0, 0); \
|
|
}
|
|
#define JACOBIAN_SET_JC_END \
|
|
} \
|
|
else { \
|
|
jxc[0] = 0; \
|
|
jxc[1] = 0; \
|
|
jxc[2] = 0; \
|
|
jxc[3] = 0; \
|
|
jxc[4] = 0; \
|
|
jxc[5] = 0; \
|
|
jxc[6] = 0; \
|
|
jxc[7] = 0; \
|
|
jyc[0] = 0; \
|
|
jyc[1] = 0; \
|
|
jyc[2] = 0; \
|
|
jyc[3] = 0; \
|
|
jyc[4] = 0; \
|
|
jyc[5] = 0; \
|
|
jyc[6] = 0; \
|
|
jyc[7] = 0; \
|
|
}
|
|
#else
|
|
#define JACOBIAN_SET_JC_BEGIN
|
|
#define JFRT_SET_JC_END
|
|
#define JACOBIAN_SET_JC_END
|
|
#endif
|
|
|
|
// projection model ei = K(RX + T) - (1 + r * m^2) * m
|
|
template <bool md, bool pd, bool scaling, bool shuffle>
|
|
__global__ void jacobian_frt_kernel(float4* jc, float4* jp, int nproj, int ptx,
|
|
int rowsz, float jic) {
|
|
////////////////////////////////
|
|
int tidx = blockIdx.x * blockDim.x + threadIdx.x + blockIdx.y * rowsz;
|
|
|
|
if (tidx >= nproj) return;
|
|
int2 proj = tex1Dfetch(tex_jacobian_idx, tidx);
|
|
int camera_pos = proj.x << 1;
|
|
|
|
__shared__ float rr_data[JACOBIAN_FRT_KWIDTH * 9];
|
|
float* r = rr_data + IMUL(9, threadIdx.x);
|
|
float4 ft = tex1Dfetch(tex_jacobian_cam, camera_pos);
|
|
float4 r1 = tex1Dfetch(tex_jacobian_cam, camera_pos + 1);
|
|
r[0] = r1.x;
|
|
r[1] = r1.y;
|
|
r[2] = r1.z;
|
|
r[3] = r1.w;
|
|
float4 r2 = tex1Dfetch(tex_jacobian_cam, camera_pos + 2);
|
|
r[4] = r2.x;
|
|
r[5] = r2.y;
|
|
r[6] = r2.z;
|
|
r[7] = r2.w;
|
|
float4 r3 = tex1Dfetch(tex_jacobian_cam, camera_pos + 3);
|
|
r[8] = r3.x;
|
|
|
|
float4 temp = tex1Dfetch(tex_jacobian_pts, proj.y);
|
|
float m[3];
|
|
m[0] = temp.x;
|
|
m[1] = temp.y;
|
|
m[2] = temp.z;
|
|
|
|
float x0 = r[0] * m[0] + r[1] * m[1] + r[2] * m[2];
|
|
float y0 = r[3] * m[0] + r[4] * m[1] + r[5] * m[2];
|
|
float z0 = r[6] * m[0] + r[7] * m[1] + r[8] * m[2];
|
|
float f_p2 = FDIV(ft.x, z0 + ft.w);
|
|
float p0_p2 = FDIV(x0 + ft.y, z0 + ft.w);
|
|
float p1_p2 = FDIV(y0 + ft.z, z0 + ft.w);
|
|
|
|
// dp/dx = [f/p2 0 -f*p0/p2/p2]
|
|
// [0 f/p2 -f*p1/p2/p2]
|
|
// dx/dw = [ 0 z -y]
|
|
// [-z 0 x]
|
|
// [ y -x 0]
|
|
// R(dw) (x y z)' = (0 -z y)' dw0 + (z 0 -x)'dw1 + (-y x 0)'dw2
|
|
int jc_pos;
|
|
if (shuffle) {
|
|
jc_pos = tex1Dfetch(tex_jacobian_shuffle, tidx) << 2;
|
|
} else {
|
|
jc_pos = tidx << 2;
|
|
}
|
|
|
|
if (pd) {
|
|
float rr1 = r3.y * p0_p2 * p0_p2;
|
|
float rr2 = r3.y * p1_p2 * p1_p2;
|
|
float f_p2_x = f_p2 * (1.0 + 3.0 * rr1 + rr2);
|
|
float f_p2_y = f_p2 * (1.0 + 3.0 * rr2 + rr1);
|
|
if (scaling == false) {
|
|
if (jc) {
|
|
JACOBIAN_SET_JC_BEGIN
|
|
// float jic = (r3.w != 1.0f && r3.w != 2.0f) ? 1.0f : 0.0f;
|
|
// float jec = (r3.w != 1.0f && r3.w != 3.0f) ? 1.0f : 0.0f;
|
|
float jfc = jic * (1 + rr1 + rr2);
|
|
float ft_x_pn = jic * ft.x * (p0_p2 * p0_p2 + p1_p2 * p1_p2);
|
|
jc[jc_pos] = make_float4(p0_p2 * jfc, f_p2_x, 0, -f_p2_x * p0_p2);
|
|
jc[jc_pos + 1] =
|
|
make_float4(-f_p2_x * p0_p2 * y0, f_p2_x * (z0 + x0 * p0_p2),
|
|
-f_p2_x * y0, ft_x_pn * p0_p2);
|
|
jc[jc_pos + 2] = make_float4(p1_p2 * jfc, 0, f_p2_y, -f_p2 * p1_p2);
|
|
jc[jc_pos + 3] =
|
|
make_float4(-f_p2_y * (z0 + y0 * p1_p2), f_p2_y * x0 * p1_p2,
|
|
f_p2_y * x0, ft_x_pn * p1_p2);
|
|
JFRT_SET_JC_END
|
|
}
|
|
////////////////////
|
|
jp[(tidx << 1)] = make_float4(f_p2_x * (r[0] - r[6] * p0_p2),
|
|
f_p2_x * (r[1] - r[7] * p0_p2),
|
|
f_p2_x * (r[2] - r[8] * p0_p2), 0);
|
|
jp[(tidx << 1) + 1] = make_float4(f_p2_y * (r[3] - r[6] * p1_p2),
|
|
f_p2_y * (r[4] - r[7] * p1_p2),
|
|
f_p2_y * (r[5] - r[8] * p1_p2), 0);
|
|
} else {
|
|
////////////////////
|
|
if (jc) {
|
|
JACOBIAN_SET_JC_BEGIN
|
|
float jfc = jic * (1 + rr1 + rr2);
|
|
float ft_x_pn = jic * ft.x * (p0_p2 * p0_p2 + p1_p2 * p1_p2);
|
|
float4 sc1 = tex1Dfetch(tex_jacobian_sj, proj.x);
|
|
jc[jc_pos] = make_float4(p0_p2 * jfc * sc1.x, f_p2_x * sc1.y, 0,
|
|
-f_p2_x * p0_p2 * sc1.w);
|
|
jc[jc_pos + 2] = make_float4(p1_p2 * jfc * sc1.x, 0, f_p2_y * sc1.z,
|
|
-f_p2_y * p1_p2 * sc1.w);
|
|
|
|
float4 sc2 = tex1Dfetch(tex_jacobian_sj, proj.x + 1);
|
|
jc[jc_pos + 1] = make_float4(
|
|
-sc2.x * f_p2_x * p0_p2 * y0, sc2.y * f_p2_x * (z0 + x0 * p0_p2),
|
|
-sc2.z * f_p2_x * y0, ft_x_pn * p0_p2 * sc2.w);
|
|
jc[jc_pos + 3] = make_float4(
|
|
-sc2.x * f_p2_y * (z0 + y0 * p1_p2), sc2.y * f_p2_y * x0 * p1_p2,
|
|
sc2.z * f_p2_y * x0, ft_x_pn * p1_p2 * sc2.w);
|
|
JFRT_SET_JC_END
|
|
}
|
|
|
|
float4 sc3 = tex1Dfetch(tex_jacobian_sj, proj.y + ptx);
|
|
jp[(tidx << 1)] = make_float4(sc3.x * f_p2_x * (r[0] - r[6] * p0_p2),
|
|
sc3.y * f_p2_x * (r[1] - r[7] * p0_p2),
|
|
sc3.z * f_p2_x * (r[2] - r[8] * p0_p2), 0);
|
|
jp[(tidx << 1) + 1] =
|
|
make_float4(sc3.x * f_p2_y * (r[3] - r[6] * p1_p2),
|
|
sc3.y * f_p2_y * (r[4] - r[7] * p1_p2),
|
|
sc3.z * f_p2_y * (r[5] - r[8] * p1_p2), 0);
|
|
}
|
|
} else if (md) {
|
|
if (scaling == false) {
|
|
if (jc) {
|
|
JACOBIAN_SET_JC_BEGIN
|
|
float2 ms = tex1Dfetch(tex_jacobian_meas, tidx);
|
|
float msn = (ms.x * ms.x + ms.y * ms.y) * jic;
|
|
jc[jc_pos] = make_float4(p0_p2 * jic, f_p2, 0, -f_p2 * p0_p2);
|
|
jc[jc_pos + 1] =
|
|
make_float4(-f_p2 * p0_p2 * y0, f_p2 * (z0 + x0 * p0_p2),
|
|
-f_p2 * y0, -ms.x * msn);
|
|
jc[jc_pos + 2] = make_float4(p1_p2 * jic, 0, f_p2, -f_p2 * p1_p2);
|
|
jc[jc_pos + 3] = make_float4(-f_p2 * (z0 + y0 * p1_p2),
|
|
f_p2 * x0 * p1_p2, f_p2 * x0, -ms.y * msn);
|
|
JFRT_SET_JC_END
|
|
}
|
|
////////////////////
|
|
jp[(tidx << 1)] = make_float4(f_p2 * (r[0] - r[6] * p0_p2),
|
|
f_p2 * (r[1] - r[7] * p0_p2),
|
|
f_p2 * (r[2] - r[8] * p0_p2), 0);
|
|
jp[(tidx << 1) + 1] = make_float4(f_p2 * (r[3] - r[6] * p1_p2),
|
|
f_p2 * (r[4] - r[7] * p1_p2),
|
|
f_p2 * (r[5] - r[8] * p1_p2), 0);
|
|
} else {
|
|
if (jc) {
|
|
JACOBIAN_SET_JC_BEGIN
|
|
float4 sc1 = tex1Dfetch(tex_jacobian_sj, proj.x);
|
|
jc[jc_pos] = make_float4(p0_p2 * jic * sc1.x, f_p2 * sc1.y, 0,
|
|
-f_p2 * p0_p2 * sc1.w);
|
|
jc[jc_pos + 2] = make_float4(p1_p2 * jic * sc1.x, 0, f_p2 * sc1.z,
|
|
-f_p2 * p1_p2 * sc1.w);
|
|
|
|
float4 sc2 = tex1Dfetch(tex_jacobian_sj, proj.x + 1);
|
|
float2 ms = tex1Dfetch(tex_jacobian_meas, tidx);
|
|
float msn = (ms.x * ms.x + ms.y * ms.y) * jic;
|
|
jc[jc_pos + 1] = make_float4(-sc2.x * f_p2 * p0_p2 * y0,
|
|
sc2.y * f_p2 * (z0 + x0 * p0_p2),
|
|
-sc2.z * f_p2 * y0, -msn * ms.x * sc2.w);
|
|
jc[jc_pos + 3] = make_float4(-sc2.x * f_p2 * (z0 + y0 * p1_p2),
|
|
sc2.y * f_p2 * x0 * p1_p2,
|
|
sc2.z * f_p2 * x0, -msn * ms.y * sc2.w);
|
|
JFRT_SET_JC_END
|
|
}
|
|
float4 sc3 = tex1Dfetch(tex_jacobian_sj, proj.y + ptx);
|
|
jp[(tidx << 1)] = make_float4(sc3.x * f_p2 * (r[0] - r[6] * p0_p2),
|
|
sc3.y * f_p2 * (r[1] - r[7] * p0_p2),
|
|
sc3.z * f_p2 * (r[2] - r[8] * p0_p2), 0);
|
|
jp[(tidx << 1) + 1] =
|
|
make_float4(sc3.x * f_p2 * (r[3] - r[6] * p1_p2),
|
|
sc3.y * f_p2 * (r[4] - r[7] * p1_p2),
|
|
sc3.z * f_p2 * (r[5] - r[8] * p1_p2), 0);
|
|
}
|
|
|
|
} else {
|
|
if (scaling == false) {
|
|
if (jc) {
|
|
JACOBIAN_SET_JC_BEGIN
|
|
jc[jc_pos] = make_float4(p0_p2 * jic, f_p2, 0, -f_p2 * p0_p2);
|
|
jc[jc_pos + 1] = make_float4(-f_p2 * p0_p2 * y0,
|
|
f_p2 * (z0 + x0 * p0_p2), -f_p2 * y0, 0);
|
|
jc[jc_pos + 2] = make_float4(p1_p2 * jic, 0, f_p2, -f_p2 * p1_p2);
|
|
jc[jc_pos + 3] = make_float4(-f_p2 * (z0 + y0 * p1_p2),
|
|
f_p2 * x0 * p1_p2, f_p2 * x0, 0);
|
|
JFRT_SET_JC_END
|
|
}
|
|
////////////////////
|
|
jp[(tidx << 1)] = make_float4(f_p2 * (r[0] - r[6] * p0_p2),
|
|
f_p2 * (r[1] - r[7] * p0_p2),
|
|
f_p2 * (r[2] - r[8] * p0_p2), 0);
|
|
jp[(tidx << 1) + 1] = make_float4(f_p2 * (r[3] - r[6] * p1_p2),
|
|
f_p2 * (r[4] - r[7] * p1_p2),
|
|
f_p2 * (r[5] - r[8] * p1_p2), 0);
|
|
} else {
|
|
if (jc) {
|
|
JACOBIAN_SET_JC_BEGIN
|
|
float4 sc1 = tex1Dfetch(tex_jacobian_sj, proj.x);
|
|
jc[jc_pos] = make_float4(p0_p2 * jic * sc1.x, f_p2 * sc1.y, 0,
|
|
-f_p2 * p0_p2 * sc1.w);
|
|
jc[jc_pos + 2] = make_float4(p1_p2 * jic * sc1.x, 0, f_p2 * sc1.z,
|
|
-f_p2 * p1_p2 * sc1.w);
|
|
float4 sc2 = tex1Dfetch(tex_jacobian_sj, proj.x + 1);
|
|
jc[jc_pos + 1] = make_float4(-sc2.x * f_p2 * p0_p2 * y0,
|
|
sc2.y * f_p2 * (z0 + x0 * p0_p2),
|
|
-sc2.z * f_p2 * y0, 0);
|
|
jc[jc_pos + 3] =
|
|
make_float4(-sc2.x * f_p2 * (z0 + y0 * p1_p2),
|
|
sc2.y * f_p2 * x0 * p1_p2, sc2.z * f_p2 * x0, 0);
|
|
JFRT_SET_JC_END
|
|
}
|
|
|
|
float4 sc3 = tex1Dfetch(tex_jacobian_sj, proj.y + ptx);
|
|
jp[(tidx << 1)] = make_float4(sc3.x * f_p2 * (r[0] - r[6] * p0_p2),
|
|
sc3.y * f_p2 * (r[1] - r[7] * p0_p2),
|
|
sc3.z * f_p2 * (r[2] - r[8] * p0_p2), 0);
|
|
jp[(tidx << 1) + 1] =
|
|
make_float4(sc3.x * f_p2 * (r[3] - r[6] * p1_p2),
|
|
sc3.y * f_p2 * (r[4] - r[7] * p1_p2),
|
|
sc3.z * f_p2 * (r[5] - r[8] * p1_p2), 0);
|
|
}
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////
|
|
void ProgramCU::ComputeJacobian(CuTexImage& camera, CuTexImage& point,
|
|
CuTexImage& jc, CuTexImage& jp,
|
|
CuTexImage& proj_map, CuTexImage& sj,
|
|
CuTexImage& meas, CuTexImage& cmlist,
|
|
bool intrinsic_fixed, int radial_distortion,
|
|
bool shuffle) {
|
|
float jfc = intrinsic_fixed ? 0.0f : 1.0f;
|
|
unsigned int len = proj_map.GetImgWidth();
|
|
unsigned int bsize = JACOBIAN_FRT_KWIDTH;
|
|
unsigned int nblock = (len + bsize - 1) / bsize;
|
|
unsigned int bw, bh;
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
|
|
camera.BindTexture(tex_jacobian_cam);
|
|
point.BindTexture(tex_jacobian_pts);
|
|
proj_map.BindTexture(tex_jacobian_idx);
|
|
|
|
if (!jc.IsValid()) shuffle = false;
|
|
if (shuffle) cmlist.BindTexture(tex_jacobian_shuffle);
|
|
if (sj.IsValid()) sj.BindTexture(tex_jacobian_sj);
|
|
|
|
if (radial_distortion == -1) {
|
|
meas.BindTexture(tex_jacobian_meas);
|
|
if (sj.IsValid()) {
|
|
if (shuffle)
|
|
jacobian_frt_kernel<true, false, true, true><<<grid, block>>>(
|
|
(float4*)jc.data(), (float4*)jp.data(), len,
|
|
camera.GetImgWidth() * 2, bw * bsize, jfc);
|
|
else
|
|
jacobian_frt_kernel<true, false, true, false><<<grid, block>>>(
|
|
(float4*)jc.data(), (float4*)jp.data(), len,
|
|
camera.GetImgWidth() * 2, bw * bsize, jfc);
|
|
} else {
|
|
if (shuffle)
|
|
jacobian_frt_kernel<true, false, false, true><<<grid, block>>>(
|
|
(float4*)jc.data(), (float4*)jp.data(), len,
|
|
camera.GetImgWidth() * 2, bw * bsize, jfc);
|
|
else
|
|
jacobian_frt_kernel<true, false, false, false><<<grid, block>>>(
|
|
(float4*)jc.data(), (float4*)jp.data(), len,
|
|
camera.GetImgWidth() * 2, bw * bsize, jfc);
|
|
}
|
|
} else if (radial_distortion) {
|
|
if (sj.IsValid()) {
|
|
if (shuffle)
|
|
jacobian_frt_kernel<false, true, true, true><<<grid, block>>>(
|
|
(float4*)jc.data(), (float4*)jp.data(), len,
|
|
camera.GetImgWidth() * 2, bw * bsize, jfc);
|
|
else
|
|
jacobian_frt_kernel<false, true, true, false><<<grid, block>>>(
|
|
(float4*)jc.data(), (float4*)jp.data(), len,
|
|
camera.GetImgWidth() * 2, bw * bsize, jfc);
|
|
} else {
|
|
if (shuffle)
|
|
jacobian_frt_kernel<false, true, false, true><<<grid, block>>>(
|
|
(float4*)jc.data(), (float4*)jp.data(), len,
|
|
camera.GetImgWidth() * 2, bw * bsize, jfc);
|
|
else
|
|
jacobian_frt_kernel<false, true, false, false><<<grid, block>>>(
|
|
(float4*)jc.data(), (float4*)jp.data(), len,
|
|
camera.GetImgWidth() * 2, bw * bsize, jfc);
|
|
}
|
|
} else {
|
|
if (sj.IsValid()) {
|
|
if (shuffle)
|
|
jacobian_frt_kernel<false, false, true, true><<<grid, block>>>(
|
|
(float4*)jc.data(), (float4*)jp.data(), len,
|
|
camera.GetImgWidth() * 2, bw * bsize, jfc);
|
|
else
|
|
jacobian_frt_kernel<false, false, true, false><<<grid, block>>>(
|
|
(float4*)jc.data(), (float4*)jp.data(), len,
|
|
camera.GetImgWidth() * 2, bw * bsize, jfc);
|
|
} else {
|
|
if (shuffle)
|
|
jacobian_frt_kernel<false, false, false, true><<<grid, block>>>(
|
|
(float4*)jc.data(), (float4*)jp.data(), len,
|
|
camera.GetImgWidth() * 2, bw * bsize, jfc);
|
|
else
|
|
jacobian_frt_kernel<false, false, false, false><<<grid, block>>>(
|
|
(float4*)jc.data(), (float4*)jp.data(), len,
|
|
camera.GetImgWidth() * 2, bw * bsize, jfc);
|
|
}
|
|
}
|
|
|
|
ProgramCU::CheckErrorCUDA("ComputeJacobian");
|
|
}
|
|
|
|
texture<float4, 1, cudaReadModeElementType> tex_compact_cam;
|
|
__global__ void uncompress_frt_kernel(int ncam, float4* ucam) {
|
|
int tidx = IMUL(blockIdx.x, blockDim.x) + threadIdx.x;
|
|
if (tidx >= ncam) return;
|
|
int fetch_index = tidx << 1;
|
|
int write_index = IMUL(tidx, 4);
|
|
float4 temp1 = tex1Dfetch(tex_compact_cam, fetch_index);
|
|
ucam[write_index] = temp1;
|
|
|
|
float4 temp2 = tex1Dfetch(tex_compact_cam, fetch_index + 1);
|
|
float rx = temp2.x;
|
|
float ry = temp2.y;
|
|
float rz = temp2.z;
|
|
float rx_rx = rx * rx;
|
|
float ry_ry = ry * ry;
|
|
float rz_rz = rz * rz;
|
|
float aa = sqrt(rx_rx + ry_ry + rz_rz);
|
|
float caa, saa;
|
|
sincosf(aa, &saa, &caa);
|
|
float ct = aa == 0.0 ? 0.5 : FDIV2(1.0 - caa, aa * aa);
|
|
float st = aa == 0.0 ? 1 : FDIV2(saa, aa);
|
|
float rz_st = rz * st;
|
|
float rx_st = rx * st;
|
|
float ry_st = ry * st;
|
|
float ry_ry_ct = ry_ry * ct;
|
|
float rx_rx_ct = rx_rx * ct;
|
|
float rz_rz_ct = rz_rz * ct;
|
|
float rx_ry_ct = rx * ry * ct;
|
|
float rz_rx_ct = rz * rx * ct;
|
|
float ry_rz_ct = ry * rz * ct;
|
|
|
|
////////////////////////////////////////////////////////////
|
|
ucam[write_index + 1] =
|
|
make_float4((1.0 - (ry_ry_ct + rz_rz_ct)), (rx_ry_ct - rz_st),
|
|
(rz_rx_ct + ry_st), (rx_ry_ct + rz_st));
|
|
|
|
ucam[write_index + 2] =
|
|
make_float4((1.0 - (rz_rz_ct + rx_rx_ct)), (ry_rz_ct - rx_st),
|
|
(rz_rx_ct - ry_st), (ry_rz_ct + rx_st));
|
|
|
|
ucam[write_index + 3] =
|
|
make_float4((1.0 - (rx_rx_ct + ry_ry_ct)), temp2.w, 0, 0);
|
|
}
|
|
|
|
void ProgramCU::UncompressCamera(int ncam, CuTexImage& camera,
|
|
CuTexImage& result) {
|
|
unsigned int len = ncam;
|
|
unsigned int bsize = 64;
|
|
unsigned int nblock = (len + bsize - 1) / bsize;
|
|
dim3 grid(nblock);
|
|
dim3 block(bsize);
|
|
camera.BindTexture(tex_compact_cam);
|
|
uncompress_frt_kernel<<<grid, block>>>(len, (float4*)result.data());
|
|
CheckErrorCUDA("UncompressCamera");
|
|
}
|
|
|
|
texture<float4, 1, cudaReadModeElementType> tex_uncompressed_cam;
|
|
|
|
__global__ void compress_frt_kernel(int ncam, float4* zcam) {
|
|
int tidx = IMUL(blockIdx.x, blockDim.x) + threadIdx.x;
|
|
if (tidx >= ncam) return;
|
|
int fetch_index = tidx << 2;
|
|
int write_index = tidx << 1;
|
|
float4 temp1 = tex1Dfetch(tex_compact_cam, fetch_index);
|
|
zcam[write_index] = temp1;
|
|
|
|
float4 r1 = tex1Dfetch(tex_compact_cam, fetch_index + 1);
|
|
float4 r2 = tex1Dfetch(tex_compact_cam, fetch_index + 2);
|
|
float4 r3 = tex1Dfetch(tex_compact_cam, fetch_index + 3);
|
|
|
|
float a = (r1.x + r2.x + r3.x - 1.0) / 2.0;
|
|
if (a >= 1.0) {
|
|
zcam[write_index + 1] = make_float4(0, 0, 0, 0);
|
|
} else {
|
|
float aa = acos(a), b = 0.5 * aa * rsqrt(1 - a * a);
|
|
zcam[write_index + 1] = make_float4(b * (r2.w - r2.y), b * (r1.z - r2.z),
|
|
b * (r1.w - r1.y), r3.y);
|
|
}
|
|
}
|
|
|
|
void ProgramCU::CompressCamera(int ncam, CuTexImage& camera0,
|
|
CuTexImage& result) {
|
|
unsigned int len = ncam;
|
|
unsigned int bsize = 64;
|
|
unsigned int nblock = (len + bsize - 1) / bsize;
|
|
dim3 grid(nblock), block(bsize);
|
|
camera0.BindTexture(tex_uncompressed_cam);
|
|
compress_frt_kernel<<<grid, block>>>(ncam, (float4*)result.data());
|
|
CheckErrorCUDA("CompressCamera");
|
|
}
|
|
|
|
__device__ inline void uncompress_rodrigues_rotation(float rx, float ry,
|
|
float rz, float* r) {
|
|
float rx_rx = rx * rx;
|
|
float ry_ry = ry * ry;
|
|
float rz_rz = rz * rz;
|
|
float aa = sqrt(rx_rx + ry_ry + rz_rz);
|
|
float caa, saa;
|
|
sincosf(aa, &saa, &caa);
|
|
float ct = aa == 0.0 ? 0.5 : FDIV2(1.0 - caa, aa * aa);
|
|
float st = aa == 0.0 ? 1 : FDIV2(saa, aa);
|
|
float rz_st = rz * st;
|
|
float rx_st = rx * st;
|
|
float ry_st = ry * st;
|
|
float ry_ry_ct = ry_ry * ct;
|
|
float rx_rx_ct = rx_rx * ct;
|
|
float rz_rz_ct = rz_rz * ct;
|
|
float rx_ry_ct = rx * ry * ct;
|
|
float rz_rx_ct = rz * rx * ct;
|
|
float ry_rz_ct = ry * rz * ct;
|
|
r[0] = (1.0 - (ry_ry_ct + rz_rz_ct));
|
|
r[1] = (rx_ry_ct - rz_st);
|
|
r[2] = (rz_rx_ct + ry_st);
|
|
r[3] = (rx_ry_ct + rz_st);
|
|
r[4] = (1.0 - (rz_rz_ct + rx_rx_ct));
|
|
r[5] = (ry_rz_ct - rx_st);
|
|
r[6] = (rz_rx_ct - ry_st);
|
|
r[7] = (ry_rz_ct + rx_st);
|
|
r[8] = (1.0 - (rx_rx_ct + ry_ry_ct));
|
|
}
|
|
|
|
texture<float4, 1, cudaReadModeElementType> tex_update_cam;
|
|
texture<float4, 1, cudaReadModeElementType> tex_update_cam_delta;
|
|
|
|
__global__ void update_camera_kernel(int ncam, float4* newcam) {
|
|
int tidx = IMUL(blockIdx.x, blockDim.x) + threadIdx.x;
|
|
if (tidx >= ncam) return;
|
|
int index0 = tidx << 2;
|
|
int index1 = tidx << 1;
|
|
{
|
|
float4 c1 = tex1Dfetch(tex_update_cam, index0);
|
|
float4 d1 = tex1Dfetch(tex_update_cam_delta, index1);
|
|
float4 c2 = make_float4(max(c1.x + d1.x, 1e-10f), c1.y + d1.y, c1.z + d1.z,
|
|
c1.w + d1.w);
|
|
newcam[index0] = c2;
|
|
}
|
|
{
|
|
float r[9], dr[9]; //, nr[9];
|
|
float4 r1 = tex1Dfetch(tex_update_cam, index0 + 1);
|
|
r[0] = r1.x;
|
|
r[1] = r1.y;
|
|
r[2] = r1.z;
|
|
r[3] = r1.w;
|
|
float4 r2 = tex1Dfetch(tex_update_cam, index0 + 2);
|
|
r[4] = r2.x;
|
|
r[5] = r2.y;
|
|
r[6] = r2.z;
|
|
r[7] = r2.w;
|
|
float4 r3 = tex1Dfetch(tex_update_cam, index0 + 3);
|
|
r[8] = r3.x;
|
|
|
|
float4 dd = tex1Dfetch(tex_update_cam_delta, index1 + 1);
|
|
uncompress_rodrigues_rotation(dd.x, dd.y, dd.z, dr);
|
|
|
|
///////////////////////////////////////////////
|
|
newcam[index0 + 1] =
|
|
make_float4(dr[0] * r[0] + dr[1] * r[3] + dr[2] * r[6],
|
|
dr[0] * r[1] + dr[1] * r[4] + dr[2] * r[7],
|
|
dr[0] * r[2] + dr[1] * r[5] + dr[2] * r[8],
|
|
dr[3] * r[0] + dr[4] * r[3] + dr[5] * r[6]);
|
|
newcam[index0 + 2] =
|
|
make_float4(dr[3] * r[1] + dr[4] * r[4] + dr[5] * r[7],
|
|
dr[3] * r[2] + dr[4] * r[5] + dr[5] * r[8],
|
|
dr[6] * r[0] + dr[7] * r[3] + dr[8] * r[6],
|
|
dr[6] * r[1] + dr[7] * r[4] + dr[8] * r[7]);
|
|
newcam[index0 + 3] = make_float4(dr[6] * r[2] + dr[7] * r[5] + dr[8] * r[8],
|
|
r3.y + dd.w, r3.z, r3.w);
|
|
}
|
|
}
|
|
|
|
void ProgramCU::UpdateCameraPoint(int ncam, CuTexImage& camera,
|
|
CuTexImage& point, CuTexImage& delta,
|
|
CuTexImage& new_camera, CuTexImage& new_point,
|
|
int mode) {
|
|
if (mode != 2) {
|
|
unsigned int len = ncam;
|
|
unsigned int bsize = 64;
|
|
unsigned int nblock = (len + bsize - 1) / bsize;
|
|
dim3 grid(nblock), block(bsize);
|
|
camera.BindTexture(tex_update_cam);
|
|
delta.BindTexture(tex_update_cam_delta);
|
|
update_camera_kernel<<<grid, block>>>(len, (float4*)new_camera.data());
|
|
CheckErrorCUDA("UpdateCamera");
|
|
}
|
|
|
|
// update the points
|
|
if (mode != 1) {
|
|
CuTexImage dp;
|
|
dp.SetTexture(delta.data() + 8 * ncam, point.GetLength());
|
|
ComputeSAXPY(1.0f, dp, point, new_point);
|
|
CheckErrorCUDA("UpdatePoint");
|
|
}
|
|
}
|
|
|
|
#define PROJECTION_FRT_KWIDTH 64
|
|
|
|
texture<float4, 1, cudaReadModeElementType> tex_projection_cam;
|
|
texture<int2, 1, cudaReadModeElementType> tex_projection_idx;
|
|
texture<float4, 1, cudaReadModeElementType> tex_projection_pts;
|
|
texture<float2, 1, cudaReadModeElementType> tex_projection_mea;
|
|
|
|
// run 32/64/128 projections in a block
|
|
template <bool md, bool pd>
|
|
__global__ void projection_frt_kernel(int nproj, int rowsz, float2* pj) {
|
|
////////////////////////////////
|
|
int tidx = threadIdx.x + blockIdx.x * blockDim.x + blockIdx.y * rowsz;
|
|
if (tidx >= nproj) return;
|
|
float f, m[3], t[3]; // r[9],
|
|
__shared__ float rr_data[PROJECTION_FRT_KWIDTH * 9];
|
|
float* r = rr_data + IMUL(9, threadIdx.x);
|
|
int2 proj = tex1Dfetch(tex_projection_idx, tidx);
|
|
int cpos = proj.x << 1;
|
|
float4 ft = tex1Dfetch(tex_projection_cam, cpos);
|
|
f = ft.x;
|
|
t[0] = ft.y;
|
|
t[1] = ft.z;
|
|
t[2] = ft.w;
|
|
float4 r1 = tex1Dfetch(tex_projection_cam, cpos + 1);
|
|
r[0] = r1.x;
|
|
r[1] = r1.y;
|
|
r[2] = r1.z;
|
|
r[3] = r1.w;
|
|
float4 r2 = tex1Dfetch(tex_projection_cam, cpos + 2);
|
|
r[4] = r2.x;
|
|
r[5] = r2.y;
|
|
r[6] = r2.z;
|
|
r[7] = r2.w;
|
|
float4 r3 = tex1Dfetch(tex_projection_cam, cpos + 3);
|
|
r[8] = r3.x;
|
|
|
|
float4 temp = tex1Dfetch(tex_projection_pts, proj.y);
|
|
m[0] = temp.x;
|
|
m[1] = temp.y;
|
|
m[2] = temp.z;
|
|
|
|
float p0 = r[0] * m[0] + r[1] * m[1] + r[2] * m[2] + t[0];
|
|
float p1 = r[3] * m[0] + r[4] * m[1] + r[5] * m[2] + t[1];
|
|
float p2 = r[6] * m[0] + r[7] * m[1] + r[8] * m[2] + t[2];
|
|
|
|
if (pd) {
|
|
float rr = 1.0 + r3.y * (p0 * p0 + p1 * p1) / (p2 * p2);
|
|
float f_p2 = FDIV2(f * rr, p2);
|
|
float2 ms = tex1Dfetch(tex_projection_mea, tidx);
|
|
pj[tidx] = make_float2(ms.x - p0 * f_p2, ms.y - p1 * f_p2);
|
|
} else if (md) {
|
|
float f_p2 = FDIV2(f, p2);
|
|
float2 ms = tex1Dfetch(tex_projection_mea, tidx);
|
|
float rd = 1.0 + r3.y * (ms.x * ms.x + ms.y * ms.y);
|
|
pj[tidx] = make_float2(ms.x * rd - p0 * f_p2, ms.y * rd - p1 * f_p2);
|
|
} else {
|
|
float f_p2 = FDIV2(f, p2);
|
|
float2 ms = tex1Dfetch(tex_projection_mea, tidx);
|
|
pj[tidx] = make_float2(ms.x - p0 * f_p2, ms.y - p1 * f_p2);
|
|
}
|
|
}
|
|
|
|
void ProgramCU::ComputeProjection(CuTexImage& camera, CuTexImage& point,
|
|
CuTexImage& meas, CuTexImage& proj_map,
|
|
CuTexImage& proj, int radial) {
|
|
unsigned int len = proj_map.GetImgWidth();
|
|
unsigned int bsize = PROJECTION_FRT_KWIDTH;
|
|
unsigned int nblock = (len + bsize - 1) / bsize;
|
|
camera.BindTexture(tex_projection_cam);
|
|
point.BindTexture(tex_projection_pts);
|
|
proj_map.BindTexture(tex_projection_idx);
|
|
unsigned int bw, bh;
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
meas.BindTexture(tex_projection_mea);
|
|
if (radial == -1)
|
|
projection_frt_kernel<true, false><<<grid, block>>>(len, bw * bsize,
|
|
(float2*)proj.data());
|
|
else if (radial)
|
|
projection_frt_kernel<false, true><<<grid, block>>>(len, bw * bsize,
|
|
(float2*)proj.data());
|
|
else
|
|
projection_frt_kernel<false, false><<<grid, block>>>(len, bw * bsize,
|
|
(float2*)proj.data());
|
|
CheckErrorCUDA("ComputeProjection");
|
|
}
|
|
|
|
template <bool md, bool pd>
|
|
__global__ void projectionx_frt_kernel(int nproj, int rowsz, float2* pj) {
|
|
////////////////////////////////
|
|
int tidx = threadIdx.x + blockIdx.x * blockDim.x + blockIdx.y * rowsz;
|
|
if (tidx >= nproj) return;
|
|
float f, m[3], t[3]; // r[9],
|
|
__shared__ float rr_data[PROJECTION_FRT_KWIDTH * 9];
|
|
float* r = rr_data + IMUL(9, threadIdx.x);
|
|
int2 proj = tex1Dfetch(tex_projection_idx, tidx);
|
|
int cpos = proj.x << 1;
|
|
float4 ft = tex1Dfetch(tex_projection_cam, cpos);
|
|
f = ft.x;
|
|
t[0] = ft.y;
|
|
t[1] = ft.z;
|
|
t[2] = ft.w;
|
|
float4 r1 = tex1Dfetch(tex_projection_cam, cpos + 1);
|
|
r[0] = r1.x;
|
|
r[1] = r1.y;
|
|
r[2] = r1.z;
|
|
r[3] = r1.w;
|
|
float4 r2 = tex1Dfetch(tex_projection_cam, cpos + 2);
|
|
r[4] = r2.x;
|
|
r[5] = r2.y;
|
|
r[6] = r2.z;
|
|
r[7] = r2.w;
|
|
float4 r3 = tex1Dfetch(tex_projection_cam, cpos + 3);
|
|
r[8] = r3.x;
|
|
|
|
float4 temp = tex1Dfetch(tex_projection_pts, proj.y);
|
|
m[0] = temp.x;
|
|
m[1] = temp.y;
|
|
m[2] = temp.z;
|
|
|
|
float p0 = r[0] * m[0] + r[1] * m[1] + r[2] * m[2] + t[0];
|
|
float p1 = r[3] * m[0] + r[4] * m[1] + r[5] * m[2] + t[1];
|
|
float p2 = r[6] * m[0] + r[7] * m[1] + r[8] * m[2] + t[2];
|
|
if (pd) {
|
|
float rr = 1.0 + r3.y * (p0 * p0 + p1 * p1) / (p2 * p2);
|
|
float f_p2 = FDIV2(f, p2);
|
|
float2 ms = tex1Dfetch(tex_projection_mea, tidx);
|
|
pj[tidx] = make_float2(ms.x / rr - p0 * f_p2, ms.y / rr - p1 * f_p2);
|
|
} else if (md) {
|
|
float f_p2 = FDIV2(f, p2);
|
|
float2 ms = tex1Dfetch(tex_projection_mea, tidx);
|
|
float rd = 1.0 + r3.y * (ms.x * ms.x + ms.y * ms.y);
|
|
pj[tidx] = make_float2(ms.x - p0 * f_p2 / rd, ms.y - p1 * f_p2 / rd);
|
|
} else {
|
|
float f_p2 = FDIV2(f, p2);
|
|
float2 ms = tex1Dfetch(tex_projection_mea, tidx);
|
|
pj[tidx] = make_float2(ms.x - p0 * f_p2, ms.y - p1 * f_p2);
|
|
}
|
|
}
|
|
|
|
void ProgramCU::ComputeProjectionX(CuTexImage& camera, CuTexImage& point,
|
|
CuTexImage& meas, CuTexImage& proj_map,
|
|
CuTexImage& proj, int radial) {
|
|
unsigned int len = proj_map.GetImgWidth();
|
|
unsigned int bsize = PROJECTION_FRT_KWIDTH;
|
|
unsigned int nblock = (len + bsize - 1) / bsize;
|
|
camera.BindTexture(tex_projection_cam);
|
|
point.BindTexture(tex_projection_pts);
|
|
proj_map.BindTexture(tex_projection_idx);
|
|
unsigned int bw, bh;
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
meas.BindTexture(tex_projection_mea);
|
|
if (radial == -1)
|
|
projectionx_frt_kernel<true, false><<<grid, block>>>(len, bw * bsize,
|
|
(float2*)proj.data());
|
|
else if (radial)
|
|
projectionx_frt_kernel<false, true><<<grid, block>>>(len, bw * bsize,
|
|
(float2*)proj.data());
|
|
else
|
|
projectionx_frt_kernel<false, false><<<grid, block>>>(len, bw * bsize,
|
|
(float2*)proj.data());
|
|
CheckErrorCUDA("ComputeProjection");
|
|
}
|
|
|
|
texture<float2, 1, cudaReadModeElementType> tex_jte_pe;
|
|
texture<float, 1, cudaReadModeElementType> tex_jte_pex;
|
|
texture<float4, 1, cudaReadModeElementType> tex_jte_jc;
|
|
texture<float4, 1, cudaReadModeElementType> tex_jte_jc2;
|
|
texture<int, 1, cudaReadModeElementType> tex_jte_cmp;
|
|
texture<int, 1, cudaReadModeElementType> tex_jte_cmt;
|
|
texture<float4, 1, cudaReadModeElementType> tex_jte_jc3;
|
|
texture<float4, 1, cudaReadModeElementType> tex_jte_jc4;
|
|
|
|
__global__ void jte_cam_kernel(int num, float* jc, float* jte) {
|
|
__shared__ float value[128];
|
|
|
|
// 8thread per camera
|
|
int col = IMUL(blockIdx.x, blockDim.x) + threadIdx.x;
|
|
if (col >= num) return;
|
|
|
|
int cam = col >> 4; // 8 thread per camera
|
|
|
|
// read data range for this camera, 8 thread will do the same thing
|
|
int idx1 = tex1Dfetch(tex_jte_cmp, cam) << 4; // first camera
|
|
int idx2 = tex1Dfetch(tex_jte_cmp, cam + 1) << 4; // last camera + 1
|
|
|
|
///////////////////////////////
|
|
int offset = threadIdx.x & 0xf; // which parameter of this camera
|
|
int part = offset >= 8 ? 1 : 0;
|
|
/////////////////////////////
|
|
|
|
float result = 0;
|
|
// loop to read the index of the projection.
|
|
// so to get the location to read the jacobian
|
|
for (int i = idx1 + offset; i < idx2; i += 16) {
|
|
float temp = jc[i];
|
|
// every 8 thread will read the same position.
|
|
int index = tex1Dfetch(tex_jte_cmt, i >> 4);
|
|
float v = tex1Dfetch(tex_jte_pex, (index << 1) + part);
|
|
//////////////////////
|
|
result += temp * v;
|
|
}
|
|
value[threadIdx.x] = result;
|
|
// write back
|
|
if (offset < 8) jte[(cam << 3) + offset] = (result + value[threadIdx.x + 8]);
|
|
}
|
|
|
|
template <int KH, int TEXN>
|
|
__global__ void jte_cam_vec_kernel(int num, float* jte) {
|
|
__shared__ float value[KH * 128];
|
|
int cam = blockIdx.x * KH + threadIdx.y;
|
|
if (cam >= num) return;
|
|
|
|
// read data range for this camera
|
|
// 8 thread will do the same thing
|
|
int idx1 = tex1Dfetch(tex_jte_cmp, cam) << 2; // first camera
|
|
int idx2 = tex1Dfetch(tex_jte_cmp, cam + 1) << 2; // last camera + 1
|
|
int part = (threadIdx.x & 0x02) ? 1 : 0;
|
|
|
|
float rx = 0, ry = 0, rz = 0, rw = 0;
|
|
// loop to read the index of the projection.
|
|
// so to get the location to read the jacobian
|
|
for (int i = idx1 + threadIdx.x; i < idx2; i += 32) {
|
|
float4 temp;
|
|
if (TEXN == 1) {
|
|
temp = tex1Dfetch(tex_jte_jc, i);
|
|
}
|
|
if (TEXN == 2) {
|
|
int texid = i >> 25;
|
|
if (texid == 0)
|
|
temp = tex1Dfetch(tex_jte_jc, i);
|
|
else
|
|
temp = tex1Dfetch(tex_jte_jc2, (i & 0x1ffffff));
|
|
}
|
|
if (TEXN == 4) {
|
|
int index = tex1Dfetch(tex_jte_cmt, i >> 2);
|
|
int iii = (index << 2) + (i & 0x3);
|
|
int texid = iii >> 25;
|
|
/////////////////////////////////
|
|
if (texid == 0)
|
|
temp = tex1Dfetch(tex_jte_jc, iii);
|
|
else if (texid == 1)
|
|
temp = tex1Dfetch(tex_jte_jc2, (iii & 0x1ffffff));
|
|
else if (texid == 2)
|
|
temp = tex1Dfetch(tex_jte_jc3, (iii & 0x1ffffff));
|
|
else
|
|
temp = tex1Dfetch(tex_jte_jc4, (iii & 0x1ffffff));
|
|
}
|
|
int index = tex1Dfetch(tex_jte_cmt, i >> 2);
|
|
float vv = tex1Dfetch(tex_jte_pex, (index << 1) + part);
|
|
rx += temp.x * vv;
|
|
ry += temp.y * vv;
|
|
rz += temp.z * vv;
|
|
rw += temp.w * vv;
|
|
}
|
|
////////////////////////////////////
|
|
int widx = (threadIdx.y << 7) + (threadIdx.x << 2);
|
|
///////////////////////////////////
|
|
// write back
|
|
value[widx] = rx;
|
|
value[widx + 1] = ry;
|
|
value[widx + 2] = rz;
|
|
value[widx + 3] = rw;
|
|
////////////////////////////////////
|
|
int ridx = (threadIdx.y << 7) + threadIdx.x;
|
|
value[ridx] = ((value[ridx] + value[ridx + 32]) +
|
|
(value[ridx + 64] + value[ridx + 96]));
|
|
if (threadIdx.x < 16) value[ridx] += value[ridx + 16];
|
|
if (threadIdx.x < 8)
|
|
jte[(cam << 3) + threadIdx.x] = value[ridx] + value[ridx + 8];
|
|
}
|
|
|
|
template <int KH, bool JT>
|
|
__global__ void jte_cam_vec32_kernel(int num, float* jc, float* jte) {
|
|
__shared__ float value[KH * 32];
|
|
int cam = blockIdx.x * KH + threadIdx.y;
|
|
if (cam >= num) return;
|
|
float sum = 0;
|
|
int rowpos = (threadIdx.y << 5);
|
|
int index = threadIdx.x + rowpos;
|
|
int xypart = (threadIdx.x & 0x08) ? 1 : 0;
|
|
int part2 = threadIdx.x & 0xf;
|
|
// read data range for this camera
|
|
// 8 thread will do the same thing
|
|
int idx1 = tex1Dfetch(tex_jte_cmp, cam) << 4; // first camera
|
|
int idx2 = tex1Dfetch(tex_jte_cmp, cam + 1) << 4; // last camera + 1
|
|
|
|
// loop to read the index of the projection.
|
|
// so to get the location to read the jacobian
|
|
for (int i = idx1 + threadIdx.x; i < idx2; i += 32) {
|
|
int index = tex1Dfetch(tex_jte_cmt, i >> 4);
|
|
float temp;
|
|
if (JT)
|
|
temp = jc[i];
|
|
else
|
|
temp = jc[(index << 4) + part2];
|
|
|
|
float v = tex1Dfetch(tex_jte_pex, (index << 1) + xypart);
|
|
sum += temp * v;
|
|
}
|
|
value[index] = sum;
|
|
|
|
if (threadIdx.x < 16) value[index] += value[index + 16];
|
|
if (threadIdx.x < 8)
|
|
jte[(cam << 3) + threadIdx.x] = value[index] + value[index + 8];
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////
|
|
texture<float4, 1, cudaReadModeElementType> tex_jte_jp;
|
|
texture<int, 1, cudaReadModeElementType> tex_jte_pmp;
|
|
texture<float4, 1, cudaReadModeElementType> tex_jte_jp2;
|
|
|
|
__global__ void jte_point_kernel(int num, float4* jte) {
|
|
////////////////////////////
|
|
int index = blockIdx.x * blockDim.x + threadIdx.x;
|
|
if (index >= num) return;
|
|
|
|
int idx1 = tex1Dfetch(tex_jte_pmp, index); // first camera
|
|
int idx2 = tex1Dfetch(tex_jte_pmp, index + 1); // last camera + 1
|
|
float4 result = make_float4(0, 0, 0, 0);
|
|
for (int i = idx1; i < idx2; ++i) {
|
|
// error vector
|
|
float2 ev = tex1Dfetch(tex_jte_pe, i);
|
|
|
|
float4 j1 = tex1Dfetch(tex_jte_jp, i << 1);
|
|
result.x += j1.x * ev.x;
|
|
result.y += j1.y * ev.x;
|
|
result.z += j1.z * ev.x;
|
|
|
|
float4 j2 = tex1Dfetch(tex_jte_jp, 1 + (i << 1));
|
|
result.x += j2.x * ev.y;
|
|
result.y += j2.y * ev.y;
|
|
result.z += j2.z * ev.y;
|
|
}
|
|
jte[index] = result;
|
|
}
|
|
|
|
////////////////////
|
|
// faster but not always more accurate
|
|
//#define JTE_POINT_VEC2
|
|
|
|
template <int KH, int TEXN>
|
|
__global__ void jte_point_vec_kernel(int num, int rowsz, float* jte) {
|
|
////////////////////////////
|
|
__shared__ float value[KH * 128];
|
|
int index = blockIdx.x * KH + threadIdx.y + blockIdx.y * rowsz;
|
|
if (index >= num) return;
|
|
#ifdef JTE_POINT_VEC2
|
|
int idx1 = tex1Dfetch(tex_jte_pmp, index); // first
|
|
int idx2 = tex1Dfetch(tex_jte_pmp, index + 1); // last + 1
|
|
#else
|
|
int idx1 = tex1Dfetch(tex_jte_pmp, index) << 1; // first
|
|
int idx2 = tex1Dfetch(tex_jte_pmp, index + 1) << 1; // last + 1
|
|
#endif
|
|
float rx = 0, ry = 0, rz = 0;
|
|
for (int i = idx1 + threadIdx.x; i < idx2; i += 32) {
|
|
if (TEXN == 2 && i >> 25) {
|
|
#ifdef JTE_POINT_VEC2
|
|
|
|
float2 vv = tex1Dfetch(tex_jte_pe, i);
|
|
float4 jp1 = tex1Dfetch(tex_jte_jp, ((i & 0x1ffffff) << 1));
|
|
float4 jp2 = tex1Dfetch(tex_jte_jp, ((i & 0x1ffffff) << 1) + 1);
|
|
rx += (jp1.x * vv.x + jp2.x * vv.y);
|
|
ry += (jp1.y * vv.x + jp2.y * vv.y);
|
|
rz += (jp1.z * vv.x + jp2.z * vv.y);
|
|
#else
|
|
float vv = tex1Dfetch(tex_jte_pex, i);
|
|
float4 jpi = tex1Dfetch(tex_jte_jp2, i & 0x1ffffff);
|
|
rx += jpi.x * vv;
|
|
ry += jpi.y * vv;
|
|
rz += jpi.z * vv;
|
|
#endif
|
|
} else {
|
|
#ifdef JTE_POINT_VEC2
|
|
float2 vv = tex1Dfetch(tex_jte_pe, i);
|
|
float4 jp1 = tex1Dfetch(tex_jte_jp, (i << 1));
|
|
float4 jp2 = tex1Dfetch(tex_jte_jp, (i << 1) + 1);
|
|
rx += (jp1.x * vv.x + jp2.x * vv.y);
|
|
ry += (jp1.y * vv.x + jp2.y * vv.y);
|
|
rz += (jp1.z * vv.x + jp2.z * vv.y);
|
|
#else
|
|
float vv = tex1Dfetch(tex_jte_pex, i);
|
|
float4 jpi = tex1Dfetch(tex_jte_jp, i);
|
|
rx += jpi.x * vv;
|
|
ry += jpi.y * vv;
|
|
rz += jpi.z * vv;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
int rowp = threadIdx.y << 7;
|
|
int loc = (threadIdx.x << 2) + rowp;
|
|
value[loc] = rx;
|
|
value[loc + 1] = ry;
|
|
value[loc + 2] = rz;
|
|
value[loc + 3] = 0;
|
|
|
|
int ridx = threadIdx.x + rowp;
|
|
value[ridx] = ((value[ridx] + value[ridx + 32]) +
|
|
(value[ridx + 64] + value[ridx + 96]));
|
|
if (threadIdx.x < 16) value[ridx] += value[ridx + 16];
|
|
if (threadIdx.x < 8) value[ridx] += value[ridx + 8];
|
|
if (threadIdx.x < 4)
|
|
jte[(index << 2) + threadIdx.x] = value[ridx] + value[ridx + 4];
|
|
}
|
|
|
|
#define JTE_CAMERA_VEC
|
|
#define JTE_POINT_VEC
|
|
|
|
void ProgramCU::ComputeJtE(CuTexImage& pe, CuTexImage& jc, CuTexImage& cmap,
|
|
CuTexImage& cmlist, CuTexImage& jp, CuTexImage& pmap,
|
|
CuTexImage& jte, bool jc_transpose, int mode) {
|
|
//////////////////////////////////////////////////////////
|
|
int ncam = int(cmap.GetImgWidth() - 1); // how many cameras
|
|
size_t szjc = jc.GetDataSize();
|
|
|
|
//////////////////////////////
|
|
cmap.BindTexture(tex_jte_cmp);
|
|
cmlist.BindTexture(tex_jte_cmt);
|
|
#ifdef JTE_CAMERA_VEC2
|
|
pe.BindTexture(tex_jte_pex);
|
|
const unsigned int bheight = 2;
|
|
dim3 block1(32, bheight), grid1((ncam + bheight - 1) / bheight);
|
|
if (mode == 2) {
|
|
} else if (jc_transpose)
|
|
jte_cam_vec32_kernel<bheight, true><<<grid1, block1>>>(ncam, jc.data(),
|
|
jte.data());
|
|
else
|
|
jte_cam_vec32_kernel<bheight, false><<<grid1, block1>>>(ncam, jc.data(),
|
|
jte.data());
|
|
|
|
#elif defined(JTE_CAMERA_VEC)
|
|
pe.BindTexture(tex_jte_pex);
|
|
const unsigned int bheight = 2;
|
|
unsigned int len1 = ncam * 32;
|
|
unsigned int bsize1 = 32 * bheight;
|
|
unsigned int nblock1 = (len1 + bsize1 - 1) / bsize1;
|
|
dim3 grid1(nblock1);
|
|
dim3 block1(32, bheight);
|
|
if (mode == 2) {
|
|
// skip camera
|
|
} else if (szjc > 2 * MAX_TEXSIZE || !jc_transpose) {
|
|
if (jc_transpose)
|
|
jte_cam_vec32_kernel<bheight, true><<<grid1, block1>>>(ncam, jc.data(),
|
|
jte.data());
|
|
else
|
|
jte_cam_vec32_kernel<bheight, false><<<grid1, block1>>>(ncam, jc.data(),
|
|
jte.data());
|
|
} else if (szjc > MAX_TEXSIZE) {
|
|
jc.BindTexture2(tex_jte_jc, tex_jte_jc2);
|
|
jte_cam_vec_kernel<bheight, 2><<<grid1, block1>>>(ncam, jte.data());
|
|
} else {
|
|
jc.BindTexture(tex_jte_jc);
|
|
jte_cam_vec_kernel<bheight, 1><<<grid1, block1>>>(ncam, jte.data());
|
|
}
|
|
#else
|
|
pe.BindTexture(tex_jte_pex);
|
|
unsigned int len1 = ncam * 16;
|
|
unsigned int bsize1 = len1 > 32 * 128 ? 128 : (len1 > 32 * 64 ? 64 : 32);
|
|
unsigned int nblock1 = (len1 + bsize1 - 1) / bsize1;
|
|
dim3 grid1(nblock1), block1(bsize1);
|
|
jte_cam_kernel<<<grid1, block1>>>(len1, jc.data(), jte.data());
|
|
#endif
|
|
CheckErrorCUDA("ComputeJtE<Camera>");
|
|
|
|
////////////////////////////////////////////
|
|
pmap.BindTexture(tex_jte_pmp);
|
|
unsigned int npoint = (pmap.GetImgWidth() - 1);
|
|
#ifndef JTE_POINT_VEC
|
|
size_t len2 = npoint;
|
|
unsigned int bsize2 = 64;
|
|
unsigned int nblock2 = (len2 + bsize2 - 1) / bsize2;
|
|
dim3 grid2(nblock2), block2(bsize2);
|
|
pe.BindTexture(tex_jte_pe);
|
|
jp.BindTexture(tex_jte_jp);
|
|
jte_point_kernel<<<grid2, block2>>>(len2, ((float4*)jte.data()) + 2 * ncam);
|
|
#else
|
|
|
|
#ifdef JTE_POINT_VEC2
|
|
pe.BindTexture(tex_jte_pe);
|
|
#else
|
|
pe.BindTexture(tex_jte_pex);
|
|
#endif
|
|
const unsigned int bheight2 = 2;
|
|
unsigned int bsize2 = 32;
|
|
unsigned int nblock2 = (unsigned int)((npoint + bheight2 - 1) / bheight2);
|
|
unsigned int offsetv = 8 * ncam;
|
|
unsigned int bw, bh;
|
|
GetBlockConfiguration(nblock2, bw, bh);
|
|
dim3 grid2(bw, bh), block2(bsize2, bheight2);
|
|
if (mode == 1) {
|
|
// skip point
|
|
} else if (jp.GetDataSize() > MAX_TEXSIZE) {
|
|
jp.BindTexture2(tex_jte_jp, tex_jte_jp2);
|
|
jte_point_vec_kernel<bheight2, 2><<<grid2, block2>>>(
|
|
npoint, bw * bheight2, ((float*)jte.data()) + offsetv);
|
|
} else {
|
|
jp.BindTexture(tex_jte_jp);
|
|
jte_point_vec_kernel<bheight2, 1><<<grid2, block2>>>(
|
|
npoint, bw * bheight2, ((float*)jte.data()) + offsetv);
|
|
}
|
|
#endif
|
|
CheckErrorCUDA("ComputeJtE<Point>");
|
|
}
|
|
|
|
texture<int, 1, cudaReadModeElementType> tex_jtjd_cmp;
|
|
texture<int, 1, cudaReadModeElementType> tex_jtjd_cmlist;
|
|
|
|
template <int VN, int KH, bool JT>
|
|
__global__ void jtjd_cam_vec32_kernel(int num, int add_existing_dq, float* jc,
|
|
float* jtjd, float* jtjdi) {
|
|
__shared__ float value[KH * 32];
|
|
|
|
// 8thread per camera
|
|
int cam = blockIdx.x * KH + threadIdx.y;
|
|
int part = threadIdx.x & 0x7; // which parameter of this camera
|
|
int part2 = threadIdx.x & 0xf;
|
|
int campos = threadIdx.y << 5;
|
|
int index = threadIdx.x + campos;
|
|
float sum = 0;
|
|
if (cam < num && part < VN) {
|
|
// read data range for this camera
|
|
// 8 thread will do the same thing
|
|
int idx1 = tex1Dfetch(tex_jtjd_cmp, cam) << 4; // first camera
|
|
int idx2 = tex1Dfetch(tex_jtjd_cmp, cam + 1) << 4; // last camera + 1
|
|
|
|
// loop to read the index of the projection.
|
|
// so to get the location to read the jacobian
|
|
for (int i = idx1 + threadIdx.x; i < idx2; i += 32) {
|
|
if (JT) {
|
|
float temp = jc[i];
|
|
sum += temp * temp;
|
|
} else {
|
|
int ii = tex1Dfetch(tex_jtjd_cmlist, i >> 4) << 4;
|
|
float temp = jc[ii + part2];
|
|
sum += temp * temp;
|
|
}
|
|
}
|
|
}
|
|
__syncthreads();
|
|
|
|
if (cam >= num) return;
|
|
// save all the results?
|
|
value[index] = sum;
|
|
if (threadIdx.x < 16) value[index] += value[index + 16];
|
|
if (threadIdx.x < 8)
|
|
|
|
// write back
|
|
if (threadIdx.x < 8) {
|
|
float temp = value[index] + value[index + 8];
|
|
int wpos = threadIdx.x + (cam << 3);
|
|
if (add_existing_dq) temp += jtjd[wpos];
|
|
jtjd[wpos] = temp;
|
|
jtjdi[wpos] = temp == 0 ? 0 : 1 / (temp);
|
|
}
|
|
}
|
|
|
|
texture<float4, 1, cudaReadModeElementType> tex_jtjd_jp;
|
|
texture<int, 1, cudaReadModeElementType> tex_jtjd_pmp;
|
|
texture<float4, 1, cudaReadModeElementType> tex_jtjd_jp2;
|
|
|
|
#define JTJD_POINT_KWIDTH 64
|
|
|
|
template <int TEXN>
|
|
__global__ void jtjd_point_kernel(int num, int rowsz, float4* jtjd,
|
|
float4* jtjdi) {
|
|
////////////////////////////
|
|
int index = blockIdx.x * blockDim.x + threadIdx.x + blockIdx.y * rowsz;
|
|
if (index >= num) return;
|
|
|
|
int idx1 = tex1Dfetch(tex_jtjd_pmp, index); // first camera
|
|
int idx2 = tex1Dfetch(tex_jtjd_pmp, index + 1); // last camera + 1
|
|
float rx = 0, ry = 0, rz = 0;
|
|
for (int i = idx1; i < idx2; ++i) {
|
|
if (TEXN == 2 && i > 0xffffff) {
|
|
float4 j1 = tex1Dfetch(tex_jtjd_jp2, (i & 0xffffff) << 1);
|
|
rx += j1.x * j1.x;
|
|
ry += j1.y * j1.y;
|
|
rz += j1.z * j1.z;
|
|
|
|
float4 j2 = tex1Dfetch(tex_jtjd_jp2, 1 + ((i & 0xffffff) << 1));
|
|
rx += j2.x * j2.x;
|
|
ry += j2.y * j2.y;
|
|
rz += j2.z * j2.z;
|
|
} else {
|
|
float4 j1 = tex1Dfetch(tex_jtjd_jp, i << 1);
|
|
rx += j1.x * j1.x;
|
|
ry += j1.y * j1.y;
|
|
rz += j1.z * j1.z;
|
|
|
|
float4 j2 = tex1Dfetch(tex_jtjd_jp, 1 + (i << 1));
|
|
rx += j2.x * j2.x;
|
|
ry += j2.y * j2.y;
|
|
rz += j2.z * j2.z;
|
|
}
|
|
}
|
|
|
|
if (jtjd) jtjd[index] = make_float4(rx, ry, rz, 0.0f);
|
|
jtjdi[index] = make_float4(1.0f / rx, 1.0f / ry, 1.0f / rz, 0.0f);
|
|
}
|
|
|
|
void ProgramCU::ComputeDiagonal(CuTexImage& jc, CuTexImage& cmap,
|
|
CuTexImage& jp, CuTexImage& pmap,
|
|
CuTexImage& cmlist, CuTexImage& jtjd,
|
|
CuTexImage& jtjdi, bool jc_transpose,
|
|
int radial, bool add_existing_diagc) {
|
|
//////////////////////////////////////////////////////////
|
|
size_t szjc = jc.GetDataSize();
|
|
unsigned int ncam = (cmap.GetImgWidth() - 1); // how many cameras
|
|
|
|
const unsigned int bheight = 2;
|
|
dim3 block1x(32, bheight), grid1x((ncam + bheight - 1) / bheight);
|
|
cmap.BindTexture(tex_jtjd_cmp);
|
|
if (jc_transpose) {
|
|
if (radial)
|
|
jtjd_cam_vec32_kernel<8, bheight, true><<<grid1x, block1x>>>(
|
|
ncam, add_existing_diagc, jc.data(), jtjd.data(), jtjdi.data());
|
|
else
|
|
jtjd_cam_vec32_kernel<7, bheight, true><<<grid1x, block1x>>>(
|
|
ncam, add_existing_diagc, jc.data(), jtjd.data(), jtjdi.data());
|
|
} else {
|
|
cmlist.BindTexture(tex_jtjd_cmlist);
|
|
if (radial)
|
|
jtjd_cam_vec32_kernel<8, bheight, false><<<grid1x, block1x>>>(
|
|
ncam, add_existing_diagc, jc.data(), jtjd.data(), jtjdi.data());
|
|
else
|
|
jtjd_cam_vec32_kernel<7, bheight, false><<<grid1x, block1x>>>(
|
|
ncam, add_existing_diagc, jc.data(), jtjd.data(), jtjdi.data());
|
|
}
|
|
CheckErrorCUDA("ComputeDiagonal<Camera>");
|
|
|
|
////////////////////////////////////////////
|
|
unsigned int npoint = (pmap.GetImgWidth() - 1);
|
|
unsigned int len2 = npoint;
|
|
unsigned int bsize2 = JTJD_POINT_KWIDTH;
|
|
unsigned int nblock2 = (len2 + bsize2 - 1) / bsize2;
|
|
unsigned int bw, bh;
|
|
GetBlockConfiguration(nblock2, bw, bh);
|
|
dim3 grid2(bw, bh), block2(bsize2);
|
|
pmap.BindTexture(tex_jtjd_pmp);
|
|
|
|
if (jp.GetDataSize() > MAX_TEXSIZE) {
|
|
jp.BindTexture2(tex_jtjd_jp, tex_jtjd_jp2);
|
|
jtjd_point_kernel<2><<<grid2, block2>>>(len2, (bw * bsize2),
|
|
((float4*)jtjd.data()) + 2 * ncam,
|
|
((float4*)jtjdi.data()) + 2 * ncam);
|
|
} else {
|
|
jp.BindTexture(tex_jtjd_jp);
|
|
jtjd_point_kernel<1><<<grid2, block2>>>(len2, (bw * bsize2),
|
|
((float4*)jtjd.data()) + 2 * ncam,
|
|
((float4*)jtjdi.data()) + 2 * ncam);
|
|
}
|
|
CheckErrorCUDA("ComputeDiagonal<Point>");
|
|
}
|
|
|
|
// for each
|
|
template <bool SJ>
|
|
__global__ void jtjd_cam_q_kernel(int num, int rowsz, float* qw, float4* diag) {
|
|
int bindex = IMUL(blockIdx.x, blockDim.x) + rowsz * blockIdx.y;
|
|
int index = bindex + threadIdx.x;
|
|
if (index >= num) return;
|
|
int tid = index & 0x1;
|
|
float w = qw[index], ws = w * w * 2.0f;
|
|
if (SJ) {
|
|
float4 sj = tex1Dfetch(tex_jacobian_sj, index);
|
|
float4 dj = tid == 0 ? make_float4(sj.x * sj.x * ws, 0, 0, 0)
|
|
: make_float4(0, 0, 0, sj.w * sj.w * ws);
|
|
diag[index] = dj;
|
|
} else {
|
|
float4 dj = tid == 0 ? make_float4(ws, 0, 0, 0) : make_float4(0, 0, 0, ws);
|
|
diag[index] = dj;
|
|
}
|
|
}
|
|
|
|
void ProgramCU::ComputeDiagonalQ(CuTexImage& qlistw, CuTexImage& sj,
|
|
CuTexImage& diag) {
|
|
unsigned int bsize = 32;
|
|
unsigned int len = qlistw.GetImgWidth() * 2;
|
|
unsigned int nblock = (len + bsize - 1) / bsize;
|
|
unsigned int bw, bh;
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
if (sj.IsValid()) {
|
|
sj.BindTexture(tex_jacobian_sj);
|
|
jtjd_cam_q_kernel<true><<<grid, block>>>(len, (bw * bsize), qlistw.data(),
|
|
(float4*)diag.data());
|
|
} else {
|
|
jtjd_cam_q_kernel<false><<<grid, block>>>(len, (bw * bsize), qlistw.data(),
|
|
(float4*)diag.data());
|
|
}
|
|
CheckErrorCUDA("ComputeDiagonalQ");
|
|
}
|
|
|
|
template <int VN, int KH, bool JT>
|
|
__global__ void jtjd_cam_block_vec32_kernel(int num, float lambda1,
|
|
float lambda2, float* jc,
|
|
float* diag, float* blocks,
|
|
bool add_existing_diagc) {
|
|
__shared__ float value[KH * 32 * VN];
|
|
|
|
// 8thread per camera
|
|
int cam = blockIdx.x * KH + threadIdx.y;
|
|
int part = threadIdx.x & 0x7; // which parameter of this camera
|
|
int part2 = threadIdx.x & 0xf;
|
|
int index = threadIdx.x + (threadIdx.y << 5);
|
|
float row[8] = {0, 0, 0, 0, 0, 0, 0, 0};
|
|
if (cam < num) {
|
|
int rowpos = index - part;
|
|
// read data range for this camera
|
|
// 8 thread will do the same thing
|
|
int idx1 = tex1Dfetch(tex_jtjd_cmp, cam) << 4; // first camera
|
|
int idx2 = tex1Dfetch(tex_jtjd_cmp, cam + 1) << 4; // last camera + 1
|
|
|
|
// loop to read the index of the projection.
|
|
// so to get the location to read the jacobian
|
|
for (int i = idx1 + threadIdx.x; i < idx2; i += 32) {
|
|
if (JT) {
|
|
float temp = jc[i];
|
|
value[index] = temp;
|
|
for (int j = 0; j < VN; ++j) row[j] += (temp * value[rowpos + j]);
|
|
} else {
|
|
int ii = tex1Dfetch(tex_jtjd_cmlist, i >> 4) << 4;
|
|
float temp = jc[ii + part2];
|
|
value[index] = temp;
|
|
for (int j = 0; j < VN; ++j) row[j] += (temp * value[rowpos + j]);
|
|
}
|
|
}
|
|
}
|
|
__syncthreads();
|
|
|
|
if (cam >= num) return;
|
|
// save all the results?
|
|
for (int i = 0; i < VN; ++i) value[index * VN + i] = row[i];
|
|
int campos = threadIdx.y * (32 * VN);
|
|
for (int i = threadIdx.x; i < (VN * 16); i += 32)
|
|
value[campos + i] += value[campos + i + (16 * VN)];
|
|
for (int i = threadIdx.x; i < (VN * 8); i += 32)
|
|
value[campos + i] += value[campos + i + (8 * VN)];
|
|
|
|
if (VN == 7) {
|
|
bool zero = (part >= VN);
|
|
|
|
// write back
|
|
if (threadIdx.x < 8) {
|
|
float* dp = value + campos + threadIdx.x * (VN + 1);
|
|
float temp = zero ? 0 : dp[0];
|
|
int didx = threadIdx.x + (cam << 3);
|
|
if (add_existing_diagc) temp += diag[didx];
|
|
diag[didx] = temp;
|
|
dp[0] = lambda1 + lambda2 * temp;
|
|
}
|
|
int wpos = cam * (8 * VN) + threadIdx.x;
|
|
int rpos = campos + threadIdx.x - (threadIdx.x >> 3);
|
|
blocks[wpos] = zero ? 0 : value[rpos];
|
|
if (threadIdx.x < (VN * 8 - 32))
|
|
blocks[wpos + 32] = zero ? 0 : value[rpos + 28];
|
|
} else {
|
|
// write back
|
|
if (threadIdx.x < 8) {
|
|
float* dp = value + campos + threadIdx.x * (VN + 1);
|
|
float temp = dp[0];
|
|
int didx = threadIdx.x + (cam << 3);
|
|
if (add_existing_diagc) temp += diag[didx];
|
|
diag[didx] = temp;
|
|
dp[0] = lambda1 + lambda2 * temp; // max(, 1e-6) ;
|
|
}
|
|
int wpos = cam * (8 * VN) + threadIdx.x;
|
|
int rpos = campos + threadIdx.x;
|
|
blocks[wpos] = value[rpos];
|
|
blocks[wpos + 32] = value[rpos + 32];
|
|
}
|
|
}
|
|
|
|
#define JTJD_POINT_BLOCK_KWIDTH 64
|
|
|
|
template <int TEXN>
|
|
__global__ void jtjd_point_block_kernel(int num, int rowsz, float lambda1,
|
|
float lambda2, float4* diag,
|
|
float4* blocks) {
|
|
////////////////////////////
|
|
int index = blockIdx.x * blockDim.x + threadIdx.x + blockIdx.y * rowsz;
|
|
if (index >= num) return;
|
|
|
|
int idx1 = tex1Dfetch(tex_jtjd_pmp, index); // first camera
|
|
int idx2 = tex1Dfetch(tex_jtjd_pmp, index + 1); // last camera + 1
|
|
|
|
float M00 = 0, M01 = 0, M02 = 0, M11 = 0, M12 = 0, M22 = 0;
|
|
for (int i = idx1; i < idx2; ++i) {
|
|
if (TEXN == 2 && i > 0xffffff) {
|
|
float4 j1 = tex1Dfetch(tex_jtjd_jp2, (i & 0xffffff) << 1);
|
|
M00 += j1.x * j1.x;
|
|
M01 += j1.x * j1.y;
|
|
M02 += j1.x * j1.z;
|
|
M11 += j1.y * j1.y;
|
|
M12 += j1.y * j1.z;
|
|
M22 += j1.z * j1.z;
|
|
|
|
float4 j2 = tex1Dfetch(tex_jtjd_jp2, 1 + ((i & 0xffffff) << 1));
|
|
M00 += j2.x * j2.x;
|
|
M01 += j2.x * j2.y;
|
|
M02 += j2.x * j2.z;
|
|
M11 += j2.y * j2.y;
|
|
M12 += j2.y * j2.z;
|
|
M22 += j2.z * j2.z;
|
|
} else {
|
|
float4 j1 = tex1Dfetch(tex_jtjd_jp, i << 1);
|
|
M00 += j1.x * j1.x;
|
|
M01 += j1.x * j1.y;
|
|
M02 += j1.x * j1.z;
|
|
M11 += j1.y * j1.y;
|
|
M12 += j1.y * j1.z;
|
|
M22 += j1.z * j1.z;
|
|
|
|
float4 j2 = tex1Dfetch(tex_jtjd_jp, 1 + (i << 1));
|
|
M00 += j2.x * j2.x;
|
|
M01 += j2.x * j2.y;
|
|
M02 += j2.x * j2.z;
|
|
M11 += j2.y * j2.y;
|
|
M12 += j2.y * j2.z;
|
|
M22 += j2.z * j2.z;
|
|
}
|
|
}
|
|
|
|
diag[index] = make_float4(M00, M11, M22, 0);
|
|
|
|
M00 = lambda2 * M00 + lambda1;
|
|
M11 = lambda2 * M11 + lambda1;
|
|
M22 = lambda2 * M22 + lambda1;
|
|
|
|
// invert the 3x3 matrix.
|
|
float det = (M00 * M11 - M01 * M01) * M22 + 2.0 * M01 * M12 * M02 -
|
|
M02 * M02 * M11 - M12 * M12 * M00;
|
|
if (det >= FLT_MAX || det <= FLT_MIN * 2.0f) {
|
|
int write_pos = index * 3;
|
|
blocks[write_pos] = make_float4(0, 0, 0, 0);
|
|
blocks[write_pos + 1] = make_float4(0, 0, 0, 0);
|
|
blocks[write_pos + 2] = make_float4(0, 0, 0, 0);
|
|
} else {
|
|
float m00 = (M11 * M22 - M12 * M12) / det;
|
|
float m01 = -(M01 * M22 - M12 * M02) / det;
|
|
float m02 = (M01 * M12 - M02 * M11) / det;
|
|
int write_pos = index * 3;
|
|
blocks[write_pos] = make_float4(m00, m01, m02, 0);
|
|
|
|
float m11 = (M00 * M22 - M02 * M02) / det;
|
|
float m12 = -(M00 * M12 - M01 * M02) / det;
|
|
blocks[write_pos + 1] = make_float4(m01, m11, m12, 0);
|
|
|
|
float m22 = (M00 * M11 - M01 * M01) / det;
|
|
blocks[write_pos + 2] = make_float4(m02, m12, m22, 0);
|
|
}
|
|
}
|
|
|
|
#define JTJD_BLOCK_CAM_INVERT_KWIDTH 64
|
|
template <int VN>
|
|
__global__ void jtjd_cam_block_invert_kernel(int num, float4* blocks) {
|
|
// N / 8 cameras...each have 64 floats,,,, N * 8 float
|
|
// each will read 8 float......
|
|
__shared__ float value[JTJD_BLOCK_CAM_INVERT_KWIDTH * VN];
|
|
__shared__ bool invalid[JTJD_BLOCK_CAM_INVERT_KWIDTH / 8];
|
|
//////////////////////////////////////////////
|
|
|
|
int bindex = IMUL(blockIdx.x, blockDim.x);
|
|
int index = bindex + threadIdx.x;
|
|
int block_read_pos = IMUL(bindex, VN);
|
|
for (int i = 0; i < JTJD_BLOCK_CAM_INVERT_KWIDTH * VN;
|
|
i += JTJD_BLOCK_CAM_INVERT_KWIDTH)
|
|
value[threadIdx.x + i] = ((float*)blocks)[block_read_pos + threadIdx.x + i];
|
|
__syncthreads();
|
|
const int cam_id = threadIdx.x >> 3;
|
|
const int cam_pos = IMUL(cam_id, VN * 8);
|
|
const int col = threadIdx.x & 0x7, rowj_pos = col << 3;
|
|
; //
|
|
|
|
float* a = value + cam_pos;
|
|
for (int i = 0; i < VN; ++i) {
|
|
int rowi_pos = i << 3, dpos = i + rowi_pos;
|
|
if (col == i && a[dpos] > 0) a[dpos] = rsqrt(a[dpos]);
|
|
__syncthreads();
|
|
float diag = a[dpos];
|
|
if (diag == 0 || col >= VN) continue;
|
|
if (col < i) {
|
|
a[rowi_pos + col] = 0;
|
|
} else if (col > i) {
|
|
float aij = a[rowi_pos + col] * diag;
|
|
a[rowi_pos + col] = aij;
|
|
for (int k = col; k < VN; ++k) a[rowj_pos + k] -= a[rowi_pos + k] * aij;
|
|
}
|
|
}
|
|
|
|
if (index >= num) return;
|
|
|
|
if (col == 0) invalid[cam_id] = false;
|
|
if (col < VN) {
|
|
for (int i = 1; i < VN; ++i) {
|
|
int rowi_pos = i << 3, dpos = i + rowi_pos;
|
|
if (a[dpos] == 0) continue;
|
|
if (col < i) {
|
|
float sum = 0;
|
|
for (int k = col; k < i; ++k)
|
|
sum += (a[(k << 3) + i] * a[rowj_pos + k]);
|
|
a[rowj_pos + i] = -sum * a[dpos];
|
|
}
|
|
}
|
|
float ai[8], amax = 0;
|
|
for (int i = 0; i < VN * 8; i += 8) {
|
|
float sum = 0;
|
|
for (int k = 0; k < VN; k++) sum += a[rowj_pos + k] * a[i + k];
|
|
ai[i >> 3] = sum;
|
|
amax = max(amax, sum);
|
|
}
|
|
|
|
if (isinf(amax)) invalid[cam_id] = true;
|
|
int write_pos = IMUL((index >> 3), (VN * 2)) + (col << 1);
|
|
if (invalid[cam_id]) // a better way would be using a threshold
|
|
{
|
|
blocks[write_pos] = make_float4(0, 0, 0, 0);
|
|
blocks[write_pos + 1] = make_float4(0, 0, 0, 0);
|
|
} else {
|
|
blocks[write_pos] = make_float4(ai[0], ai[1], ai[2], ai[3]);
|
|
blocks[write_pos + 1] =
|
|
make_float4(ai[4], ai[5], ai[6], VN < 8 ? 0 : ai[7]);
|
|
}
|
|
}
|
|
}
|
|
|
|
void ProgramCU::ComputeDiagonalBlock(float lambda, bool dampd, CuTexImage& jc,
|
|
CuTexImage& cmap, CuTexImage& jp,
|
|
CuTexImage& pmap, CuTexImage& cmlist,
|
|
CuTexImage& diag, CuTexImage& blocks,
|
|
int radial_distortion, bool jc_transpose,
|
|
bool add_existing_diagc, int mode) {
|
|
size_t szjc = jc.GetDataSize();
|
|
unsigned int ncam = (cmap.GetImgWidth() - 1); // how many cameras
|
|
float lambda1 = dampd ? 0.0f : lambda;
|
|
float lambda2 = dampd ? (1.0f + lambda) : 1.0f;
|
|
const unsigned int bheight = 2;
|
|
dim3 block1x(32, bheight), grid1x((ncam + bheight - 1) / bheight);
|
|
cmap.BindTexture(tex_jtjd_cmp);
|
|
|
|
if (mode == 2) {
|
|
// point only mode?
|
|
} else if (radial_distortion) {
|
|
if (jc_transpose) {
|
|
jtjd_cam_block_vec32_kernel<8, bheight, true><<<grid1x, block1x>>>(
|
|
ncam, lambda1, lambda2, jc.data(), diag.data(), blocks.data(),
|
|
add_existing_diagc);
|
|
} else {
|
|
cmlist.BindTexture(tex_jtjd_cmlist);
|
|
jtjd_cam_block_vec32_kernel<8, bheight, false><<<grid1x, block1x>>>(
|
|
ncam, lambda1, lambda2, jc.data(), diag.data(), blocks.data(),
|
|
add_existing_diagc);
|
|
}
|
|
} else {
|
|
if (jc_transpose) {
|
|
jtjd_cam_block_vec32_kernel<7, bheight, true><<<grid1x, block1x>>>(
|
|
ncam, lambda1, lambda2, jc.data(), diag.data(), blocks.data(),
|
|
add_existing_diagc);
|
|
} else {
|
|
cmlist.BindTexture(tex_jtjd_cmlist);
|
|
jtjd_cam_block_vec32_kernel<7, bheight, false><<<grid1x, block1x>>>(
|
|
ncam, lambda1, lambda2, jc.data(), diag.data(), blocks.data(),
|
|
add_existing_diagc);
|
|
}
|
|
}
|
|
CheckErrorCUDA("ComputeDiagonalBlock<Camera>");
|
|
|
|
////////////////////////////////////////////
|
|
unsigned int npoint = (pmap.GetImgWidth() - 1);
|
|
unsigned int len2 = npoint;
|
|
unsigned int bsize2 = JTJD_POINT_BLOCK_KWIDTH;
|
|
unsigned int nblock2 = (len2 + bsize2 - 1) / bsize2;
|
|
unsigned int bw, bh;
|
|
unsigned int offsetd = 2 * ncam;
|
|
unsigned int offsetb = (radial_distortion ? 16 : 14) * ncam;
|
|
GetBlockConfiguration(nblock2, bw, bh);
|
|
dim3 grid2(bw, bh), block2(bsize2);
|
|
pmap.BindTexture(tex_jtjd_pmp);
|
|
if (mode == 1) {
|
|
// camera only mode?
|
|
} else if (jp.GetDataSize() > MAX_TEXSIZE) {
|
|
jp.BindTexture2(tex_jtjd_jp, tex_jtjd_jp2);
|
|
jtjd_point_block_kernel<2><<<grid2, block2>>>(
|
|
len2, (bw * bsize2), lambda1, lambda2, ((float4*)diag.data()) + offsetd,
|
|
((float4*)blocks.data()) + offsetb);
|
|
} else {
|
|
jp.BindTexture(tex_jtjd_jp);
|
|
jtjd_point_block_kernel<1><<<grid2, block2>>>(
|
|
len2, (bw * bsize2), lambda1, lambda2, ((float4*)diag.data()) + offsetd,
|
|
((float4*)blocks.data()) + offsetb);
|
|
}
|
|
CheckErrorCUDA("ComputeDiagonalBlock<Point>");
|
|
|
|
if (mode != 2) {
|
|
unsigned int len3 = ncam * 8;
|
|
unsigned int bsize3 = JTJD_BLOCK_CAM_INVERT_KWIDTH;
|
|
unsigned int nblock3 = (len3 + bsize3 - 1) / bsize3;
|
|
dim3 grid3(nblock3), block3(bsize3);
|
|
if (radial_distortion)
|
|
jtjd_cam_block_invert_kernel<8><<<grid3, block3>>>(
|
|
len3, (float4*)blocks.data());
|
|
else
|
|
jtjd_cam_block_invert_kernel<7><<<grid3, block3>>>(
|
|
len3, (float4*)blocks.data());
|
|
CheckErrorCUDA("ComputeDiagonalBlockInverse<Camera>");
|
|
}
|
|
}
|
|
|
|
template <int WIDTH, int BBIT, int VSZ>
|
|
__global__ void multiply_block_conditioner_kernel(int num, int rowsz,
|
|
float* blocks, float* x,
|
|
float* result) {
|
|
__shared__ float mat[WIDTH * VSZ];
|
|
__shared__ float val[WIDTH];
|
|
const int BSZ = 1 << BBIT;
|
|
const int BMASK = BSZ - 1;
|
|
int bindex = IMUL(blockIdx.x, blockDim.x) + rowsz * blockIdx.y;
|
|
int index = bindex + threadIdx.x;
|
|
int block_read_pos = bindex * VSZ;
|
|
val[threadIdx.x] = x[index];
|
|
for (int i = 0; i < VSZ * WIDTH; i += WIDTH)
|
|
mat[i + threadIdx.x] = blocks[i + block_read_pos + threadIdx.x];
|
|
__syncthreads();
|
|
if (index >= num) return;
|
|
float* ac = mat + (threadIdx.x >> BBIT) * (BSZ * VSZ) + (threadIdx.x & BMASK);
|
|
float* xc = val + (threadIdx.x & (~BMASK));
|
|
float sum = 0;
|
|
for (int i = 0; i < VSZ; ++i) sum += ac[i << BBIT] * xc[i];
|
|
result[index] = sum; // isinf(sum) ? 0 : sum ; //
|
|
}
|
|
|
|
void ProgramCU::MultiplyBlockConditioner(int ncam, int npoint,
|
|
CuTexImage& blocks, CuTexImage& vector,
|
|
CuTexImage& result, int radial,
|
|
int mode) {
|
|
const unsigned int bsize1 = 64;
|
|
unsigned int bw, bh;
|
|
|
|
if (mode != 2) {
|
|
unsigned int len1 = ncam * 8;
|
|
unsigned int nblock1 = (len1 + bsize1 - 1) / bsize1;
|
|
GetBlockConfiguration(nblock1, bw, bh);
|
|
dim3 grid1(bw, bh), block1(bsize1);
|
|
if (radial)
|
|
multiply_block_conditioner_kernel<bsize1, 3, 8><<<grid1, block1>>>(
|
|
len1, (bw * bsize1), blocks.data(), vector.data(), result.data());
|
|
else
|
|
multiply_block_conditioner_kernel<bsize1, 3, 7><<<grid1, block1>>>(
|
|
len1, (bw * bsize1), blocks.data(), vector.data(), result.data());
|
|
CheckErrorCUDA("MultiplyBlockConditioner<Camera>");
|
|
}
|
|
|
|
if (mode != 1) {
|
|
const unsigned int bsize2 = 128;
|
|
unsigned int len2 = npoint * 4;
|
|
unsigned int nblock2 = (len2 + bsize2 - 1) / bsize2;
|
|
unsigned int cbsz = radial ? 64 : 56;
|
|
unsigned int offsetb = ncam * cbsz;
|
|
unsigned int offsetd = ncam * 8;
|
|
GetBlockConfiguration(nblock2, bw, bh);
|
|
dim3 grid2(bw, bh), block2(bsize2);
|
|
multiply_block_conditioner_kernel<bsize2, 2, 3><<<grid2, block2>>>(
|
|
len2, (bw * bsize2), blocks.data() + offsetb, vector.data() + offsetd,
|
|
result.data() + offsetd);
|
|
CheckErrorCUDA("MultiplyBlockConditioner<Point>");
|
|
}
|
|
}
|
|
|
|
texture<float4, 1, cudaReadModeElementType> tex_shuffle_jc;
|
|
texture<int, 1, cudaReadModeElementType> tex_shuffle_map;
|
|
texture<float4, 1, cudaReadModeElementType> tex_shuffle_jc2;
|
|
template <int TEXN>
|
|
__global__ void shuffle_camera_jacobian_kernel(int num, int bwidth,
|
|
float4* jc) {
|
|
int index = threadIdx.x + blockIdx.x * blockDim.x + blockIdx.y * bwidth;
|
|
if (index >= num) return;
|
|
int fetch_idx = tex1Dfetch(tex_shuffle_map, index >> 2);
|
|
if (TEXN == 2) {
|
|
int texidx = fetch_idx >> 23,
|
|
fidx = ((fetch_idx & 0x7fffff) << 2) + (index & 0x3);
|
|
if (texidx == 0)
|
|
jc[index] = tex1Dfetch(tex_shuffle_jc, fidx);
|
|
else if (texidx == 1)
|
|
jc[index] = tex1Dfetch(tex_shuffle_jc2, fidx);
|
|
}
|
|
if (TEXN == 1) {
|
|
jc[index] = tex1Dfetch(tex_shuffle_jc, (fetch_idx << 2) + (index & 0x3));
|
|
}
|
|
}
|
|
|
|
bool ProgramCU::ShuffleCameraJacobian(CuTexImage& jc, CuTexImage& map,
|
|
CuTexImage& result) {
|
|
if (!result.IsValid()) return false;
|
|
size_t szjc = jc.GetDataSize();
|
|
unsigned int len = map.GetImgWidth() * 4;
|
|
unsigned int bsize = 128;
|
|
unsigned int nblock = (len + bsize - 1) / bsize;
|
|
|
|
map.BindTexture(tex_shuffle_map);
|
|
|
|
if (szjc > 2 * MAX_TEXSIZE) {
|
|
fprintf(stderr, "datasize way too big %lX, %lX+...\n", szjc,
|
|
(szjc) / MAX_TEXSIZE);
|
|
return false;
|
|
} else if (szjc > MAX_TEXSIZE) {
|
|
unsigned int bw, bh;
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
jc.BindTexture2(tex_shuffle_jc, tex_shuffle_jc2);
|
|
shuffle_camera_jacobian_kernel<2><<<grid, block>>>(len, (bw * bsize),
|
|
(float4*)result.data());
|
|
} else {
|
|
jc.BindTexture(tex_shuffle_jc);
|
|
unsigned int bw, bh;
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
shuffle_camera_jacobian_kernel<1><<<grid, block>>>(len, (bw * bsize),
|
|
(float4*)result.data());
|
|
}
|
|
CheckErrorCUDA("ShuffleCameraJacobian");
|
|
return true;
|
|
}
|
|
|
|
texture<float4, 1, cudaReadModeElementType> tex_mjx_jc;
|
|
texture<float4, 1, cudaReadModeElementType> tex_mjx_jc2;
|
|
texture<float4, 1, cudaReadModeElementType> tex_mjx_jc3;
|
|
texture<float4, 1, cudaReadModeElementType> tex_mjx_jc4;
|
|
texture<float4, 1, cudaReadModeElementType> tex_mjx_jp;
|
|
texture<float4, 1, cudaReadModeElementType> tex_mjx_jp2;
|
|
texture<int2, 1, cudaReadModeElementType> tex_mjx_idx;
|
|
texture<float4, 1, cudaReadModeElementType> tex_mjx_x;
|
|
|
|
template <int TEXN>
|
|
__global__ void multiply_jx_kernel(int num, int bwidth, int offset,
|
|
float* result) {
|
|
int index = threadIdx.x + blockIdx.x * blockDim.x + blockIdx.y * bwidth;
|
|
if (index >= num) return;
|
|
|
|
if (TEXN == 4 && (index >> 24) == 3) {
|
|
////////////////////////////////////////////
|
|
int2 proj = tex1Dfetch(tex_mjx_idx, index >> 1);
|
|
float4 xc1 = tex1Dfetch(tex_mjx_x, proj.x);
|
|
float4 xc2 = tex1Dfetch(tex_mjx_x, proj.x + 1);
|
|
float4 xp = tex1Dfetch(tex_mjx_x, proj.y + offset);
|
|
|
|
////////////////////////////////////////////
|
|
float4 jp, jc1, jc2;
|
|
jp = tex1Dfetch(tex_mjx_jp2, index & 0x1ffffff);
|
|
jc1 = tex1Dfetch(tex_mjx_jc4, (index & 0xffffff) << 1);
|
|
jc2 = tex1Dfetch(tex_mjx_jc4, ((index & 0xffffff) << 1) + 1);
|
|
|
|
/////////////////////////////////////
|
|
result[index] = jc1.x * xc1.x + jc1.y * xc1.y + jc1.z * xc1.z +
|
|
jc1.w * xc1.w + jc2.x * xc2.x + jc2.y * xc2.y +
|
|
jc2.z * xc2.z + jc2.w * xc2.w + jp.x * xp.x + jp.y * xp.y +
|
|
jp.z * xp.z;
|
|
} else if (TEXN > 2 && (index >> 24) == 2) {
|
|
////////////////////////////////////////////
|
|
int2 proj = tex1Dfetch(tex_mjx_idx, index >> 1);
|
|
float4 xc1 = tex1Dfetch(tex_mjx_x, proj.x);
|
|
float4 xc2 = tex1Dfetch(tex_mjx_x, proj.x + 1);
|
|
float4 xp = tex1Dfetch(tex_mjx_x, proj.y + offset);
|
|
|
|
////////////////////////////////////////////
|
|
float4 jp, jc1, jc2;
|
|
jp = tex1Dfetch(tex_mjx_jp2, index & 0x1ffffff);
|
|
jc1 = tex1Dfetch(tex_mjx_jc3, (index & 0xffffff) << 1);
|
|
jc2 = tex1Dfetch(tex_mjx_jc3, ((index & 0xffffff) << 1) + 1);
|
|
|
|
/////////////////////////////////////
|
|
result[index] = jc1.x * xc1.x + jc1.y * xc1.y + jc1.z * xc1.z +
|
|
jc1.w * xc1.w + jc2.x * xc2.x + jc2.y * xc2.y +
|
|
jc2.z * xc2.z + jc2.w * xc2.w + jp.x * xp.x + jp.y * xp.y +
|
|
jp.z * xp.z;
|
|
} else if (TEXN > 1 && (index > 0xffffff)) {
|
|
////////////////////////////////////////////
|
|
int2 proj = tex1Dfetch(tex_mjx_idx, index >> 1);
|
|
float4 xc1 = tex1Dfetch(tex_mjx_x, proj.x);
|
|
float4 xc2 = tex1Dfetch(tex_mjx_x, proj.x + 1);
|
|
float4 xp = tex1Dfetch(tex_mjx_x, proj.y + offset);
|
|
|
|
////////////////////////////////////////////
|
|
float4 jp, jc1, jc2;
|
|
jp = tex1Dfetch(tex_mjx_jp, index & 0x1ffffff);
|
|
jc1 = tex1Dfetch(tex_mjx_jc2, (index & 0xffffff) << 1);
|
|
jc2 = tex1Dfetch(tex_mjx_jc2, ((index & 0xffffff) << 1) + 1);
|
|
|
|
/////////////////////////////////////
|
|
result[index] = jc1.x * xc1.x + jc1.y * xc1.y + jc1.z * xc1.z +
|
|
jc1.w * xc1.w + jc2.x * xc2.x + jc2.y * xc2.y +
|
|
jc2.z * xc2.z + jc2.w * xc2.w + jp.x * xp.x + jp.y * xp.y +
|
|
jp.z * xp.z;
|
|
} else {
|
|
////////////////////////////////////////////
|
|
int2 proj = tex1Dfetch(tex_mjx_idx, index >> 1);
|
|
float4 xc1 = tex1Dfetch(tex_mjx_x, proj.x);
|
|
float4 xc2 = tex1Dfetch(tex_mjx_x, proj.x + 1);
|
|
float4 xp = tex1Dfetch(tex_mjx_x, proj.y + offset);
|
|
|
|
////////////////////////////////////////////
|
|
float4 jp, jc1, jc2;
|
|
jp = tex1Dfetch(tex_mjx_jp, index);
|
|
jc1 = tex1Dfetch(tex_mjx_jc, index << 1);
|
|
jc2 = tex1Dfetch(tex_mjx_jc, (index << 1) + 1);
|
|
|
|
/////////////////////////////////////
|
|
result[index] = jc1.x * xc1.x + jc1.y * xc1.y + jc1.z * xc1.z +
|
|
jc1.w * xc1.w + jc2.x * xc2.x + jc2.y * xc2.y +
|
|
jc2.z * xc2.z + jc2.w * xc2.w + jp.x * xp.x + jp.y * xp.y +
|
|
jp.z * xp.z;
|
|
}
|
|
}
|
|
|
|
template <int TEXN>
|
|
__global__ void multiply_jcx_kernel(int num, int bwidth, float* result) {
|
|
int index = threadIdx.x + blockIdx.x * blockDim.x + blockIdx.y * bwidth;
|
|
if (index >= num) return;
|
|
|
|
if (TEXN == 4 && (index >> 24) == 3) {
|
|
////////////////////////////////////////////
|
|
int2 proj = tex1Dfetch(tex_mjx_idx, index >> 1);
|
|
float4 xc1 = tex1Dfetch(tex_mjx_x, proj.x);
|
|
float4 xc2 = tex1Dfetch(tex_mjx_x, proj.x + 1);
|
|
|
|
////////////////////////////////////////////
|
|
float4 jc1, jc2;
|
|
jc1 = tex1Dfetch(tex_mjx_jc4, (index & 0xffffff) << 1);
|
|
jc2 = tex1Dfetch(tex_mjx_jc4, ((index & 0xffffff) << 1) + 1);
|
|
|
|
/////////////////////////////////////
|
|
result[index] = jc1.x * xc1.x + jc1.y * xc1.y + jc1.z * xc1.z +
|
|
jc1.w * xc1.w + jc2.x * xc2.x + jc2.y * xc2.y +
|
|
jc2.z * xc2.z + jc2.w * xc2.w;
|
|
} else if (TEXN > 2 && (index >> 24) == 2) {
|
|
////////////////////////////////////////////
|
|
int2 proj = tex1Dfetch(tex_mjx_idx, index >> 1);
|
|
float4 xc1 = tex1Dfetch(tex_mjx_x, proj.x);
|
|
float4 xc2 = tex1Dfetch(tex_mjx_x, proj.x + 1);
|
|
|
|
////////////////////////////////////////////
|
|
float4 jc1, jc2;
|
|
jc1 = tex1Dfetch(tex_mjx_jc3, (index & 0xffffff) << 1);
|
|
jc2 = tex1Dfetch(tex_mjx_jc3, ((index & 0xffffff) << 1) + 1);
|
|
|
|
/////////////////////////////////////
|
|
result[index] = jc1.x * xc1.x + jc1.y * xc1.y + jc1.z * xc1.z +
|
|
jc1.w * xc1.w + jc2.x * xc2.x + jc2.y * xc2.y +
|
|
jc2.z * xc2.z + jc2.w * xc2.w;
|
|
} else if (TEXN > 1 && (index > 0xffffff)) {
|
|
////////////////////////////////////////////
|
|
int2 proj = tex1Dfetch(tex_mjx_idx, index >> 1);
|
|
float4 xc1 = tex1Dfetch(tex_mjx_x, proj.x);
|
|
float4 xc2 = tex1Dfetch(tex_mjx_x, proj.x + 1);
|
|
|
|
////////////////////////////////////////////
|
|
float4 jc1, jc2;
|
|
jc1 = tex1Dfetch(tex_mjx_jc2, (index & 0xffffff) << 1);
|
|
jc2 = tex1Dfetch(tex_mjx_jc2, ((index & 0xffffff) << 1) + 1);
|
|
|
|
/////////////////////////////////////
|
|
result[index] = jc1.x * xc1.x + jc1.y * xc1.y + jc1.z * xc1.z +
|
|
jc1.w * xc1.w + jc2.x * xc2.x + jc2.y * xc2.y +
|
|
jc2.z * xc2.z + jc2.w * xc2.w;
|
|
} else {
|
|
////////////////////////////////////////////
|
|
int2 proj = tex1Dfetch(tex_mjx_idx, index >> 1);
|
|
float4 xc1 = tex1Dfetch(tex_mjx_x, proj.x);
|
|
float4 xc2 = tex1Dfetch(tex_mjx_x, proj.x + 1);
|
|
|
|
////////////////////////////////////////////
|
|
float4 jc1, jc2;
|
|
jc1 = tex1Dfetch(tex_mjx_jc, index << 1);
|
|
jc2 = tex1Dfetch(tex_mjx_jc, (index << 1) + 1);
|
|
|
|
/////////////////////////////////////
|
|
result[index] = jc1.x * xc1.x + jc1.y * xc1.y + jc1.z * xc1.z +
|
|
jc1.w * xc1.w + jc2.x * xc2.x + jc2.y * xc2.y +
|
|
jc2.z * xc2.z + jc2.w * xc2.w;
|
|
}
|
|
}
|
|
|
|
template <int TEXN>
|
|
__global__ void multiply_jpx_kernel(int num, int bwidth, int offset,
|
|
float* result) {
|
|
int index = threadIdx.x + blockIdx.x * blockDim.x + blockIdx.y * bwidth;
|
|
if (index >= num) return;
|
|
|
|
if (TEXN == 2 && index > 0x1ffffff) {
|
|
////////////////////////////////////////////
|
|
int2 proj = tex1Dfetch(tex_mjx_idx, index >> 1);
|
|
float4 xp = tex1Dfetch(tex_mjx_x, proj.y + offset);
|
|
////////////////////////////////////////////
|
|
float4 jp = tex1Dfetch(tex_mjx_jp2, index & 0x1ffffff);
|
|
/////////////////////////////////////
|
|
result[index] = jp.x * xp.x + jp.y * xp.y + jp.z * xp.z;
|
|
} else {
|
|
////////////////////////////////////////////
|
|
int2 proj = tex1Dfetch(tex_mjx_idx, index >> 1);
|
|
float4 xp = tex1Dfetch(tex_mjx_x, proj.y + offset);
|
|
|
|
////////////////////////////////////////////
|
|
float4 jp = tex1Dfetch(tex_mjx_jp, index);
|
|
/////////////////////////////////////
|
|
result[index] = jp.x * xp.x + jp.y * xp.y + jp.z * xp.z;
|
|
}
|
|
}
|
|
|
|
template <int KW>
|
|
__global__ void multiply_jx_notex2_kernel(int num, int bwidth, int offset,
|
|
float* jcx, float* jpx,
|
|
float* result) {
|
|
int bindex = blockIdx.x * blockDim.x + blockIdx.y * bwidth;
|
|
int index = threadIdx.x + bindex;
|
|
|
|
////////////////////////////////////////////
|
|
int2 proj = tex1Dfetch(tex_mjx_idx, index >> 1);
|
|
float4 xc1 = tex1Dfetch(tex_mjx_x, proj.x);
|
|
float4 xc2 = tex1Dfetch(tex_mjx_x, proj.x + 1);
|
|
float4 xp = tex1Dfetch(tex_mjx_x, proj.y + offset);
|
|
////////////////////////////////////////////
|
|
__shared__ float jps[KW * 4];
|
|
__shared__ float jcs[KW * 8];
|
|
|
|
for (int i = threadIdx.x; i < 4 * KW; i += KW)
|
|
jps[i] = jpx[(bindex << 2) + i];
|
|
for (int i = threadIdx.x; i < 8 * KW; i += KW)
|
|
jcs[i] = jcx[(bindex << 3) + i];
|
|
|
|
__syncthreads();
|
|
if (index >= num) return;
|
|
|
|
/////////////////////////////////////
|
|
float *jp = jps + threadIdx.x * 4, *jc = jcs + threadIdx.x * 8;
|
|
result[index] = jc[0] * xc1.x + jc[1] * xc1.y + jc[2] * xc1.z +
|
|
jc[3] * xc1.w + jc[4] * xc2.x + jc[5] * xc2.y +
|
|
jc[6] * xc2.z + jc[7] * xc2.w + jp[0] * xp.x + jp[1] * xp.y +
|
|
jp[2] * xp.z;
|
|
}
|
|
|
|
template <int KW>
|
|
__global__ void multiply_jpx_notex2_kernel(int num, int bwidth, int offset,
|
|
float* jpx, float* result) {
|
|
int bindex = blockIdx.x * blockDim.x + blockIdx.y * bwidth;
|
|
int index = threadIdx.x + bindex;
|
|
|
|
////////////////////////////////////////////
|
|
int2 proj = tex1Dfetch(tex_mjx_idx, index >> 1);
|
|
float4 xp = tex1Dfetch(tex_mjx_x, proj.y + offset);
|
|
////////////////////////////////////////////
|
|
__shared__ float jps[KW * 4];
|
|
|
|
for (int i = threadIdx.x; i < 4 * KW; i += KW)
|
|
jps[i] = jpx[(bindex << 2) + i];
|
|
|
|
__syncthreads();
|
|
if (index >= num) return;
|
|
|
|
/////////////////////////////////////
|
|
float* jp = jps + threadIdx.x * 4;
|
|
result[index] = jp[0] * xp.x + jp[1] * xp.y + jp[2] * xp.z;
|
|
}
|
|
|
|
template <int KW>
|
|
__global__ void multiply_jcx_notex2_kernel(int num, int bwidth, float* jcx,
|
|
float* result) {
|
|
int bindex = blockIdx.x * blockDim.x + blockIdx.y * bwidth;
|
|
int index = threadIdx.x + bindex;
|
|
|
|
////////////////////////////////////////////
|
|
int2 proj = tex1Dfetch(tex_mjx_idx, index >> 1);
|
|
float4 xc1 = tex1Dfetch(tex_mjx_x, proj.x);
|
|
float4 xc2 = tex1Dfetch(tex_mjx_x, proj.x + 1);
|
|
////////////////////////////////////////////
|
|
|
|
__shared__ float jcs[KW * 8];
|
|
for (int i = threadIdx.x; i < 8 * KW; i += KW)
|
|
jcs[i] = jcx[(bindex << 3) + i];
|
|
|
|
__syncthreads();
|
|
if (index >= num) return;
|
|
|
|
/////////////////////////////////////
|
|
float* jc = jcs + threadIdx.x * 8;
|
|
result[index] = jc[0] * xc1.x + jc[1] * xc1.y + jc[2] * xc1.z +
|
|
jc[3] * xc1.w + jc[4] * xc2.x + jc[5] * xc2.y +
|
|
jc[6] * xc2.z + jc[7] * xc2.w;
|
|
}
|
|
|
|
void ProgramCU::ComputeJX(int point_offset, CuTexImage& x, CuTexImage& jc,
|
|
CuTexImage& jp, CuTexImage& jmap, CuTexImage& result,
|
|
int mode) {
|
|
// given a vector of parameters....
|
|
// multiply the Jacobian Matrix with it [jc jp] * p
|
|
// for each measurment, read back the jacobian
|
|
// multiply and summ up th corresponding
|
|
|
|
unsigned int nproj = jmap.GetImgWidth();
|
|
unsigned int len = nproj * 2;
|
|
unsigned int bsize = 64;
|
|
unsigned int nblock = (len + bsize - 1) / bsize;
|
|
unsigned int bw, bh;
|
|
jmap.BindTexture(tex_mjx_idx);
|
|
x.BindTexture(tex_mjx_x);
|
|
|
|
if (mode == 0) {
|
|
size_t szjc = jc.GetDataSize();
|
|
if (TEX_TOOBIG4(szjc)) {
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
multiply_jx_notex2_kernel<64><<<grid, block>>>(
|
|
len, (bw * bsize), point_offset, jc.data(), jp.data(), result.data());
|
|
} else if (szjc > 2 * MAX_TEXSIZE) {
|
|
jp.BindTexture2(tex_mjx_jp, tex_mjx_jp2);
|
|
jc.BindTexture4(tex_mjx_jc, tex_mjx_jc2, tex_mjx_jc3, tex_mjx_jc4);
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
multiply_jx_kernel<4><<<grid, block>>>(len, (bw * bsize), point_offset,
|
|
result.data());
|
|
} else if (szjc > MAX_TEXSIZE) {
|
|
jp.BindTexture(tex_mjx_jp);
|
|
jc.BindTexture2(tex_mjx_jc, tex_mjx_jc2);
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
multiply_jx_kernel<2><<<grid, block>>>(len, (bw * bsize), point_offset,
|
|
result.data());
|
|
} else {
|
|
jp.BindTexture(tex_mjx_jp);
|
|
jc.BindTexture(tex_mjx_jc);
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bh, bw), block(bsize);
|
|
multiply_jx_kernel<1><<<grid, block>>>(len, (bh * bsize), point_offset,
|
|
result.data());
|
|
}
|
|
CheckErrorCUDA("ComputeJX");
|
|
} else if (mode == 1) {
|
|
size_t szjc = jc.GetDataSize();
|
|
if (TEX_TOOBIG4(szjc)) {
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
multiply_jcx_notex2_kernel<64><<<grid, block>>>(len, (bw * bsize),
|
|
jc.data(), result.data());
|
|
} else if (szjc > 2 * MAX_TEXSIZE) {
|
|
jc.BindTexture4(tex_mjx_jc, tex_mjx_jc2, tex_mjx_jc3, tex_mjx_jc4);
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
multiply_jcx_kernel<4><<<grid, block>>>(len, (bw * bsize), result.data());
|
|
} else if (szjc > MAX_TEXSIZE) {
|
|
jc.BindTexture2(tex_mjx_jc, tex_mjx_jc2);
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
multiply_jcx_kernel<2><<<grid, block>>>(len, (bw * bsize), result.data());
|
|
} else {
|
|
jc.BindTexture(tex_mjx_jc);
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bh, bw), block(bsize);
|
|
multiply_jcx_kernel<1><<<grid, block>>>(len, (bh * bsize), result.data());
|
|
}
|
|
CheckErrorCUDA("ComputeJCX");
|
|
} else if (mode == 2) {
|
|
size_t szjp = jp.GetDataSize();
|
|
if (szjp > MAX_TEXSIZE) {
|
|
jp.BindTexture(tex_mjx_jp);
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
multiply_jpx_kernel<2><<<grid, block>>>(len, (bw * bsize), point_offset,
|
|
result.data());
|
|
} else {
|
|
jp.BindTexture(tex_mjx_jp);
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bh, bw), block(bsize);
|
|
multiply_jpx_kernel<1><<<grid, block>>>(len, (bh * bsize), point_offset,
|
|
result.data());
|
|
}
|
|
CheckErrorCUDA("ComputeJPX");
|
|
}
|
|
}
|
|
|
|
template <bool md, bool pd>
|
|
__device__ void jacobian_internal(int camera_pos, int pt_pos, int tidx,
|
|
float* r, float jic, float* jxc, float* jyc,
|
|
float* jxp, float* jyp) {
|
|
float m[3];
|
|
float4 ft = tex1Dfetch(tex_jacobian_cam, camera_pos);
|
|
float4 r1 = tex1Dfetch(tex_jacobian_cam, camera_pos + 1);
|
|
r[0] = r1.x;
|
|
r[1] = r1.y;
|
|
r[2] = r1.z;
|
|
r[3] = r1.w;
|
|
float4 r2 = tex1Dfetch(tex_jacobian_cam, camera_pos + 2);
|
|
r[4] = r2.x;
|
|
r[5] = r2.y;
|
|
r[6] = r2.z;
|
|
r[7] = r2.w;
|
|
float4 r3 = tex1Dfetch(tex_jacobian_cam, camera_pos + 3);
|
|
r[8] = r3.x;
|
|
|
|
float4 temp = tex1Dfetch(tex_jacobian_pts, pt_pos);
|
|
m[0] = temp.x;
|
|
m[1] = temp.y;
|
|
m[2] = temp.z;
|
|
|
|
float x0 = r[0] * m[0] + r[1] * m[1] + r[2] * m[2];
|
|
float y0 = r[3] * m[0] + r[4] * m[1] + r[5] * m[2];
|
|
float z0 = r[6] * m[0] + r[7] * m[1] + r[8] * m[2];
|
|
float f_p2 = FDIV(ft.x, z0 + ft.w);
|
|
float p0_p2 = FDIV(x0 + ft.y, z0 + ft.w);
|
|
float p1_p2 = FDIV(y0 + ft.z, z0 + ft.w);
|
|
|
|
if (pd) {
|
|
float rr1 = r3.y * p0_p2 * p0_p2;
|
|
float rr2 = r3.y * p1_p2 * p1_p2;
|
|
float f_p2_x = f_p2 * (1.0 + 3.0 * rr1 + rr2);
|
|
float f_p2_y = f_p2 * (1.0 + 3.0 * rr2 + rr1);
|
|
|
|
JACOBIAN_SET_JC_BEGIN
|
|
float jfc = jic * (1 + rr1 + rr2);
|
|
float ft_x_pn = jic * ft.x * (p0_p2 * p0_p2 + p1_p2 * p1_p2);
|
|
/////////////////////////////////////////////////////
|
|
jxc[0] = p0_p2 * jfc;
|
|
jxc[1] = f_p2_x;
|
|
jxc[2] = 0;
|
|
jxc[3] = -f_p2_x * p0_p2;
|
|
jxc[4] = -f_p2_x * p0_p2 * y0;
|
|
jxc[5] = f_p2_x * (z0 + x0 * p0_p2);
|
|
jxc[6] = -f_p2_x * y0;
|
|
jxc[7] = ft_x_pn * p0_p2;
|
|
|
|
jyc[0] = p1_p2 * jfc;
|
|
jyc[1] = 0;
|
|
jyc[2] = f_p2_y;
|
|
jyc[3] = -f_p2_y * p1_p2;
|
|
jyc[4] = -f_p2_y * (z0 + y0 * p1_p2);
|
|
jyc[5] = f_p2_y * x0 * p1_p2;
|
|
jyc[6] = f_p2_y * x0;
|
|
jyc[7] = ft_x_pn * p1_p2;
|
|
JACOBIAN_SET_JC_END
|
|
///////////////////////////////////
|
|
jxp[0] = f_p2_x * (r[0] - r[6] * p0_p2);
|
|
jxp[1] = f_p2_x * (r[1] - r[7] * p0_p2);
|
|
jxp[2] = f_p2_x * (r[2] - r[8] * p0_p2);
|
|
jyp[0] = f_p2_y * (r[3] - r[6] * p1_p2);
|
|
jyp[1] = f_p2_y * (r[4] - r[7] * p1_p2);
|
|
jyp[2] = f_p2_y * (r[5] - r[8] * p1_p2);
|
|
} else {
|
|
JACOBIAN_SET_JC_BEGIN
|
|
jxc[0] = p0_p2 * jic;
|
|
jxc[1] = f_p2;
|
|
jxc[2] = 0;
|
|
jxc[3] = -f_p2 * p0_p2;
|
|
jxc[4] = -f_p2 * p0_p2 * y0;
|
|
jxc[5] = f_p2 * (z0 + x0 * p0_p2);
|
|
jxc[6] = -f_p2 * y0;
|
|
|
|
jyc[0] = p1_p2 * jic;
|
|
jyc[1] = 0;
|
|
jyc[2] = f_p2;
|
|
jyc[3] = -f_p2 * p1_p2;
|
|
jyc[4] = -f_p2 * (z0 + y0 * p1_p2);
|
|
jyc[5] = f_p2 * x0 * p1_p2;
|
|
jyc[6] = f_p2 * x0;
|
|
|
|
if (md) {
|
|
float2 ms = tex1Dfetch(tex_jacobian_meas, tidx);
|
|
float msn = (ms.x * ms.x + ms.y * ms.y) * jic;
|
|
jxc[7] = -ms.x * msn;
|
|
jyc[7] = -ms.y * msn;
|
|
} else {
|
|
jxc[7] = 0;
|
|
jyc[7] = 0;
|
|
}
|
|
JACOBIAN_SET_JC_END
|
|
///////////////////////////////////
|
|
jxp[0] = f_p2 * (r[0] - r[6] * p0_p2);
|
|
jxp[1] = f_p2 * (r[1] - r[7] * p0_p2);
|
|
jxp[2] = f_p2 * (r[2] - r[8] * p0_p2);
|
|
jyp[0] = f_p2 * (r[3] - r[6] * p1_p2);
|
|
jyp[1] = f_p2 * (r[4] - r[7] * p1_p2);
|
|
jyp[2] = f_p2 * (r[5] - r[8] * p1_p2);
|
|
}
|
|
}
|
|
|
|
template <bool md, bool pd>
|
|
__device__ void jacobian_camera_internal(int camera_pos, int pt_pos, int tidx,
|
|
float* r, float jic, float* jxc,
|
|
float* jyc) {
|
|
float m[3];
|
|
float4 ft = tex1Dfetch(tex_jacobian_cam, camera_pos);
|
|
float4 r1 = tex1Dfetch(tex_jacobian_cam, camera_pos + 1);
|
|
r[0] = r1.x;
|
|
r[1] = r1.y;
|
|
r[2] = r1.z;
|
|
r[3] = r1.w;
|
|
float4 r2 = tex1Dfetch(tex_jacobian_cam, camera_pos + 2);
|
|
r[4] = r2.x;
|
|
r[5] = r2.y;
|
|
r[6] = r2.z;
|
|
r[7] = r2.w;
|
|
float4 r3 = tex1Dfetch(tex_jacobian_cam, camera_pos + 3);
|
|
r[8] = r3.x;
|
|
|
|
float4 temp = tex1Dfetch(tex_jacobian_pts, pt_pos);
|
|
m[0] = temp.x;
|
|
m[1] = temp.y;
|
|
m[2] = temp.z;
|
|
|
|
float x0 = r[0] * m[0] + r[1] * m[1] + r[2] * m[2];
|
|
float y0 = r[3] * m[0] + r[4] * m[1] + r[5] * m[2];
|
|
float z0 = r[6] * m[0] + r[7] * m[1] + r[8] * m[2];
|
|
float f_p2 = FDIV(ft.x, z0 + ft.w);
|
|
float p0_p2 = FDIV(x0 + ft.y, z0 + ft.w);
|
|
float p1_p2 = FDIV(y0 + ft.z, z0 + ft.w);
|
|
#ifndef PBA_DISABLE_CONST_CAMERA
|
|
if (r3.w != 0.0f) {
|
|
jxc[0] = 0;
|
|
jxc[1] = 0;
|
|
jxc[2] = 0;
|
|
jxc[3] = 0;
|
|
jxc[4] = 0;
|
|
jxc[5] = 0;
|
|
jxc[6] = 0;
|
|
jxc[7] = 0;
|
|
jyc[0] = 0;
|
|
jyc[1] = 0;
|
|
jyc[2] = 0;
|
|
jyc[3] = 0;
|
|
jyc[4] = 0;
|
|
jyc[5] = 0;
|
|
jyc[6] = 0;
|
|
jyc[7] = 0;
|
|
} else
|
|
#endif
|
|
if (pd) {
|
|
float rr1 = r3.y * p0_p2 * p0_p2;
|
|
float rr2 = r3.y * p1_p2 * p1_p2;
|
|
float f_p2_x = f_p2 * (1.0 + 3.0 * rr1 + rr2);
|
|
float f_p2_y = f_p2 * (1.0 + 3.0 * rr2 + rr1);
|
|
float jfc = jic * (1 + rr1 + rr2);
|
|
float ft_x_pn = jic * ft.x * (p0_p2 * p0_p2 + p1_p2 * p1_p2);
|
|
/////////////////////////////////////////////////////
|
|
jxc[0] = p0_p2 * jfc;
|
|
jxc[1] = f_p2_x;
|
|
jxc[2] = 0;
|
|
jxc[3] = -f_p2_x * p0_p2;
|
|
jxc[4] = -f_p2_x * p0_p2 * y0;
|
|
jxc[5] = f_p2_x * (z0 + x0 * p0_p2);
|
|
jxc[6] = -f_p2_x * y0;
|
|
jxc[7] = ft_x_pn * p0_p2;
|
|
|
|
jyc[0] = p1_p2 * jfc;
|
|
jyc[1] = 0;
|
|
jyc[2] = f_p2_y;
|
|
jyc[3] = -f_p2_y * p1_p2;
|
|
jyc[4] = -f_p2_y * (z0 + y0 * p1_p2);
|
|
jyc[5] = f_p2_y * x0 * p1_p2;
|
|
jyc[6] = f_p2_y * x0;
|
|
jyc[7] = ft_x_pn * p1_p2;
|
|
} else {
|
|
jxc[0] = p0_p2 * jic;
|
|
jxc[1] = f_p2;
|
|
jxc[2] = 0;
|
|
jxc[3] = -f_p2 * p0_p2;
|
|
jxc[4] = -f_p2 * p0_p2 * y0;
|
|
jxc[5] = f_p2 * (z0 + x0 * p0_p2);
|
|
jxc[6] = -f_p2 * y0;
|
|
|
|
jyc[0] = p1_p2 * jic;
|
|
jyc[1] = 0;
|
|
jyc[2] = f_p2;
|
|
jyc[3] = -f_p2 * p1_p2;
|
|
jyc[4] = -f_p2 * (z0 + y0 * p1_p2);
|
|
jyc[5] = f_p2 * x0 * p1_p2;
|
|
jyc[6] = f_p2 * x0;
|
|
|
|
if (md) {
|
|
float2 ms = tex1Dfetch(tex_jacobian_meas, tidx);
|
|
float msn = (ms.x * ms.x + ms.y * ms.y) * jic;
|
|
jxc[7] = -ms.x * msn;
|
|
jyc[7] = -ms.y * msn;
|
|
} else {
|
|
jxc[7] = 0;
|
|
jyc[7] = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
template <bool pd>
|
|
__device__ void jacobian_point_internal(int camera_pos, int pt_pos, int tidx,
|
|
float* r, float* jxp, float* jyp) {
|
|
float m[3];
|
|
float4 ft = tex1Dfetch(tex_jacobian_cam, camera_pos);
|
|
float4 r1 = tex1Dfetch(tex_jacobian_cam, camera_pos + 1);
|
|
r[0] = r1.x;
|
|
r[1] = r1.y;
|
|
r[2] = r1.z;
|
|
r[3] = r1.w;
|
|
float4 r2 = tex1Dfetch(tex_jacobian_cam, camera_pos + 2);
|
|
r[4] = r2.x;
|
|
r[5] = r2.y;
|
|
r[6] = r2.z;
|
|
r[7] = r2.w;
|
|
float4 r3 = tex1Dfetch(tex_jacobian_cam, camera_pos + 3);
|
|
r[8] = r3.x;
|
|
|
|
float4 temp = tex1Dfetch(tex_jacobian_pts, pt_pos);
|
|
m[0] = temp.x;
|
|
m[1] = temp.y;
|
|
m[2] = temp.z;
|
|
|
|
float x0 = r[0] * m[0] + r[1] * m[1] + r[2] * m[2];
|
|
float y0 = r[3] * m[0] + r[4] * m[1] + r[5] * m[2];
|
|
float z0 = r[6] * m[0] + r[7] * m[1] + r[8] * m[2];
|
|
float f_p2 = FDIV(ft.x, z0 + ft.w);
|
|
float p0_p2 = FDIV(x0 + ft.y, z0 + ft.w);
|
|
float p1_p2 = FDIV(y0 + ft.z, z0 + ft.w);
|
|
|
|
if (pd) {
|
|
float rr1 = r3.y * p0_p2 * p0_p2;
|
|
float rr2 = r3.y * p1_p2 * p1_p2;
|
|
float f_p2_x = f_p2 * (1.0 + 3.0 * rr1 + rr2);
|
|
float f_p2_y = f_p2 * (1.0 + 3.0 * rr2 + rr1);
|
|
///////////////////////////////////
|
|
jxp[0] = f_p2_x * (r[0] - r[6] * p0_p2);
|
|
jxp[1] = f_p2_x * (r[1] - r[7] * p0_p2);
|
|
jxp[2] = f_p2_x * (r[2] - r[8] * p0_p2);
|
|
jyp[0] = f_p2_y * (r[3] - r[6] * p1_p2);
|
|
jyp[1] = f_p2_y * (r[4] - r[7] * p1_p2);
|
|
jyp[2] = f_p2_y * (r[5] - r[8] * p1_p2);
|
|
} else {
|
|
///////////////////////////////////
|
|
jxp[0] = f_p2 * (r[0] - r[6] * p0_p2);
|
|
jxp[1] = f_p2 * (r[1] - r[7] * p0_p2);
|
|
jxp[2] = f_p2 * (r[2] - r[8] * p0_p2);
|
|
jyp[0] = f_p2 * (r[3] - r[6] * p1_p2);
|
|
jyp[1] = f_p2 * (r[4] - r[7] * p1_p2);
|
|
jyp[2] = f_p2 * (r[5] - r[8] * p1_p2);
|
|
}
|
|
}
|
|
|
|
template <bool md, bool pd>
|
|
__global__ void multiply_jx_noj_kernel(int num, int bwidth, int offset,
|
|
float jic, float2* result) {
|
|
int index = threadIdx.x + blockIdx.x * blockDim.x + blockIdx.y * bwidth;
|
|
if (index >= num) return;
|
|
|
|
__shared__ float data[9 * 64];
|
|
////////////////////////////////////////////
|
|
int2 proj = tex1Dfetch(tex_mjx_idx, index);
|
|
float4 xc1 = tex1Dfetch(tex_mjx_x, proj.x);
|
|
float4 xc2 = tex1Dfetch(tex_mjx_x, proj.x + 1);
|
|
float4 xp = tex1Dfetch(tex_mjx_x, proj.y + offset);
|
|
|
|
////////////////////////////////////////////
|
|
float jxc[8], jyc[8], jxp[3], jyp[3];
|
|
jacobian_internal<md, pd>(proj.x << 1, proj.y, index, data + 9 * threadIdx.x,
|
|
jic, jxc, jyc, jxp, jyp);
|
|
|
|
/////////////////////////////////////
|
|
result[index] = make_float2(
|
|
jxc[0] * xc1.x + jxc[1] * xc1.y + jxc[2] * xc1.z + jxc[3] * xc1.w +
|
|
jxc[4] * xc2.x + jxc[5] * xc2.y + jxc[6] * xc2.z + jxc[7] * xc2.w +
|
|
jxp[0] * xp.x + jxp[1] * xp.y + jxp[2] * xp.z,
|
|
jyc[0] * xc1.x + jyc[1] * xc1.y + jyc[2] * xc1.z + jyc[3] * xc1.w +
|
|
jyc[4] * xc2.x + jyc[5] * xc2.y + jyc[6] * xc2.z + jyc[7] * xc2.w +
|
|
jyp[0] * xp.x + jyp[1] * xp.y + jyp[2] * xp.z);
|
|
}
|
|
|
|
template <bool md, bool pd>
|
|
__global__ void multiply_jcx_noj_kernel(int num, int bwidth, float jic,
|
|
float2* result) {
|
|
int index = threadIdx.x + blockIdx.x * blockDim.x + blockIdx.y * bwidth;
|
|
if (index >= num) return;
|
|
|
|
__shared__ float data[9 * 64];
|
|
////////////////////////////////////////////
|
|
int2 proj = tex1Dfetch(tex_mjx_idx, index);
|
|
float4 xc1 = tex1Dfetch(tex_mjx_x, proj.x);
|
|
float4 xc2 = tex1Dfetch(tex_mjx_x, proj.x + 1);
|
|
|
|
////////////////////////////////////////////
|
|
float jxc[8], jyc[8];
|
|
jacobian_camera_internal<md, pd>(proj.x << 1, proj.y, index,
|
|
data + 9 * threadIdx.x, jic, jxc, jyc);
|
|
|
|
/////////////////////////////////////
|
|
result[index] = make_float2(
|
|
jxc[0] * xc1.x + jxc[1] * xc1.y + jxc[2] * xc1.z + jxc[3] * xc1.w +
|
|
jxc[4] * xc2.x + jxc[5] * xc2.y + jxc[6] * xc2.z + jxc[7] * xc2.w,
|
|
jyc[0] * xc1.x + jyc[1] * xc1.y + jyc[2] * xc1.z + jyc[3] * xc1.w +
|
|
jyc[4] * xc2.x + jyc[5] * xc2.y + jyc[6] * xc2.z + jyc[7] * xc2.w);
|
|
}
|
|
|
|
template <bool pd>
|
|
__global__ void multiply_jpx_noj_kernel(int num, int bwidth, int offset,
|
|
float2* result) {
|
|
int index = threadIdx.x + blockIdx.x * blockDim.x + blockIdx.y * bwidth;
|
|
if (index >= num) return;
|
|
|
|
__shared__ float data[9 * 64];
|
|
////////////////////////////////////////////
|
|
int2 proj = tex1Dfetch(tex_mjx_idx, index);
|
|
float4 xp = tex1Dfetch(tex_mjx_x, proj.y + offset);
|
|
|
|
////////////////////////////////////////////
|
|
float jxp[3], jyp[3];
|
|
jacobian_point_internal<pd>(proj.x << 1, proj.y, index,
|
|
data + 9 * threadIdx.x, jxp, jyp);
|
|
|
|
/////////////////////////////////////
|
|
result[index] = make_float2(jxp[0] * xp.x + jxp[1] * xp.y + jxp[2] * xp.z,
|
|
jyp[0] * xp.x + jyp[1] * xp.y + jyp[2] * xp.z);
|
|
}
|
|
|
|
void ProgramCU::ComputeJX_(CuTexImage& x, CuTexImage& jx, CuTexImage& camera,
|
|
CuTexImage& point, CuTexImage& meas,
|
|
CuTexImage& pjmap, bool intrinsic_fixed,
|
|
int radial_distortion, int mode) {
|
|
unsigned int nproj = pjmap.GetImgWidth();
|
|
unsigned int len = nproj;
|
|
unsigned int bsize = 64;
|
|
unsigned int nblock = (len + bsize - 1) / bsize;
|
|
unsigned int bw, bh;
|
|
int point_offset = camera.GetImgWidth() * 2;
|
|
float jfc = intrinsic_fixed ? 0 : 1.0f;
|
|
|
|
/////////////////////////////
|
|
pjmap.BindTexture(tex_mjx_idx);
|
|
x.BindTexture(tex_mjx_x);
|
|
camera.BindTexture(tex_jacobian_cam);
|
|
point.BindTexture(tex_jacobian_pts);
|
|
|
|
///////////////////////////////////
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
|
|
if (mode == 0) {
|
|
if (radial_distortion == -1) {
|
|
meas.BindTexture(tex_jacobian_meas);
|
|
multiply_jx_noj_kernel<true, false><<<grid, block>>>(
|
|
len, (bw * bsize), point_offset, jfc, (float2*)jx.data());
|
|
} else if (radial_distortion) {
|
|
multiply_jx_noj_kernel<false, true><<<grid, block>>>(
|
|
len, (bw * bsize), point_offset, jfc, (float2*)jx.data());
|
|
} else {
|
|
multiply_jx_noj_kernel<false, false><<<grid, block>>>(
|
|
len, (bw * bsize), point_offset, jfc, (float2*)jx.data());
|
|
}
|
|
|
|
CheckErrorCUDA("ComputeJX_");
|
|
} else if (mode == 1) {
|
|
if (radial_distortion == -1) {
|
|
meas.BindTexture(tex_jacobian_meas);
|
|
multiply_jcx_noj_kernel<true, false><<<grid, block>>>(
|
|
len, (bw * bsize), jfc, (float2*)jx.data());
|
|
} else if (radial_distortion) {
|
|
multiply_jcx_noj_kernel<false, true><<<grid, block>>>(
|
|
len, (bw * bsize), jfc, (float2*)jx.data());
|
|
} else {
|
|
multiply_jcx_noj_kernel<false, false><<<grid, block>>>(
|
|
len, (bw * bsize), jfc, (float2*)jx.data());
|
|
}
|
|
|
|
CheckErrorCUDA("ComputeJCX_");
|
|
} else if (mode == 2) {
|
|
if (radial_distortion == 1) {
|
|
multiply_jpx_noj_kernel<true><<<grid, block>>>(
|
|
len, (bw * bsize), point_offset, (float2*)jx.data());
|
|
} else {
|
|
multiply_jpx_noj_kernel<false><<<grid, block>>>(
|
|
len, (bw * bsize), point_offset, (float2*)jx.data());
|
|
}
|
|
|
|
CheckErrorCUDA("ComputeJX_");
|
|
}
|
|
}
|
|
|
|
template <bool md, bool pd, int KH>
|
|
__global__ void jte_cam_vec_noj_kernel(int num, int rowsz, float jic,
|
|
float* jte) {
|
|
__shared__ float value[KH * 32 * 9]; // 8 * KH * 32
|
|
int cam = blockIdx.x * KH + threadIdx.y + blockIdx.y * rowsz;
|
|
if (cam >= num) return;
|
|
|
|
// read data range for this camera
|
|
// 8 thread will do the same thing
|
|
int idx1 = tex1Dfetch(tex_jte_cmp, cam); // first camera
|
|
int idx2 = tex1Dfetch(tex_jte_cmp, cam + 1); // last camera + 1
|
|
|
|
float* valuec = value + 32 * 9 * threadIdx.y;
|
|
float* rp = valuec + threadIdx.x * 9;
|
|
float rr[8], jxc[8], jyc[8];
|
|
for (int i = 0; i < 8; ++i) rr[i] = 0;
|
|
|
|
// loop to read the index of the projection.
|
|
// so to get the location to read the jacobian
|
|
for (int i = idx1 + threadIdx.x; i < idx2; i += 32) {
|
|
int index = tex1Dfetch(tex_jte_cmt, i);
|
|
int2 proj = tex1Dfetch(tex_jacobian_idx, index);
|
|
jacobian_camera_internal<md, pd>(cam << 2, proj.y, index, rp, jic, jxc,
|
|
jyc);
|
|
float2 vv = tex1Dfetch(tex_jte_pe, index);
|
|
//
|
|
for (int j = 0; j < 8; ++j) rr[j] += (jxc[j] * vv.x + jyc[j] * vv.y);
|
|
}
|
|
|
|
float* valuei = valuec + 8 * threadIdx.x;
|
|
for (int i = 0; i < 8; ++i) valuei[i] = rr[i];
|
|
valuec[threadIdx.x] = (valuec[threadIdx.x] + valuec[threadIdx.x + 32] +
|
|
valuec[threadIdx.x + 64] + valuec[threadIdx.x + 96] +
|
|
valuec[threadIdx.x + 128] + valuec[threadIdx.x + 160] +
|
|
valuec[threadIdx.x + 192] + valuec[threadIdx.x + 224]);
|
|
if (threadIdx.x < 16) valuec[threadIdx.x] += valuec[threadIdx.x + 16];
|
|
if (threadIdx.x < 8)
|
|
valuec[threadIdx.x] = valuec[threadIdx.x] + valuec[threadIdx.x + 8];
|
|
|
|
////////////////////////////////////
|
|
if (threadIdx.x < 8) jte[(cam << 3) + threadIdx.x] = valuec[threadIdx.x];
|
|
}
|
|
|
|
template <bool pd, int KH>
|
|
__global__ void jte_point_vec_noj_kernel(int num, int rowsz, float* jte) {
|
|
////////////////////////////
|
|
__shared__ float value[KH * (9 * 32)];
|
|
int index = blockIdx.x * KH + threadIdx.y + blockIdx.y * rowsz;
|
|
if (index >= num) return;
|
|
|
|
int idx1 = tex1Dfetch(tex_jte_pmp, index); // first
|
|
int idx2 = tex1Dfetch(tex_jte_pmp, index + 1); // last + 1
|
|
float rx = 0, ry = 0, rz = 0, jxp[3], jyp[3];
|
|
int rowp = threadIdx.y * 9 * 32;
|
|
float* rp = value + threadIdx.x * 9 + rowp;
|
|
for (int i = idx1 + threadIdx.x; i < idx2; i += 32) {
|
|
float2 ev = tex1Dfetch(tex_jte_pe, i);
|
|
int2 proj = tex1Dfetch(tex_jacobian_idx, i);
|
|
jacobian_point_internal<pd>(proj.x << 1, proj.y, i, rp, jxp, jyp);
|
|
rx += (jxp[0] * ev.x + jyp[0] * ev.y);
|
|
ry += (jxp[1] * ev.x + jyp[1] * ev.y);
|
|
rz += (jxp[2] * ev.x + jyp[2] * ev.y);
|
|
}
|
|
|
|
int loc = (threadIdx.x << 2) + rowp;
|
|
value[loc] = rx;
|
|
value[loc + 1] = ry;
|
|
value[loc + 2] = rz;
|
|
value[loc + 3] = 0;
|
|
|
|
int ridx = threadIdx.x + rowp;
|
|
value[ridx] = ((value[ridx] + value[ridx + 32]) +
|
|
(value[ridx + 64] + value[ridx + 96]));
|
|
if (threadIdx.x < 16) value[ridx] += value[ridx + 16];
|
|
if (threadIdx.x < 8) value[ridx] += value[ridx + 8];
|
|
if (threadIdx.x < 4)
|
|
jte[(index << 2) + threadIdx.x] = value[ridx] + value[ridx + 4];
|
|
}
|
|
|
|
void ProgramCU::ComputeJtE_(CuTexImage& e, CuTexImage& jte, CuTexImage& camera,
|
|
CuTexImage& point, CuTexImage& meas,
|
|
CuTexImage& cmap, CuTexImage& cmlist,
|
|
CuTexImage& pmap, CuTexImage& pjmap, CuTexImage& jp,
|
|
bool intrinsic_fixed, int radial_distortion,
|
|
int mode) {
|
|
pjmap.BindTexture(tex_jacobian_idx);
|
|
camera.BindTexture(tex_jacobian_cam);
|
|
point.BindTexture(tex_jacobian_pts);
|
|
if (radial_distortion) meas.BindTexture(tex_jacobian_meas);
|
|
|
|
cmap.BindTexture(tex_jte_cmp);
|
|
cmlist.BindTexture(tex_jte_cmt);
|
|
e.BindTexture(tex_jte_pe);
|
|
|
|
//
|
|
unsigned int bw, bh;
|
|
float jfc = intrinsic_fixed ? 0 : 1.0f;
|
|
int ncam = camera.GetImgWidth();
|
|
const int bheight1 = 2, bsize = 32;
|
|
int nblock1 = (ncam + bheight1 - 1) / bheight1;
|
|
GetBlockConfiguration(nblock1, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize, bheight1);
|
|
if (mode == 2) {
|
|
} else if (radial_distortion == -1)
|
|
jte_cam_vec_noj_kernel<true, false, bheight1><<<grid, block>>>(
|
|
ncam, bw * bheight1, jfc, jte.data());
|
|
else if (radial_distortion)
|
|
jte_cam_vec_noj_kernel<false, true, bheight1><<<grid, block>>>(
|
|
ncam, bw * bheight1, jfc, jte.data());
|
|
else
|
|
jte_cam_vec_noj_kernel<false, false, bheight1><<<grid, block>>>(
|
|
ncam, bw * bheight1, jfc, jte.data());
|
|
CheckErrorCUDA("ComputeJtE_<Camera>");
|
|
|
|
int npt = point.GetImgWidth();
|
|
unsigned int offsetv = 8 * ncam;
|
|
const int bheight2 = 2, bsize2 = 32;
|
|
int nblock2 = (npt + bheight2 - 1) / bheight2;
|
|
GetBlockConfiguration(nblock2, bw, bh);
|
|
dim3 grid2(bw, bh), block2(bsize2, bheight2);
|
|
if (mode == 1) {
|
|
} else if (jp.IsValid()) {
|
|
pmap.BindTexture(tex_jte_pmp);
|
|
e.BindTexture(tex_jte_pex);
|
|
jp.BindTexture2(tex_jte_jp, tex_jte_jp2);
|
|
if (jp.GetDataSize() > MAX_TEXSIZE)
|
|
jte_point_vec_kernel<bheight2, 2><<<grid2, block2>>>(
|
|
npt, bw * bheight2, jte.data() + offsetv);
|
|
else
|
|
jte_point_vec_kernel<bheight2, 1><<<grid2, block2>>>(
|
|
npt, bw * bheight2, jte.data() + offsetv);
|
|
} else {
|
|
pmap.BindTexture(tex_jte_pmp);
|
|
if (radial_distortion && radial_distortion != -1)
|
|
jte_point_vec_noj_kernel<true, bheight2><<<grid2, block2>>>(
|
|
npt, bw * bheight2, jte.data() + offsetv);
|
|
else
|
|
jte_point_vec_noj_kernel<false, bheight2><<<grid2, block2>>>(
|
|
npt, bw * bheight2, jte.data() + offsetv);
|
|
}
|
|
CheckErrorCUDA("ComputeJtE_<Point>");
|
|
}
|
|
|
|
template <int KH, bool md, bool pd, bool scaling>
|
|
__global__ void jtjd_cam_block_noj_kernel(int num, int rowsz, float lambda1,
|
|
float lambda2, float jic, float* diag,
|
|
float* blocks,
|
|
bool add_existing_diagc) {
|
|
const int VN = (md || pd) ? 8 : 7;
|
|
__shared__ float buffer_all[32 * 9 * KH];
|
|
__shared__ float value_all[64 * KH];
|
|
|
|
// 8thread per camera
|
|
int bcam = blockIdx.x * KH + blockIdx.y * rowsz;
|
|
|
|
int cam = bcam + threadIdx.y;
|
|
if (cam >= num) return;
|
|
|
|
float* buffer = buffer_all + threadIdx.y * (32 * 9);
|
|
float* value = value_all + threadIdx.y * 64;
|
|
|
|
float jxc[8], jyc[8];
|
|
float* rp = buffer + threadIdx.x * 9;
|
|
float row0[VN], row1[VN - 1], row2[VN - 2], row3[VN - 3];
|
|
float row4[VN - 4], row5[VN - 5], row6[VN - 6], row7[1] = {0};
|
|
// read data range for this camera
|
|
// 8 thread will do the same thing
|
|
int idx1 = tex1Dfetch(tex_jtjd_cmp, cam); // first camera
|
|
int idx2 = tex1Dfetch(tex_jtjd_cmp, cam + 1); // last camera + 1
|
|
|
|
#define REPEAT7(FUNC) \
|
|
FUNC(0); \
|
|
FUNC(1); \
|
|
FUNC(2); \
|
|
FUNC(3); \
|
|
FUNC(4); \
|
|
FUNC(5); \
|
|
FUNC(6);
|
|
#define SETZERO(k) \
|
|
for (int j = 0; j < VN - k; ++j) row##k[j] = 0;
|
|
REPEAT7(SETZERO);
|
|
|
|
float4 sjv[2];
|
|
if (scaling && (pd || md)) {
|
|
sjv[0] = tex1Dfetch(tex_jacobian_sj, (cam << 1));
|
|
sjv[1] = tex1Dfetch(tex_jacobian_sj, (cam << 1) + 1);
|
|
}
|
|
|
|
// loop to read the index of the projection.
|
|
// so to get the location to read the jacobian
|
|
for (int i = idx1 + threadIdx.x; i < idx2; i += 32) {
|
|
/////////////////////////////////////////
|
|
int index = tex1Dfetch(tex_jtjd_cmlist, i);
|
|
int2 proj = tex1Dfetch(tex_jacobian_idx, index);
|
|
|
|
///////////////////////////////////////////////
|
|
jacobian_camera_internal<md, pd>(cam << 2, proj.y, index, rp, jic, jxc,
|
|
jyc);
|
|
|
|
if (scaling && (pd || md)) {
|
|
float* sj = (float*)sjv; // 32 threads...64 values
|
|
for (int j = 0; j < VN; ++j) {
|
|
jxc[j] *= sj[j];
|
|
jyc[j] *= sj[j];
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////
|
|
#define ADDROW(k) \
|
|
for (int j = k; j < VN; ++j) \
|
|
row##k[j - k] += (jxc[k] * jxc[j] + jyc[k] * jyc[j])
|
|
|
|
///////////////
|
|
REPEAT7(ADDROW);
|
|
if (VN == 8) {
|
|
ADDROW(7);
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////
|
|
// make the matrix..//add up the 32 * 8 matrix
|
|
#define JTJDSUM8_V1() \
|
|
buffer[threadIdx.x] = \
|
|
(buffer[threadIdx.x] + buffer[threadIdx.x + 32] + \
|
|
buffer[threadIdx.x + 64] + buffer[threadIdx.x + 96] + \
|
|
buffer[threadIdx.x + 128] + buffer[threadIdx.x + 160] + \
|
|
buffer[threadIdx.x + 192] + buffer[threadIdx.x + 224]);
|
|
|
|
#define JTJDSUM8_V2() \
|
|
buffer[threadIdx.x] = \
|
|
(((buffer[threadIdx.x] + buffer[threadIdx.x + 128]) + \
|
|
(buffer[threadIdx.x + 64] + buffer[threadIdx.x + 192])) + \
|
|
((buffer[threadIdx.x + 32] + buffer[threadIdx.x + 160]) + \
|
|
(buffer[threadIdx.x + 96] + buffer[threadIdx.x + 224])));
|
|
|
|
#define STORE_ROWS(k) \
|
|
for (int i = 0; i < (VN - k); ++i) bufi[i] = row##k[i]; \
|
|
JTJDSUM8_V2(); \
|
|
if (threadIdx.x < 16 - k) buffer[threadIdx.x] += buffer[threadIdx.x + 16]; \
|
|
if (threadIdx.x < 8 - k) \
|
|
value[threadIdx.x + k * 9] = buffer[threadIdx.x] + buffer[threadIdx.x + 8];
|
|
|
|
float* bufi = buffer + threadIdx.x * 8;
|
|
REPEAT7(STORE_ROWS);
|
|
if (VN == 8) {
|
|
STORE_ROWS(7);
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
//////////////////////////////// (8 * i + j) -> (8 * j + i)
|
|
//#define COPYSYM(i) if(threadIdx.x < VN - i - 1) value[threadIdx.x * 8 + i *
|
|
//9 + 8] = value[threadIdx.x + i * 9 + 1];
|
|
if (threadIdx.x < VN - 1) value[threadIdx.x * 8 + 8] = value[threadIdx.x + 1];
|
|
if (threadIdx.x < VN - 2)
|
|
value[threadIdx.x * 8 + 17] = value[threadIdx.x + 10];
|
|
if (threadIdx.x < VN - 3)
|
|
value[threadIdx.x * 8 + 26] = value[threadIdx.x + 19];
|
|
if (threadIdx.x < VN - 4)
|
|
value[threadIdx.x * 8 + 35] = value[threadIdx.x + 28];
|
|
if (threadIdx.x < VN - 5)
|
|
value[threadIdx.x * 8 + 44] = value[threadIdx.x + 37];
|
|
if (threadIdx.x < VN - 6)
|
|
value[threadIdx.x * 8 + 53] = value[threadIdx.x + 46];
|
|
if (VN == 8 && threadIdx.x < VN - 7)
|
|
value[threadIdx.x * 8 + 62] = value[threadIdx.x + 55];
|
|
|
|
if (scaling && !pd && !md) {
|
|
float4 sjv[2];
|
|
float* sj = (float*)sjv; // 32 threads...64 values
|
|
sjv[0] = tex1Dfetch(tex_jacobian_sj, (cam << 1));
|
|
sjv[1] = tex1Dfetch(tex_jacobian_sj, (cam << 1) + 1);
|
|
float sji = sj[threadIdx.x & 0x07];
|
|
value[threadIdx.x] *= (sji * sj[threadIdx.x / 8]);
|
|
value[threadIdx.x + 32] *= (sji * sj[4 + threadIdx.x / 8]);
|
|
}
|
|
|
|
bool zero = ((threadIdx.x & 0x7) == VN);
|
|
|
|
///////////write back
|
|
if (threadIdx.x < 8) {
|
|
float* dp = value + threadIdx.x * 9;
|
|
float temp = zero ? 0 : dp[0];
|
|
int didx = threadIdx.x + (cam << 3);
|
|
if (add_existing_diagc) temp += diag[didx];
|
|
diag[didx] = temp;
|
|
dp[0] = lambda1 + lambda2 * temp;
|
|
}
|
|
int wpos = cam * (8 * VN) + threadIdx.x;
|
|
blocks[wpos] = zero ? 0 : value[threadIdx.x];
|
|
if (threadIdx.x < VN * 8 - 32)
|
|
blocks[wpos + 32] = zero ? 0 : value[threadIdx.x + 32];
|
|
}
|
|
|
|
template <int KW, bool pd, bool scaling>
|
|
__global__ void jtjd_point_block_noj_kernel(int num, int rowsz, float lambda1,
|
|
float lambda2, float4* diag,
|
|
float4* blocks, int ptx) {
|
|
////////////////////////////
|
|
int index = blockIdx.x * blockDim.x + threadIdx.x + blockIdx.y * rowsz;
|
|
if (index >= num) return;
|
|
|
|
__shared__ float value[KW * 9];
|
|
int idx1 = tex1Dfetch(tex_jtjd_pmp, index); // first
|
|
int idx2 = tex1Dfetch(tex_jtjd_pmp, index + 1); // last + 1
|
|
|
|
float M00 = 0, M01 = 0, M02 = 0, M11 = 0, M12 = 0, M22 = 0;
|
|
float jxp[3], jyp[3];
|
|
float* rp = value + threadIdx.x * 9;
|
|
|
|
float4 sj;
|
|
if (scaling && pd) sj = tex1Dfetch(tex_jacobian_sj, index + ptx);
|
|
|
|
for (int i = idx1; i < idx2; ++i) {
|
|
int2 proj = tex1Dfetch(tex_jacobian_idx, i);
|
|
jacobian_point_internal<pd>(proj.x << 1, proj.y, i, rp, jxp, jyp);
|
|
|
|
if (scaling && pd) {
|
|
jxp[0] *= sj.x;
|
|
jxp[1] *= sj.y;
|
|
jxp[2] *= sj.z;
|
|
jyp[0] *= sj.x;
|
|
jyp[1] *= sj.y;
|
|
jyp[2] *= sj.z;
|
|
}
|
|
M00 += (jxp[0] * jxp[0] + jyp[0] * jyp[0]);
|
|
M01 += (jxp[0] * jxp[1] + jyp[0] * jyp[1]);
|
|
M02 += (jxp[0] * jxp[2] + jyp[0] * jyp[2]);
|
|
M11 += (jxp[1] * jxp[1] + jyp[1] * jyp[1]);
|
|
M12 += (jxp[1] * jxp[2] + jyp[1] * jyp[2]);
|
|
M22 += (jxp[2] * jxp[2] + jyp[2] * jyp[2]);
|
|
}
|
|
|
|
if (scaling && !pd) {
|
|
sj = tex1Dfetch(tex_jacobian_sj, index + ptx);
|
|
M00 *= (sj.x * sj.x);
|
|
M01 *= (sj.x * sj.y);
|
|
M02 *= (sj.x * sj.z);
|
|
M11 *= (sj.y * sj.y);
|
|
M12 *= (sj.y * sj.z);
|
|
M22 *= (sj.z * sj.z);
|
|
}
|
|
|
|
diag[index] = make_float4(M00, M11, M22, 0);
|
|
|
|
M00 = lambda2 * M00 + lambda1;
|
|
M11 = lambda2 * M11 + lambda1;
|
|
M22 = lambda2 * M22 + lambda1;
|
|
|
|
// invert the 3x3 matrix.
|
|
float det = (M00 * M11 - M01 * M01) * M22 + 2.0 * M01 * M12 * M02 -
|
|
M02 * M02 * M11 - M12 * M12 * M00;
|
|
if (det >= FLT_MAX || det <= FLT_MIN * 2.0f) {
|
|
int write_pos = index * 3;
|
|
blocks[write_pos] = make_float4(0, 0, 0, 0);
|
|
blocks[write_pos + 1] = make_float4(0, 0, 0, 0);
|
|
blocks[write_pos + 2] = make_float4(0, 0, 0, 0);
|
|
} else {
|
|
float m00 = (M11 * M22 - M12 * M12) / det;
|
|
float m01 = -(M01 * M22 - M12 * M02) / det;
|
|
float m02 = (M01 * M12 - M02 * M11) / det;
|
|
int write_pos = index * 3;
|
|
blocks[write_pos] = make_float4(m00, m01, m02, 0);
|
|
|
|
float m11 = (M00 * M22 - M02 * M02) / det;
|
|
float m12 = -(M00 * M12 - M01 * M02) / det;
|
|
blocks[write_pos + 1] = make_float4(m01, m11, m12, 0);
|
|
|
|
float m22 = (M00 * M11 - M01 * M01) / det;
|
|
blocks[write_pos + 2] = make_float4(m02, m12, m22, 0);
|
|
}
|
|
}
|
|
|
|
void ProgramCU::ComputeDiagonalBlock_(
|
|
float lambda, bool dampd, CuTexImage& camera, CuTexImage& point,
|
|
CuTexImage& meas, CuTexImage& cmap, CuTexImage& cmlist, CuTexImage& pmap,
|
|
CuTexImage& jmap, CuTexImage& jp, CuTexImage& sj, CuTexImage& diag,
|
|
CuTexImage& blocks, bool intrinsic_fixed, int radial_distortion,
|
|
bool add_existing_diagc, int mode) {
|
|
float lambda1 = dampd ? 0.0f : lambda;
|
|
float lambda2 = dampd ? (1.0f + lambda) : 1.0f;
|
|
float jfc = intrinsic_fixed ? 0.0f : 1.0f;
|
|
|
|
//////////////////////////////////
|
|
jmap.BindTexture(tex_jacobian_idx);
|
|
camera.BindTexture(tex_jacobian_cam);
|
|
point.BindTexture(tex_jacobian_pts);
|
|
cmap.BindTexture(tex_jtjd_cmp);
|
|
cmlist.BindTexture(tex_jtjd_cmlist);
|
|
|
|
////////////////////////////////////////////////////
|
|
const unsigned int bsize1 = 32;
|
|
const unsigned int bheight1 = 2;
|
|
unsigned int ncam = camera.GetImgWidth(); // how many cameras
|
|
unsigned int nblock = (ncam + bheight1 - 1) / bheight1;
|
|
unsigned int bw, bh;
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 block1(bsize1, bheight1), grid1(bw, bh);
|
|
|
|
///////////////////////////////////////////////////
|
|
if (radial_distortion == -1) meas.BindTexture(tex_jacobian_meas);
|
|
if (mode == 2) {
|
|
// skip the camera part.
|
|
} else if (sj.IsValid()) {
|
|
sj.BindTexture(tex_jacobian_sj);
|
|
if (radial_distortion == -1)
|
|
jtjd_cam_block_noj_kernel<bheight1, true, false, true><<<grid1, block1>>>(
|
|
ncam, bw * bheight1, lambda1, lambda2, jfc, diag.data(),
|
|
blocks.data(), add_existing_diagc);
|
|
else if (radial_distortion)
|
|
jtjd_cam_block_noj_kernel<bheight1, false, true, true><<<grid1, block1>>>(
|
|
ncam, bw * bheight1, lambda1, lambda2, jfc, diag.data(),
|
|
blocks.data(), add_existing_diagc);
|
|
else
|
|
jtjd_cam_block_noj_kernel<bheight1, false, false,
|
|
true><<<grid1, block1>>>(
|
|
ncam, bw * bheight1, lambda1, lambda2, jfc, diag.data(),
|
|
blocks.data(), add_existing_diagc);
|
|
} else {
|
|
if (radial_distortion == -1)
|
|
jtjd_cam_block_noj_kernel<bheight1, true, false,
|
|
false><<<grid1, block1>>>(
|
|
ncam, bw * bheight1, lambda1, lambda2, jfc, diag.data(),
|
|
blocks.data(), add_existing_diagc);
|
|
else if (radial_distortion)
|
|
jtjd_cam_block_noj_kernel<bheight1, false, true,
|
|
false><<<grid1, block1>>>(
|
|
ncam, bw * bheight1, lambda1, lambda2, jfc, diag.data(),
|
|
blocks.data(), add_existing_diagc);
|
|
else
|
|
jtjd_cam_block_noj_kernel<bheight1, false, false,
|
|
false><<<grid1, block1>>>(
|
|
ncam, bw * bheight1, lambda1, lambda2, jfc, diag.data(),
|
|
blocks.data(), add_existing_diagc);
|
|
}
|
|
CheckErrorCUDA("ComputeDiagonalBlock_<Camera>");
|
|
|
|
////////////////////////////////////////////////////
|
|
const unsigned int bsize2 = 64;
|
|
unsigned int npoint = point.GetImgWidth();
|
|
unsigned int len2 = npoint;
|
|
unsigned int nblock2 = (len2 + bsize2 - 1) / bsize2;
|
|
unsigned int offsetd = 2 * ncam;
|
|
unsigned int offsetb = (radial_distortion ? 16 : 14) * ncam;
|
|
GetBlockConfiguration(nblock2, bw, bh);
|
|
dim3 grid2(bw, bh), block2(bsize2);
|
|
pmap.BindTexture(tex_jtjd_pmp);
|
|
|
|
if (mode == 1) {
|
|
} else if (jp.IsValid()) {
|
|
jp.BindTexture2(tex_jtjd_jp, tex_jtjd_jp2);
|
|
if (jp.GetDataSize() > MAX_TEXSIZE)
|
|
jtjd_point_block_kernel<2><<<grid2, block2>>>(
|
|
len2, (bw * bsize2), lambda1, lambda2,
|
|
((float4*)diag.data()) + offsetd, ((float4*)blocks.data()) + offsetb);
|
|
else
|
|
jtjd_point_block_kernel<1><<<grid2, block2>>>(
|
|
len2, (bw * bsize2), lambda1, lambda2,
|
|
((float4*)diag.data()) + offsetd, ((float4*)blocks.data()) + offsetb);
|
|
} else {
|
|
if (sj.IsValid()) {
|
|
sj.BindTexture(tex_jacobian_sj);
|
|
if (radial_distortion && radial_distortion != -1)
|
|
jtjd_point_block_noj_kernel<bsize2, true, true><<<grid2, block2>>>(
|
|
len2, (bw * bsize2), lambda1, lambda2,
|
|
((float4*)diag.data()) + offsetd,
|
|
((float4*)blocks.data()) + offsetb, offsetd);
|
|
else
|
|
jtjd_point_block_noj_kernel<bsize2, false, true><<<grid2, block2>>>(
|
|
len2, (bw * bsize2), lambda1, lambda2,
|
|
((float4*)diag.data()) + offsetd,
|
|
((float4*)blocks.data()) + offsetb, offsetd);
|
|
} else {
|
|
if (radial_distortion && radial_distortion != -1)
|
|
jtjd_point_block_noj_kernel<bsize2, true, false><<<grid2, block2>>>(
|
|
len2, (bw * bsize2), lambda1, lambda2,
|
|
((float4*)diag.data()) + offsetd,
|
|
((float4*)blocks.data()) + offsetb, 0);
|
|
else
|
|
jtjd_point_block_noj_kernel<bsize2, false, false><<<grid2, block2>>>(
|
|
len2, (bw * bsize2), lambda1, lambda2,
|
|
((float4*)diag.data()) + offsetd,
|
|
((float4*)blocks.data()) + offsetb, 0);
|
|
}
|
|
}
|
|
CheckErrorCUDA("ComputeDiagonalBlock_<Point>");
|
|
|
|
////////////////////////////////////////////////////
|
|
if (mode != 2) {
|
|
const unsigned int bsize3 = JTJD_BLOCK_CAM_INVERT_KWIDTH;
|
|
unsigned int len3 = ncam * 8;
|
|
unsigned int nblock3 = (len3 + bsize3 - 1) / bsize3;
|
|
dim3 grid3(nblock3), block3(bsize3);
|
|
if (radial_distortion)
|
|
jtjd_cam_block_invert_kernel<8><<<grid3, block3>>>(
|
|
len3, (float4*)blocks.data());
|
|
else
|
|
jtjd_cam_block_invert_kernel<7><<<grid3, block3>>>(
|
|
len3, (float4*)blocks.data());
|
|
CheckErrorCUDA("ComputeDiagonalBlockInverse<Camera>");
|
|
}
|
|
}
|
|
|
|
__global__ void projection_q_kernel(int nproj, int rowsz, float2* pj) {
|
|
////////////////////////////////
|
|
int tidx = threadIdx.x + blockIdx.x * blockDim.x + blockIdx.y * rowsz;
|
|
if (tidx >= nproj) return;
|
|
int2 proj = tex1Dfetch(tex_projection_idx, tidx);
|
|
float2 wq = tex1Dfetch(tex_projection_mea, tidx);
|
|
///////////////////////////////////
|
|
float f1 = tex1Dfetch(tex_projection_cam, proj.x * 4).x;
|
|
float r1 = tex1Dfetch(tex_projection_cam, proj.x * 4 + 3).w;
|
|
float f2 = tex1Dfetch(tex_projection_cam, proj.y * 4).x;
|
|
float r2 = tex1Dfetch(tex_projection_cam, proj.y * 4 + 3).w;
|
|
pj[tidx] = make_float2(-wq.x * (f1 - f2), -wq.y * (r1 - r2));
|
|
}
|
|
|
|
void ProgramCU::ComputeProjectionQ(CuTexImage& camera, CuTexImage& qmap,
|
|
CuTexImage& qw, CuTexImage& proj,
|
|
int offset) {
|
|
///////////////////////////////////////
|
|
unsigned int len = qmap.GetImgWidth();
|
|
unsigned int bsize = PROJECTION_FRT_KWIDTH;
|
|
unsigned int nblock = (len + bsize - 1) / bsize;
|
|
unsigned int bw, bh;
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
|
|
///////////////////////////////////////////
|
|
camera.BindTexture(tex_projection_cam);
|
|
qmap.BindTexture(tex_projection_idx);
|
|
qw.BindTexture(tex_projection_mea);
|
|
|
|
//////////////////////////////
|
|
projection_q_kernel<<<grid, block>>>(len, bw * bsize,
|
|
((float2*)proj.data()) + offset);
|
|
}
|
|
|
|
template <bool SJ>
|
|
__global__ void multiply_jqx_kernel(int num, int bwidth, float2* result) {
|
|
int index = threadIdx.x + blockIdx.x * blockDim.x + blockIdx.y * bwidth;
|
|
if (index >= num) return;
|
|
////////////////////////////////////////////
|
|
int2 proj = tex1Dfetch(tex_mjx_idx, index);
|
|
float2 wq = tex1Dfetch(tex_jacobian_meas, index);
|
|
int idx1 = proj.x * 2, idx2 = proj.y * 2;
|
|
float x11 = tex1Dfetch(tex_mjx_x, idx1).x;
|
|
float x17 = tex1Dfetch(tex_mjx_x, idx1 + 1).w;
|
|
float x21 = tex1Dfetch(tex_mjx_x, idx2).x;
|
|
float x27 = tex1Dfetch(tex_mjx_x, idx2 + 1).w;
|
|
|
|
if (SJ) {
|
|
float s11 = tex1Dfetch(tex_jacobian_sj, idx1).x;
|
|
float s17 = tex1Dfetch(tex_jacobian_sj, idx1 + 1).w;
|
|
float s21 = tex1Dfetch(tex_jacobian_sj, idx2).x;
|
|
float s27 = tex1Dfetch(tex_jacobian_sj, idx2 + 1).w;
|
|
result[index] = make_float2((x11 * s11 - x21 * s21) * wq.x,
|
|
(x17 * s17 - x27 * s27) * wq.y);
|
|
} else {
|
|
result[index] = make_float2((x11 - x21) * wq.x, (x17 - x27) * wq.y);
|
|
}
|
|
}
|
|
|
|
void ProgramCU::ComputeJQX(CuTexImage& x, CuTexImage& qmap, CuTexImage& wq,
|
|
CuTexImage& sj, CuTexImage& jx, int offset) {
|
|
unsigned int nproj = qmap.GetImgWidth();
|
|
unsigned int len = nproj;
|
|
unsigned int bsize = 64;
|
|
unsigned int nblock = (len + bsize - 1) / bsize;
|
|
unsigned int bw, bh;
|
|
|
|
/////////////////////////////
|
|
qmap.BindTexture(tex_mjx_idx);
|
|
x.BindTexture(tex_mjx_x);
|
|
wq.BindTexture(tex_jacobian_meas);
|
|
|
|
///////////////////////////////////
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
|
|
if (sj.IsValid()) {
|
|
sj.BindTexture(tex_jacobian_sj);
|
|
multiply_jqx_kernel<true><<<grid, block>>>(len, (bw * bsize),
|
|
((float2*)jx.data()) + offset);
|
|
} else {
|
|
multiply_jqx_kernel<false><<<grid, block>>>(len, (bw * bsize),
|
|
((float2*)jx.data()) + offset);
|
|
}
|
|
}
|
|
|
|
texture<int2, 1, cudaReadModeElementType> tex_jte_q_idx;
|
|
texture<float2, 1, cudaReadModeElementType> tex_jte_q_w;
|
|
|
|
template <bool SJ>
|
|
__global__ void jte_cam_q_kernel(int num, int bwidth, float* jte) {
|
|
// int cam = blockIdx.x * KH + threadIdx.y + blockIdx.y * rowsz ;
|
|
int index = threadIdx.x + blockIdx.x * blockDim.x + blockIdx.y * bwidth;
|
|
if (index >= num) return;
|
|
int2 indexp = tex1Dfetch(tex_jte_q_idx, index);
|
|
if (indexp.x == -1) return;
|
|
float2 wq = tex1Dfetch(tex_jte_q_w, index);
|
|
float2 e1 = tex1Dfetch(tex_jte_pe, indexp.x);
|
|
float2 e2 = tex1Dfetch(tex_jte_pe, indexp.y);
|
|
int index8 = index << 3;
|
|
if (SJ) {
|
|
float s1 = tex1Dfetch(tex_jacobian_sj, index * 2).x;
|
|
jte[index8] += s1 * wq.x * (e1.x - e2.x);
|
|
float s7 = tex1Dfetch(tex_jacobian_sj, index * 2 + 1).w;
|
|
jte[index8 + 7] += s7 * wq.y * (e1.y - e2.y);
|
|
} else {
|
|
jte[index8] += wq.x * (e1.x - e2.x);
|
|
jte[index8 + 7] += wq.y * (e1.y - e2.y);
|
|
}
|
|
}
|
|
|
|
void ProgramCU::ComputeJQtEC(CuTexImage& pe, CuTexImage& qlist, CuTexImage& wq,
|
|
CuTexImage& sj, CuTexImage& jte) {
|
|
int ncam = qlist.GetImgWidth();
|
|
const int bsize = 32;
|
|
int nblock = (ncam + bsize - 1) / bsize;
|
|
unsigned int bw, bh;
|
|
GetBlockConfiguration(nblock, bw, bh);
|
|
dim3 grid(bw, bh), block(bsize);
|
|
|
|
pe.BindTexture(tex_jte_pe);
|
|
qlist.BindTexture(tex_jte_q_idx);
|
|
wq.BindTexture(tex_jte_q_w);
|
|
|
|
if (sj.IsValid()) {
|
|
sj.BindTexture(tex_jacobian_sj);
|
|
jte_cam_q_kernel<true><<<grid, block>>>(ncam, (bw * bsize), jte.data());
|
|
} else {
|
|
jte_cam_q_kernel<false><<<grid, block>>>(ncam, (bw * bsize), jte.data());
|
|
}
|
|
}
|
|
|
|
} // namespace pba
|