Skip to content

T
tile_processor_gpu
Commit 702c2f76 authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

CODEX: Add resident CUDA pose LMA preparation 

Co-authored-by: 's avatarCodex <codex@elphel.com>
parent 31f70553
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 589 additions and 36 deletions
jna/test_pose_lma_resident_jna.cu 0 → 100644
View file @ 702c2f76
// test_pose_lma_resident_jna.cu - NVRTC/TpProc resident continuation gate.
// It first launches pose_fx_jacobian, then consumes those raw device buffers
// without re-uploading fx/J. Prepared arrays are captured only for this oracle.
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <vector>
enum { NUM_PARAMS=3, NUM_COMPONENTS=2, PRODUCTS=12, RESULT=21 };
extern "C" {
void* tp_create_module(const char*, const char*);
const char* tp_last_error();
void* tp_proc_create(void*);
int tp_proc_setup(void*,int,int,int,int,int,int);
int tp_proc_exec_pose_fx_jacobian(
void*,const float*,const float*,const float*,int,const float*,int,
const float*,const float*,const float*,int,const float*,int,
const float*,const float*,const unsigned char*,int,int,
float*,float*,unsigned char*);
int tp_proc_exec_pose_lma_resident_step(
void*,float,const float*,const float*,const float*,const float*,float,
int,int,float*,float*,float*);
void tp_proc_destroy(void*);
void tp_destroy_module(void*);
}
static float fmul(float a,float b){ volatile float v=a*b; return v; }
static float fadd(float a,float b){ volatile float v=a+b; return v; }
static float fsub(float a,float b){ volatile float v=a-b; return v; }
int main(int argc,char** argv)
{
const char* srcdir=(argc>1)?argv[1]:"src";
const char* devrt=(argc>2)?argv[2]:
"/usr/local/cuda-12.8/targets/x86_64-linux/lib/libcudadevrt.a";
void* module=tp_create_module(srcdir,devrt);
if(!module){ std::fprintf(stderr,"tp_create_module: %s\n",tp_last_error()); return EXIT_FAILURE; }
void* proc=tp_proc_create(module);
if(!proc||tp_proc_setup(proc,1,1,16,16,0,0)){
std::fprintf(stderr,"tp_proc setup: %s\n",tp_last_error());
if(proc) tp_proc_destroy(proc); tp_destroy_module(module); return EXIT_FAILURE;
}
const int width=64,height=48,rbrLen=256,ersLen=height*14,numTiles=3;
const int numValues=NUM_COMPONENTS*numTiles+NUM_PARAMS;
const float meta[8]={(float)width,(float)height,4.0f,5.0f,2.85f,1.0f,0.0f,0.0004f};
const float radial[7]={0,0,0,0,0,0,0};
std::vector<float> rbr(rbrLen,1.0f),ers(ersLen,0.0f);
for(int line=0;line<height;line++) ers[(size_t)line*14+6]=1.0f;
const float pose[12]={0,0,0, 0,0,0, 0,0,0, 0,0,0};
const float centers[3*numTiles]={20,10,5, 40,30,0, 12,16,3};
const unsigned char selection[numTiles]={1,1,0};
float rawFx[2*numTiles],rawJt[NUM_PARAMS*2*numTiles]; unsigned char valid[numTiles];
int bad=0;
int rc=tp_proc_exec_pose_fx_jacobian(
proc,meta,radial,rbr.data(),rbrLen,ers.data(),ersLen,
meta,radial,rbr.data(),rbrLen,ers.data(),ersLen,
pose,centers,selection,numTiles,1,rawFx,rawJt,valid);
if(rc){ std::fprintf(stderr,"pose jacobian rc=%d: %s\n",rc,tp_last_error()); bad++; }
const float lambda=0.001f,pureWeight=0.98f;
const float vector[NUM_PARAMS]={0.0023f,-0.0017f,0.00091f};
float weights[numValues]={0},y[numValues]={0},eigen[4*numTiles]={0};
for(int tile=0;tile<2;tile++){
const int vi=2*tile,ei=4*tile;
weights[vi]=weights[vi+1]=0.245f;
y[vi]=rawFx[vi]+0.031f-0.004f*tile;
y[vi+1]=rawFx[vi+1]-0.022f+0.003f*tile;
eigen[ei]=0.82f; eigen[ei+1]=0.13f; eigen[ei+2]=0.07f; eigen[ei+3]=-0.61f;
}
for(int par=0;par<NUM_PARAMS;par++){
const int value=2*numTiles+par;
weights[value]=0.0066666668f;
y[value]=vector[par]+(par-1)*0.0002f;
}
float packed[RESULT],preparedJt[NUM_PARAMS*numValues],preparedYmfx[numValues];
rc=tp_proc_exec_pose_lma_resident_step(proc,lambda,weights,y,eigen,vector,
pureWeight,numTiles,1,packed,preparedJt,preparedYmfx);
if(rc){ std::fprintf(stderr,"resident step rc=%d: %s\n",rc,tp_last_error()); bad++; }
float expectedJt[NUM_PARAMS*numValues]={0},expectedYmfx[numValues]={0};
if(!rc){
for(int tile=0;tile<numTiles;tile++){
const int vi=2*tile,ei=4*tile;
const bool use=weights[vi]>0.0f,have=use&&valid[tile];
const float fx0=have?rawFx[vi]:0.0f,fx1=have?rawFx[vi+1]:0.0f;
float d0=0.0f,d1=0.0f;
if(use){
const float dx=fsub(y[vi],fx0),dy=fsub(y[vi+1],fx1);
d0=fadd(fmul(eigen[ei],dx),fmul(eigen[ei+1],dy));
d1=fadd(fmul(eigen[ei+2],dx),fmul(eigen[ei+3],dy));
}
expectedYmfx[vi]=fmul(d0,weights[vi]); expectedYmfx[vi+1]=fmul(d1,weights[vi+1]);
for(int par=0;par<NUM_PARAMS;par++){
const int ri=par*2*numTiles+vi,oi=par*numValues+vi;
const float jx=have?rawJt[ri]:0.0f,jy=have?rawJt[ri+1]:0.0f;
expectedJt[oi]=fadd(fmul(eigen[ei],jx),fmul(eigen[ei+1],jy));
expectedJt[oi+1]=fadd(fmul(eigen[ei+2],jx),fmul(eigen[ei+3],jy));
}
}
for(int par=0;par<NUM_PARAMS;par++){
const int value=2*numTiles+par;
expectedJt[par*numValues+value]=1.0f;
expectedYmfx[value]=fmul(fsub(y[value],vector[par]),weights[value]);
}
int preparedBad=0;
for(int i=0;i<NUM_PARAMS*numValues;i++)
if(std::memcmp(preparedJt+i,expectedJt+i,sizeof(float))) preparedBad++;
for(int i=0;i<numValues;i++)
if(std::memcmp(preparedYmfx+i,expectedYmfx+i,sizeof(float))) preparedBad++;
if(preparedBad){ std::printf("prepared bit mismatches=%d\n",preparedBad); bad++; }
if(packed[0]!=1.0f||!std::isfinite(packed[19])||!std::isfinite(packed[20])){
std::printf("invalid resident result valid=%g rms=%g/%g\n",packed[0],packed[19],packed[20]); bad++;
}
}
std::printf("NVRTC resident pose LMA: reused raw fx/J + prepared oracle + compact step -> %s\n",
bad?"FAIL":"PASS");
tp_proc_destroy(proc); tp_destroy_module(module);
return bad?EXIT_FAILURE:EXIT_SUCCESS;
}
jna/tp_jna.cpp
View file @ 702c2f76
This diff is collapsed.
src/tests/test_pose_lma_step.cu
View file @ 702c2f76
......@@ -94,6 +94,62 @@ static void cpuPoseLmaStep(
result[0] = 1.0f;
}
static void cpuResidentPrepare(
int numTiles,
const float * rawFx,
const float * rawJt,
const unsigned char * valid,
const float * y,
const float * weights,
const float * eigen,
const float * vector,
float pureWeight,
float * jt,
float * ymfx,
float * rms)
{
const int numValues = 2 * numTiles + POSE_NUM_PARAMS;
float pureL2 = 0.0f;
for (int tile = 0; tile < numTiles; tile++) {
const int vi = 2 * tile;
const int ei = 4 * tile;
const bool useTile = weights[vi] > 0.0f;
const bool haveRaw = useTile && valid[tile];
const float rawX = haveRaw ? rawFx[vi] : 0.0f;
const float rawY = haveRaw ? rawFx[vi + 1] : 0.0f;
float residual0 = 0.0f, residual1 = 0.0f;
if (useTile) {
const float dx = fsub(y[vi], rawX);
const float dy = fsub(y[vi + 1], rawY);
residual0 = fadd(fmul(eigen[ei], dx), fmul(eigen[ei + 1], dy));
residual1 = fadd(fmul(eigen[ei + 2], dx), fmul(eigen[ei + 3], dy));
}
ymfx[vi] = fmul(residual0, weights[vi]);
ymfx[vi + 1] = fmul(residual1, weights[vi + 1]);
pureL2 = fadd(pureL2, fmul(residual0, ymfx[vi]));
pureL2 = fadd(pureL2, fmul(residual1, ymfx[vi + 1]));
for (int par = 0; par < POSE_NUM_PARAMS; par++) {
const int rawIndex = par * 2 * numTiles + vi;
const float jx = haveRaw ? rawJt[rawIndex] : 0.0f;
const float jy = haveRaw ? rawJt[rawIndex + 1] : 0.0f;
const int outIndex = par * numValues + vi;
jt[outIndex] = fadd(fmul(eigen[ei], jx), fmul(eigen[ei + 1], jy));
jt[outIndex + 1] = fadd(fmul(eigen[ei + 2], jx), fmul(eigen[ei + 3], jy));
}
}
float fullL2 = pureL2;
for (int par = 0; par < POSE_NUM_PARAMS; par++) {
const int value = 2 * numTiles + par;
for (int row = 0; row < POSE_NUM_PARAMS; row++)
jt[row * numValues + value] = (row == par) ? 1.0f : 0.0f;
const float residual = fsub(y[value], vector[par]);
ymfx[value] = fmul(residual, weights[value]);
fullL2 = fadd(fullL2, fmul(residual, ymfx[value]));
}
rms[0] = std::sqrt(fullL2);
rms[1] = std::sqrt(fdiv(pureL2, pureWeight));
}
int main(int argc, char ** argv)
{
findCudaDevice(argc, (const char **) argv);
......@@ -133,5 +189,94 @@ int main(int argc, char ** argv)
}
std::printf("pose_lma_step: %d floats, bit-exact -> %s\n",
POSE_LMA_RESULT, bad ? "FAIL" : "PASS");
// Full-grid resident continuation: only 150 of 5120 tiles carry weight.
const int numTiles = 5120;
const int numValues = 2 * numTiles + POSE_NUM_PARAMS;
const int numPartials = (numTiles + POSE_LMA_REDUCE_THREADS - 1) /
POSE_LMA_REDUCE_THREADS;
std::vector<float> rawFx(2 * numTiles, NAN), rawJt((size_t) 6 * numTiles, NAN);
std::vector<unsigned char> valid(numTiles, 0);
std::vector<float> residentY(numValues, 0.0f), residentWeights(numValues, 0.0f);
std::vector<float> eigen(4 * numTiles, 0.0f);
const float residentVector[3] = {0.0023f, -0.0017f, 0.00091f};
const float fullWeight = 300.03f;
for (int selected = 0; selected < 150; selected++) {
const int tile = selected * 34;
const int vi = 2 * tile;
valid[tile] = 1;
rawFx[vi] = 80.0f + 0.031f * selected;
rawFx[vi + 1] = 55.0f - 0.017f * selected;
residentY[vi] = rawFx[vi] + 0.025f - 0.00007f * selected;
residentY[vi + 1] = rawFx[vi + 1] - 0.018f + 0.00009f * selected;
residentWeights[vi] = residentWeights[vi + 1] = 1.0f / fullWeight;
eigen[4 * tile] = 0.82f;
eigen[4 * tile + 1] = 0.13f;
eigen[4 * tile + 2] = 0.07f;
eigen[4 * tile + 3] = -0.61f;
for (int par = 0; par < POSE_NUM_PARAMS; par++) {
const int ji = par * 2 * numTiles + vi;
rawJt[ji] = 17.0f * (par + 1) + 0.0031f * selected + 0.19f * par * selected;
rawJt[ji + 1] = -9.0f + 23.0f * par - 0.0027f * selected + 0.11f * par * selected;
}
}
for (int par = 0; par < POSE_NUM_PARAMS; par++) {
const int value = 2 * numTiles + par;
residentWeights[value] = 0.01f / fullWeight;
residentY[value] = residentVector[par] + (par - 1) * 0.0002f;
}
const float pureWeight = 300.0f / fullWeight;
std::vector<float> expectedJt((size_t) POSE_NUM_PARAMS * numValues, 0.0f);
std::vector<float> expectedYmfx(numValues, 0.0f);
float expectedRms[2];
cpuResidentPrepare(numTiles, rawFx.data(), rawJt.data(), valid.data(),
residentY.data(), residentWeights.data(), eigen.data(), residentVector,
pureWeight, expectedJt.data(), expectedYmfx.data(), expectedRms);
float expectedResident[POSE_LMA_RESULT];
cpuPoseLmaStep(numValues, lambda, residentWeights.data(), expectedJt.data(),
expectedYmfx.data(), residentVector, expectedResident);
GpuBuf<float> gpuRawFx(rawFx.size()), gpuRawJt(rawJt.size());
GpuBuf<unsigned char> gpuValid(valid.size());
GpuBuf<float> gpuY(residentY.size()), gpuWeightsResident(residentWeights.size());
GpuBuf<float> gpuEigen(eigen.size()), gpuVectorResident(POSE_NUM_PARAMS);
GpuBuf<float> gpuPreparedJt(expectedJt.size()), gpuPreparedYmfx(expectedYmfx.size());
GpuBuf<float> gpuPartials((size_t) numPartials * POSE_LMA_PARTIAL_VALUES);
GpuBuf<float> gpuResidentResult(POSE_LMA_RESIDENT_RESULT);
gpuRawFx.upload(rawFx.data()); gpuRawJt.upload(rawJt.data()); gpuValid.upload(valid.data());
gpuY.upload(residentY.data()); gpuWeightsResident.upload(residentWeights.data());
gpuEigen.upload(eigen.data()); gpuVectorResident.upload(residentVector);
pose_lma_prepare_products<<<numPartials, POSE_LMA_REDUCE_THREADS>>>(
numTiles, gpuRawFx.dev(), gpuRawJt.dev(), gpuValid.dev(), gpuY.dev(),
gpuWeightsResident.dev(), gpuEigen.dev(), gpuVectorResident.dev(),
gpuPreparedJt.dev(), gpuPreparedYmfx.dev(), gpuPartials.dev());
checkCudaErrors(cudaGetLastError());
pose_lma_reduce_step<<<1, 1>>>(numPartials, lambda, pureWeight,
gpuPartials.dev(), gpuVectorResident.dev(), gpuResidentResult.dev());
checkCudaErrors(cudaGetLastError()); checkCudaErrors(cudaDeviceSynchronize());
const std::vector<float> gotJt = gpuPreparedJt.download();
const std::vector<float> gotYmfx = gpuPreparedYmfx.download();
const std::vector<float> gotResident = gpuResidentResult.download();
int preparedBad = 0;
for (size_t i = 0; i < gotJt.size(); i++)
if (std::memcmp(&gotJt[i], &expectedJt[i], sizeof(float)) != 0) preparedBad++;
for (size_t i = 0; i < gotYmfx.size(); i++)
if (std::memcmp(&gotYmfx[i], &expectedYmfx[i], sizeof(float)) != 0) preparedBad++;
float maxProducts = 0.0f, maxDelta = 0.0f, maxCandidate = 0.0f;
for (int i = 0; i < POSE_LMA_PRODUCTS; i++)
maxProducts = std::fmax(maxProducts, std::fabs(gotResident[1 + i] - expectedResident[1 + i]));
for (int i = 0; i < POSE_NUM_PARAMS; i++) {
maxDelta = std::fmax(maxDelta, std::fabs(gotResident[13 + i] - expectedResident[13 + i]));
maxCandidate = std::fmax(maxCandidate, std::fabs(gotResident[16 + i] - expectedResident[16 + i]));
}
const float maxRms = std::fmax(std::fabs(gotResident[19] - expectedRms[0]),
std::fabs(gotResident[20] - expectedRms[1]));
const bool residentOk = (preparedBad == 0) && (gotResident[0] == 1.0f) &&
(maxProducts <= 0.02f) && (maxDelta <= 2.0e-5f) &&
(maxCandidate <= 2.0e-5f) && (maxRms <= 2.0e-6f);
std::printf("pose_lma resident 5120/150: prepared bit mismatches=%d, max products=%g, delta=%g, candidate=%g, RMS=%g -> %s\n",
preparedBad, maxProducts, maxDelta, maxCandidate, maxRms,
residentOk ? "PASS" : "FAIL");
if (!residentOk) bad++;
return bad ? EXIT_FAILURE : EXIT_SUCCESS;
}
src/tp_lma.cu
View file @ 702c2f76
......@@ -488,45 +488,19 @@ static __device__ __forceinline__ float pose_lma_sub(float a, float b)
return __fsub_rn(a, b);
}
extern "C" __global__ void pose_lma_step(
int num_values,
static __device__ __forceinline__ void pose_lma_solve(
float lambda,
const float * weights,
const float * jt,
const float * ymfx_weighted,
const float * products,
const float * vector,
float * result)
{
if ((blockIdx.x != 0) || (threadIdx.x != 0)) return;
result[0] = 0.0f;
#pragma unroll
for (int i = 1; i < 1 + POSE_LMA_PRODUCTS + POSE_NUM_PARAMS; i++) result[i] = nanf("");
#pragma unroll
for (int i = 0; i < POSE_NUM_PARAMS; i++) result[16 + i] = vector[i];
float products[POSE_LMA_PRODUCTS] = {0.0f};
#pragma unroll
for (int row = 0; row < POSE_NUM_PARAMS; row++) {
const float * jr = jt + (size_t) row * num_values;
#pragma unroll
for (int column = row; column < POSE_NUM_PARAMS; column++) {
const float * jc = jt + (size_t) column * num_values;
float sum = 0.0f;
for (int value = 0; value < num_values; value++) {
const float weighted = pose_lma_mul(weights[value], jr[value]);
sum = pose_lma_add(sum, pose_lma_mul(weighted, jc[value]));
}
products[row * POSE_NUM_PARAMS + column] = sum;
products[column * POSE_NUM_PARAMS + row] = sum;
}
float sum = 0.0f;
for (int value = 0; value < num_values; value++) {
sum = pose_lma_add(sum, pose_lma_mul(jr[value], ymfx_weighted[value]));
}
products[POSE_NUM_PARAMS * POSE_NUM_PARAMS + row] = sum;
}
#pragma unroll
for (int i = 0; i < POSE_LMA_PRODUCTS; i++) result[1 + i] = products[i];
#pragma unroll
for (int i = 0; i < POSE_NUM_PARAMS; i++) result[16 + i] = vector[i];
float a[9];
#pragma unroll
......@@ -569,6 +543,191 @@ extern "C" __global__ void pose_lma_step(
result[0] = 1.0f;
}
static __device__ __forceinline__ void pose_lma_accumulate(
float * values,
float weight,
float weighted_residual,
float j0,
float j1,
float j2)
{
const float wj0 = pose_lma_mul(weight, j0);
const float wj1 = pose_lma_mul(weight, j1);
values[0] = pose_lma_add(values[0], pose_lma_mul(wj0, j0));
values[1] = pose_lma_add(values[1], pose_lma_mul(wj0, j1));
values[2] = pose_lma_add(values[2], pose_lma_mul(wj0, j2));
values[3] = pose_lma_add(values[3], pose_lma_mul(wj1, j1));
values[4] = pose_lma_add(values[4], pose_lma_mul(wj1, j2));
values[5] = pose_lma_add(values[5], pose_lma_mul(pose_lma_mul(weight, j2), j2));
values[6] = pose_lma_add(values[6], pose_lma_mul(j0, weighted_residual));
values[7] = pose_lma_add(values[7], pose_lma_mul(j1, weighted_residual));
values[8] = pose_lma_add(values[8], pose_lma_mul(j2, weighted_residual));
}
extern "C" __global__ void pose_lma_step(
int num_values,
float lambda,
const float * weights,
const float * jt,
const float * ymfx_weighted,
const float * vector,
float * result)
{
if ((blockIdx.x != 0) || (threadIdx.x != 0)) return;
float products[POSE_LMA_PRODUCTS] = {0.0f};
#pragma unroll
for (int row = 0; row < POSE_NUM_PARAMS; row++) {
const float * jr = jt + (size_t) row * num_values;
#pragma unroll
for (int column = row; column < POSE_NUM_PARAMS; column++) {
const float * jc = jt + (size_t) column * num_values;
float sum = 0.0f;
for (int value = 0; value < num_values; value++) {
const float weighted = pose_lma_mul(weights[value], jr[value]);
sum = pose_lma_add(sum, pose_lma_mul(weighted, jc[value]));
}
products[row * POSE_NUM_PARAMS + column] = sum;
products[column * POSE_NUM_PARAMS + row] = sum;
}
float sum = 0.0f;
for (int value = 0; value < num_values; value++) {
sum = pose_lma_add(sum, pose_lma_mul(jr[value], ymfx_weighted[value]));
}
products[POSE_NUM_PARAMS * POSE_NUM_PARAMS + row] = sum;
}
pose_lma_solve(lambda, products, vector, result);
}
extern "C" __global__ void pose_lma_prepare_products(
int num_tiles,
const float * raw_fx,
const float * raw_jt,
const unsigned char * valid,
const float * y,
const float * weights,
const float * eigen,
const float * vector,
float * transformed_jt,
float * ymfx_weighted,
float * partials)
{
__shared__ float reduction[POSE_LMA_PARTIAL_VALUES * POSE_LMA_REDUCE_THREADS];
const int tid = (int) threadIdx.x;
const int tile = (int) (blockIdx.x * blockDim.x + threadIdx.x);
const int num_values = POSE_NUM_COMPONENTS * num_tiles + POSE_NUM_PARAMS;
float values[POSE_LMA_PARTIAL_VALUES] = {0.0f};
if (tile < num_tiles) {
const int vi = POSE_NUM_COMPONENTS * tile;
const int ei = 4 * tile;
const bool use_tile = weights[vi] > 0.0f;
const bool have_raw = use_tile && valid[tile];
const float raw_x = have_raw ? raw_fx[vi] : 0.0f;
const float raw_y = have_raw ? raw_fx[vi + 1] : 0.0f;
float residual[2] = {0.0f, 0.0f};
if (use_tile) {
const float dx = pose_lma_sub(y[vi], raw_x);
const float dy = pose_lma_sub(y[vi + 1], raw_y);
residual[0] = pose_lma_add(pose_lma_mul(eigen[ei], dx), pose_lma_mul(eigen[ei + 1], dy));
residual[1] = pose_lma_add(pose_lma_mul(eigen[ei + 2], dx), pose_lma_mul(eigen[ei + 3], dy));
}
const float weighted0 = pose_lma_mul(residual[0], weights[vi]);
const float weighted1 = pose_lma_mul(residual[1], weights[vi + 1]);
ymfx_weighted[vi] = weighted0;
ymfx_weighted[vi + 1] = weighted1;
float transformed[POSE_NUM_PARAMS][2];
#pragma unroll
for (int par = 0; par < POSE_NUM_PARAMS; par++) {
const int raw_i = par * POSE_NUM_COMPONENTS * num_tiles + vi;
const float jx = have_raw ? raw_jt[raw_i] : 0.0f;
const float jy = have_raw ? raw_jt[raw_i + 1] : 0.0f;
transformed[par][0] = pose_lma_add(pose_lma_mul(eigen[ei], jx), pose_lma_mul(eigen[ei + 1], jy));
transformed[par][1] = pose_lma_add(pose_lma_mul(eigen[ei + 2], jx), pose_lma_mul(eigen[ei + 3], jy));
const int out_i = par * num_values + vi;
transformed_jt[out_i] = transformed[par][0];
transformed_jt[out_i + 1] = transformed[par][1];
}
pose_lma_accumulate(values, weights[vi], weighted0,
transformed[0][0], transformed[1][0], transformed[2][0]);
pose_lma_accumulate(values, weights[vi + 1], weighted1,
transformed[0][1], transformed[1][1], transformed[2][1]);
const float l20 = pose_lma_mul(residual[0], weighted0);
const float l21 = pose_lma_mul(residual[1], weighted1);
values[9] = pose_lma_add(l20, l21);
values[10] = values[9];
}
if ((blockIdx.x == 0) && (tid == 0)) {
#pragma unroll
for (int par = 0; par < POSE_NUM_PARAMS; par++) {
const int value = POSE_NUM_COMPONENTS * num_tiles + par;
#pragma unroll
for (int row = 0; row < POSE_NUM_PARAMS; row++) {
transformed_jt[row * num_values + value] = (row == par) ? 1.0f : 0.0f;
}
const float residual = pose_lma_sub(y[value], vector[par]);
const float weighted = pose_lma_mul(residual, weights[value]);
ymfx_weighted[value] = weighted;
pose_lma_accumulate(values, weights[value], weighted,
(par == 0) ? 1.0f : 0.0f,
(par == 1) ? 1.0f : 0.0f,
(par == 2) ? 1.0f : 0.0f);
values[10] = pose_lma_add(values[10], pose_lma_mul(residual, weighted));
}
}
#pragma unroll
for (int item = 0; item < POSE_LMA_PARTIAL_VALUES; item++) {
reduction[item * POSE_LMA_REDUCE_THREADS + tid] = values[item];
}
__syncthreads();
for (int offset = POSE_LMA_REDUCE_THREADS / 2; offset > 0; offset >>= 1) {
if (tid < offset) {
#pragma unroll
for (int item = 0; item < POSE_LMA_PARTIAL_VALUES; item++) {
const int index = item * POSE_LMA_REDUCE_THREADS + tid;
reduction[index] = pose_lma_add(reduction[index], reduction[index + offset]);
}
}
__syncthreads();
}
if (tid == 0) {
#pragma unroll
for (int item = 0; item < POSE_LMA_PARTIAL_VALUES; item++) {
partials[blockIdx.x * POSE_LMA_PARTIAL_VALUES + item] =
reduction[item * POSE_LMA_REDUCE_THREADS];
}
}
}
extern "C" __global__ void pose_lma_reduce_step(
int num_partials,
float lambda,
float pure_weight,
const float * partials,
const float * vector,
float * result)
{
if ((blockIdx.x != 0) || (threadIdx.x != 0)) return;
float values[POSE_LMA_PARTIAL_VALUES] = {0.0f};
for (int partial = 0; partial < num_partials; partial++) {
#pragma unroll
for (int item = 0; item < POSE_LMA_PARTIAL_VALUES; item++) {
values[item] = pose_lma_add(values[item],
partials[partial * POSE_LMA_PARTIAL_VALUES + item]);
}
}
const float products[POSE_LMA_PRODUCTS] = {
values[0],values[1],values[2],
values[1],values[3],values[4],
values[2],values[4],values[5],
values[6],values[7],values[8]};
pose_lma_solve(lambda, products, vector, result);
result[19] = (values[10] >= 0.0f) ? sqrtf(values[10]) : nanf("");
result[20] = ((values[9] >= 0.0f) && (pure_weight > 0.0f)) ?
sqrtf(__fdiv_rn(values[9], pure_weight)) : nanf("");
}
extern "C" __global__ void lma_normal_products(
int num_params,
int num_values,
......
src/tp_lma.h
View file @ 702c2f76
......@@ -29,6 +29,10 @@
#define POSE_ERS_LINE_STRIDE 14
#define POSE_LMA_PRODUCTS 12
#define POSE_LMA_RESULT 19
#define POSE_LMA_UNIQUE_PRODUCTS 9
#define POSE_LMA_PARTIAL_VALUES 11
#define POSE_LMA_RESIDENT_RESULT 21
#define POSE_LMA_REDUCE_THREADS 128
typedef struct {
int width;
......@@ -70,6 +74,32 @@ extern "C" __global__ void pose_lma_step(
const float * vector, // current three-angle parameter vector
float * result); // [POSE_LMA_RESULT]
// Consume raw fx/J left resident by pose_fx_jacobian. One thread prepares both
// components of one tile, applies the eigen transform/weights, and participates
// in a block reduction. The first block also contributes the three pull rows.
extern "C" __global__ void pose_lma_prepare_products(
int num_tiles,
const float * raw_fx,
const float * raw_jt,
const unsigned char * valid,
const float * y,
const float * weights,
const float * eigen, // [num_tiles][2][2], row-major
const float * vector,
float * transformed_jt, // [3][2*num_tiles+3]
float * ymfx_weighted, // [2*num_tiles+3]
float * partials); // [grid][POSE_LMA_PARTIAL_VALUES]
// Fixed-size tail: reduce block partials, reconstruct symmetric H/b, compute
// RMS diagnostics and call the explicit single-thread 3x3 solve.
extern "C" __global__ void pose_lma_reduce_step(
int num_partials,
float lambda,
float pure_weight,
const float * partials,
const float * vector,
float * result); // [POSE_LMA_RESIDENT_RESULT]
extern "C" __global__ void lma_normal_products(
int num_params,
int num_values,
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment