colmap/lib/PBA/ProgramCU.cu

////////////////////////////////////////////////////////////////////////////
//  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