colmap/lib/PBA/SparseBundleCU.cpp

////////////////////////////////////////////////////////////////////////////
//  File:           SparseBundleCU.cpp
//  Author:         Changchang Wu
//  Description :   implementation of the CUDA-based multicore bundle adjustment
//
//  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 <vector>
#include <iostream>
#include <utility>
#include <algorithm>
#include <fstream>
#include <iomanip>
using std::vector;
using std::cout;
using std::pair;
using std::ofstream;

#include <stdlib.h>
#include <math.h>
#include <float.h>
#include "pba.h"
#include "SparseBundleCU.h"

#include "ProgramCU.h"

using namespace pba::ProgramCU;

#ifdef _WIN32
#define finite _finite
#endif

namespace pba {

typedef float float_t;  // data type for host computation; double doesn't make
                        // much difference

#define CHECK_VEC(v1, v2)                                                 \
  for (size_t j = 0; j < v1.size(); ++j) {                                \
    if (v1[j] != v2[j]) {                                                 \
      different++;                                                        \
      std::cout << i << ' ' << j << ' ' << v1[j] << ' ' << v2[j] << '\n'; \
    }                                                                     \
  }
#define DEBUG_FUNCN(v, func, input, N)                                  \
  if (__debug_pba && v.IsValid()) {                                     \
    vector<float> buf(v.GetLength()), buf_(v.GetLength());              \
    for (int i = 0; i < N; ++i) {                                       \
      int different = 0;                                                \
      func input;                                                       \
      ProgramCU::FinishWorkCUDA();                                      \
      if (i > 0) {                                                      \
        v.CopyToHost(&buf_[0]);                                         \
        CHECK_VEC(buf, buf_);                                           \
      } else {                                                          \
        v.CopyToHost(&buf[0]);                                          \
      }                                                                 \
      if (different != 0)                                               \
        std::cout << #func << " : " << i << " : " << different << '\n'; \
    }                                                                   \
  }
#define DEBUG_FUNC(v, func, input) DEBUG_FUNCN(v, func, input, 2)

SparseBundleCU::SparseBundleCU(int device)
    : ParallelBA(PBA_INVALID_DEVICE),
      _num_camera(0),
      _num_point(0),
      _num_imgpt(0),
      _num_imgpt_q(0),
      _camera_data(NULL),
      _point_data(NULL),
      _imgpt_data(NULL),
      _camera_idx(NULL),
      _point_idx(NULL),
      _projection_sse(0) {
  __selected_device = device;
}

size_t SparseBundleCU::GetMemCapacity() {
  if (__selected_device != __current_device) SetCudaDevice(__selected_device);
  size_t sz = ProgramCU::GetCudaMemoryCap();
  if (sz < 1024) std::cout << "ERROR: CUDA is unlikely to be supported!\n";
  return sz < 1024 ? 0 : sz;
}

void SparseBundleCU::SetCameraData(size_t ncam, CameraT* cams) {
  if (sizeof(CameraT) != 16 * sizeof(float)) exit(0);  // never gonna happen...?
  _num_camera = (int)ncam;
  _camera_data = cams;
  _focal_mask = NULL;
}

void SparseBundleCU::SetFocalMask(const int* fmask, float weight) {
  _focal_mask = fmask;
  _weight_q = weight;
}

void SparseBundleCU::SetPointData(size_t npoint, Point3D* pts) {
  _num_point = (int)npoint;
  _point_data = (float*)pts;
}

void SparseBundleCU::SetProjection(size_t nproj, const Point2D* imgpts,
                                   const int* point_idx, const int* cam_idx) {
  _num_imgpt = (int)nproj;
  _imgpt_data = (float*)imgpts;
  _camera_idx = cam_idx;
  _point_idx = point_idx;
  _imgpt_datax.resize(0);
}

float SparseBundleCU::GetMeanSquaredError() {
  return float(_projection_sse /
               (_num_imgpt * __focal_scaling * __focal_scaling));
}

void SparseBundleCU::BundleAdjustment() {
  if (ValidateInputData() != STATUS_SUCCESS) return;

  //

  ////////////////////////
  TimerBA timer(this, TIMER_OVERALL);

  NormalizeData();
  if (InitializeBundle() != STATUS_SUCCESS) {
    // failed to allocate gpu storage
  } else if (__profile_pba) {
    // profiling some stuff
    RunProfileSteps();
  } else {
    // real optimization
    AdjustBundleAdjsutmentMode();
    NonlinearOptimizeLM();
    TransferDataToHost();
  }
  DenormalizeData();
}

int SparseBundleCU::RunBundleAdjustment() {
  if (__warmup_device) WarmupDevice();
  ResetBundleStatistics();
  BundleAdjustment();
  if (__num_lm_success > 0)
    SaveBundleStatistics(_num_camera, _num_point, _num_imgpt);
  if (__num_lm_success > 0) PrintBundleStatistics();
  ResetTemporarySetting();
  return __num_lm_success;
}

bool SparseBundleCU::InitializeBundleGPU() {
  bool previous_allocated = __memory_usage > 0;

  bool success = TransferDataToGPU() && InitializeStorageForCG();
  if (!success && previous_allocated) {
    if (__verbose_level) std::cout << "WARNING: try clean allocation\n";
    ClearPreviousError();
    ReleaseAllocatedData();
    success = TransferDataToGPU() && InitializeStorageForCG();
  }

  if (!success && __jc_store_original) {
    if (__verbose_level) std::cout << "WARNING: try not storing original JC\n";
    __jc_store_original = false;
    ClearPreviousError();
    ReleaseAllocatedData();
    success = TransferDataToGPU() && InitializeStorageForCG();
  }
  if (!success && __jc_store_transpose) {
    if (__verbose_level) std::cout << "WARNING: try not storing transpose JC\n";
    __jc_store_transpose = false;
    ClearPreviousError();
    ReleaseAllocatedData();
    success = TransferDataToGPU() && InitializeStorageForCG();
  }
  if (!success && !__no_jacobian_store) {
    if (__verbose_level) std::cout << "WARNING: switch to memory saving mode\n";
    __no_jacobian_store = true;
    ClearPreviousError();
    ReleaseAllocatedData();
    success = TransferDataToGPU() && InitializeStorageForCG();
  }
  return success;
}

int SparseBundleCU::ValidateInputData() {
  if (_camera_data == NULL) return STATUS_CAMERA_MISSING;
  if (_point_data == NULL) return STATUS_POINT_MISSING;
  if (_imgpt_data == NULL) return STATUS_MEASURMENT_MISSING;
  if (_camera_idx == NULL || _point_idx == NULL)
    return STATUS_PROJECTION_MISSING;
  return STATUS_SUCCESS;
}

void SparseBundleCU::WarmupDevice() {
  std::cout << "Warm up device with storage allocation...\n";
  if (__selected_device != __current_device) SetCudaDevice(__selected_device);
  CheckRequiredMemX();
  InitializeBundleGPU();
}

int SparseBundleCU::InitializeBundle() {
  /////////////////////////////////////////////////////
  TimerBA timer(this, TIMER_GPU_ALLOCATION);
  if (__selected_device != __current_device) SetCudaDevice(__selected_device);
  CheckRequiredMemX();
  ReserveStorageAuto();
  if (!InitializeBundleGPU()) return STATUS_ALLOCATION_FAIL;
  return STATUS_SUCCESS;
}

int SparseBundleCU::GetParameterLength() {
  return _num_camera * 8 + 4 * _num_point;
}

bool SparseBundleCU::CheckRequiredMemX() {
  if (CheckRequiredMem(0)) return true;
  if (__jc_store_original) {
    if (__verbose_level) std::cout << "NOTE: not storing original JC\n";
    __jc_store_original = false;
    if (CheckRequiredMem(1)) return true;
  }
  if (__jc_store_transpose) {
    if (__verbose_level) std::cout << "NOTE:  not storing camera Jacobian\n";
    __jc_store_transpose = false;
    if (CheckRequiredMem(1)) return true;
  }
  if (!__no_jacobian_store) {
    if (__verbose_level) std::cout << "NOTE: not storing any Jacobian\n";
    __no_jacobian_store = true;
    if (CheckRequiredMem(1)) return true;
  }
  return false;
}

bool SparseBundleCU::CheckRequiredMem(int fresh) {
  int m = _num_camera, n = _num_point, k = _num_imgpt;
#ifdef PBA_CUDA_ALLOCATE_MORE
  if (!fresh) {
    int m0 = _cuCameraData.GetReservedWidth();
    m = std::max(m, m0);
    int n0 = _cuPointData.GetReservedWidth();
    n = std::max(n, n0);
    int k0 = _cuMeasurements.GetReservedWidth();
    k = std::max(k, k0);
  }
#endif

  int p = 8 * m + 4 * n, q = _num_imgpt_q;
  size_t szn, total = GetCudaMemoryCap();
  size_t sz0 = 800 * 600 * 2 * 4 * sizeof(float);  //
  size_t szq = q > 0 ? (sizeof(float) * (q + m) * 4) : 0;
  size_t sz = sizeof(float) * (258 + 9 * n + 33 * m + 7 * k) + sz0;

  /////////////////////////////////// CG
  sz += p * 6 * sizeof(float);
  sz += ((__use_radial_distortion ? 64 : 56) * m + 12 * n) * sizeof(float);
  sz += (2 * (k + q) * sizeof(float));
  if (sz > total) return false;

  /////////////////////////////////////
  szn = (__no_jacobian_store ? 0 : (sizeof(float) * 8 * k));
  if (sz + szn > total)
    __no_jacobian_store = false;
  else
    sz += szn;
  /////////////////////////////
  szn = ((!__no_jacobian_store && __jc_store_transpose) ? 16 * k * sizeof(float)
                                                        : 0);
  if (sz + szn > total)
    __jc_store_transpose = false;
  else
    sz += szn;
  ///////////////////////////
  szn = ((!__no_jacobian_store && __jc_store_original) ? 16 * k * sizeof(float)
                                                       : 0);
  if (sz + szn > total)
    __jc_store_original = false;
  else
    sz += szn;
  ///////////////////////////////
  szn = ((!__no_jacobian_store && __jc_store_transpose && !__jc_store_original)
             ? k * sizeof(int)
             : 0);
  if (sz + szn > total) {
    __jc_store_transpose = false;
    sz -= (16 * k * sizeof(float));
  } else
    sz += szn;

  return sz <= total;
}

void SparseBundleCU::ReserveStorage(size_t ncam, size_t npt, size_t nproj) {
  if (ncam <= 1 || npt <= 1 || nproj <= 1) {
    ReleaseAllocatedData();
    // Reset the memory strategy to the default.
    __jc_store_transpose = true;
    __jc_store_original = true;
    __no_jacobian_store = false;
  } else {
    const int* camidx = _camera_idx;
    const int* ptidx = _point_idx;
    int ncam_ = _num_camera;
    int npt_ = _num_point;
    int nproj_ = _num_imgpt;

#ifdef PBA_CUDA_ALLOCATE_MORE
    size_t ncam_reserved = _cuCameraData.GetReservedWidth();
    size_t npt_reserved = _cuPointData.GetReservedWidth();
    size_t nproj_reserved = _cuMeasurements.GetReservedWidth();
    ncam = std::max(ncam, ncam_reserved);
    npt = std::max(npt, npt_reserved);
    nproj = std::max(nproj, nproj_reserved);
#endif

    _camera_idx = NULL;
    _point_idx = NULL;
    _num_camera = (int)ncam;
    _num_point = (int)npt;
    _num_imgpt = (int)nproj;

    if (__verbose_level)
      std::cout << "Reserving storage for ncam = " << ncam << "; npt = " << npt
                << "; nproj = " << nproj << '\n';
    InitializeBundleGPU();

    _num_camera = ncam_;
    _num_point = npt_;
    _num_imgpt = nproj_;
    _camera_idx = camidx;
    _point_idx = ptidx;
  }
}

static size_t upgrade_dimension(size_t sz) {
  size_t x = 1;
  while (x < sz) x <<= 1;
  return x;
}

void SparseBundleCU::ReserveStorageAuto() {
  if (_cuCameraData.data() == NULL || _cuPointData.data() == NULL ||
      _cuMeasurements.data() == NULL)
    return;
  ReserveStorage(upgrade_dimension(_num_camera), upgrade_dimension(_num_point),
                 upgrade_dimension(_num_imgpt));
}

#define REPORT_ALLOCATION(NAME)                                   \
  if (__verbose_allocation && NAME.GetDataSize() > 1024)          \
    std::cout << (NAME.GetDataSize() > 1024 * 1024                \
                      ? NAME.GetDataSize() / 1024 / 1024          \
                      : NAME.GetDataSize() / 1024)                \
              << (NAME.GetDataSize() > 1024 * 1024 ? "MB" : "KB") \
              << "\t allocated for " #NAME "\n";

#define ASSERT_ALLOCATION(NAME)                                    \
  if (!success) {                                                  \
    std::cerr << "WARNING: failed to allocate "                    \
              << (__verbose_allocation ? #NAME "; size = " : "")   \
              << (total_sz / 1024 / 1024) << "MB + "               \
              << (NAME.GetRequiredSize() / 1024 / 1024) << "MB\n"; \
    return false;                                                  \
  } else {                                                         \
    total_sz += NAME.GetDataSize();                                \
    REPORT_ALLOCATION(NAME);                                       \
  }

#define CHECK_ALLOCATION(NAME)                                     \
  if (NAME.GetDataSize() == 0 && NAME.GetRequiredSize() > 0) {     \
    ClearPreviousError();                                          \
    std::cerr << "WARNING: unable to allocate " #NAME ": "         \
              << (NAME.GetRequiredSize() / 1024 / 1024) << "MB\n"; \
  } else {                                                         \
    total_sz += NAME.GetDataSize();                                \
    REPORT_ALLOCATION(NAME);                                       \
  }

#define ALLOCATE_REQUIRED_DATA(NAME, num, channels) \
  {                                                 \
    success &= NAME.InitTexture(num, 1, channels);  \
    ASSERT_ALLOCATION(NAME);                        \
  }

#define ALLOCATE_OPTIONAL_DATA(NAME, num, channels, option) \
  if (option) {                                             \
    option = NAME.InitTexture(num, 1, channels);            \
    CHECK_ALLOCATION(NAME);                                 \
  } else {                                                  \
    NAME.InitTexture(0, 0, 0);                              \
  }

bool SparseBundleCU::TransferDataToGPU() {
  // given m camera, npoint, k measurements.. the number of float is
  bool success = true;
  size_t total_sz = 0;

  /////////////////////////////////////////////////////////////////////////////
  vector<int> qmap, qlist;
  vector<float> qmapw, qlistw;
  ProcessIndexCameraQ(qmap, qlist);

  //////////////////////////////////////////////////////////////////////////////
  ALLOCATE_REQUIRED_DATA(_cuBufferData, 256, 1);  // 256
  ALLOCATE_REQUIRED_DATA(_cuPointData, _num_point, 4);  // 4n
  ALLOCATE_REQUIRED_DATA(_cuCameraData, _num_camera, 16);  // 16m
  ALLOCATE_REQUIRED_DATA(_cuCameraDataEX, _num_camera, 16);  // 16m

  ////////////////////////////////////////////////////////////////
  ALLOCATE_REQUIRED_DATA(_cuCameraMeasurementMap, _num_camera + 1, 1);  // m
  ALLOCATE_REQUIRED_DATA(_cuCameraMeasurementList, _num_imgpt, 1);  // k
  ALLOCATE_REQUIRED_DATA(_cuPointMeasurementMap, _num_point + 1, 1);  // n
  ALLOCATE_REQUIRED_DATA(_cuProjectionMap, _num_imgpt, 2);  // 2k
  ALLOCATE_REQUIRED_DATA(_cuImageProj, _num_imgpt + _num_imgpt_q, 2);  // 2k
  ALLOCATE_REQUIRED_DATA(_cuPointDataEX, _num_point, 4);  // 4n
  ALLOCATE_REQUIRED_DATA(_cuMeasurements, _num_imgpt, 2);  // 2k

  //
  ALLOCATE_REQUIRED_DATA(_cuCameraQMap, _num_imgpt_q, 2);
  ALLOCATE_REQUIRED_DATA(_cuCameraQMapW, _num_imgpt_q, 2);
  ALLOCATE_REQUIRED_DATA(_cuCameraQList, (_num_imgpt_q > 0 ? _num_camera : 0),
                         2);
  ALLOCATE_REQUIRED_DATA(_cuCameraQListW, (_num_imgpt_q > 0 ? _num_camera : 0),
                         2);

  if (__no_jacobian_store) {
    _cuJacobianCamera.ReleaseData();
    _cuJacobianCameraT.ReleaseData();
    _cuJacobianPoint.ReleaseData();
    _cuCameraMeasurementListT.ReleaseData();
  } else {
    ALLOCATE_REQUIRED_DATA(_cuJacobianPoint, _num_imgpt * 2, 4);  // 8k
    ALLOCATE_OPTIONAL_DATA(_cuJacobianCameraT, _num_imgpt * 2, 8,
                           __jc_store_transpose);  // 16k
    ALLOCATE_OPTIONAL_DATA(_cuJacobianCamera, _num_imgpt * 2, 8,
                           __jc_store_original);  // 16k

    if ((!__jc_store_original || __profile_pba) && __jc_store_transpose) {
      ALLOCATE_OPTIONAL_DATA(_cuCameraMeasurementListT, _num_imgpt, 1,
                             __jc_store_transpose);  // k
      if (!__jc_store_transpose) _cuJacobianCameraT.ReleaseData();
    } else {
      _cuCameraMeasurementListT.ReleaseData();
    }
  }

  /////////////////////////////////////////////////
  if (_camera_idx && _point_idx) {
    //////////////////////////////////////////
    BundleTimerSwap(TIMER_PREPROCESSING, TIMER_GPU_ALLOCATION);
    ////mapping from camera to measuremnts
    vector<int> cpi(_num_camera + 1), cpidx(_num_imgpt);
    vector<int> cpnum(_num_camera, 0);
    cpi[0] = 0;
    for (int i = 0; i < _num_imgpt; ++i) cpnum[_camera_idx[i]]++;
    for (int i = 1; i <= _num_camera; ++i) cpi[i] = cpi[i - 1] + cpnum[i - 1];
    vector<int> cptidx = cpi;
    for (int i = 0; i < _num_imgpt; ++i) cpidx[cptidx[_camera_idx[i]]++] = i;
    if (_num_imgpt_q > 0) ProcessWeightCameraQ(cpnum, qmap, qmapw, qlistw);
    BundleTimerSwap(TIMER_PREPROCESSING, TIMER_GPU_ALLOCATION);

    ///////////////////////////////////////////////////////////////////////////////
    BundleTimerSwap(TIMER_GPU_UPLOAD, TIMER_GPU_ALLOCATION);
    _cuMeasurements.CopyFromHost(_imgpt_datax.size() > 0 ? &_imgpt_datax[0]
                                                         : _imgpt_data);
    _cuCameraData.CopyFromHost(_camera_data);
    _cuPointData.CopyFromHost(_point_data);
    _cuCameraMeasurementMap.CopyFromHost(&cpi[0]);
    _cuCameraMeasurementList.CopyFromHost(&cpidx[0]);
    if (_cuCameraMeasurementListT.IsValid()) {
      vector<int> ridx(_num_imgpt);
      for (int i = 0; i < _num_imgpt; ++i) ridx[cpidx[i]] = i;
      _cuCameraMeasurementListT.CopyFromHost(&ridx[0]);
    }
    if (_num_imgpt_q > 0) {
      _cuCameraQMap.CopyFromHost(&qmap[0]);
      _cuCameraQMapW.CopyFromHost(&qmapw[0]);
      _cuCameraQList.CopyFromHost(&qlist[0]);
      _cuCameraQListW.CopyFromHost(&qlistw[0]);
    }
    BundleTimerSwap(TIMER_GPU_UPLOAD, TIMER_GPU_ALLOCATION);

    ////////////////////////////////////////////
    ///////mapping from point to measurment
    BundleTimerSwap(TIMER_PREPROCESSING, TIMER_GPU_ALLOCATION);
    vector<int> ppi(_num_point + 1);
    for (int i = 0, last_point = -1; i < _num_imgpt; ++i) {
      int pt = _point_idx[i];
      while (last_point < pt) ppi[++last_point] = i;
    }
    ppi[_num_point] = _num_imgpt;

    //////////projection map
    vector<int> projection_map(_num_imgpt * 2);
    for (int i = 0; i < _num_imgpt; ++i) {
      int* imp = &projection_map[i * 2];
      imp[0] = _camera_idx[i] * 2;
      imp[1] = _point_idx[i];
    }
    BundleTimerSwap(TIMER_PREPROCESSING, TIMER_GPU_ALLOCATION);

    //////////////////////////////////////////////////////////////
    BundleTimerSwap(TIMER_GPU_UPLOAD, TIMER_GPU_ALLOCATION);
    _cuPointMeasurementMap.CopyFromHost(&ppi[0]);
    _cuProjectionMap.CopyFromHost(&projection_map[0]);
    BundleTimerSwap(TIMER_GPU_UPLOAD, TIMER_GPU_ALLOCATION);
  }

  __memory_usage = total_sz;
  if (__verbose_level > 1)
    std::cout << "Memory for Motion/Structure/Jacobian:\t"
              << (total_sz / 1024 / 1024) << "MB\n";
  return success;
}

bool SparseBundleCU::ProcessIndexCameraQ(vector<int>& qmap,
                                         vector<int>& qlist) {
  // reset q-data
  qmap.resize(0);
  qlist.resize(0);
  _num_imgpt_q = 0;

  // verify input
  if (_camera_idx == NULL) return true;
  if (_point_idx == NULL) return true;
  if (_focal_mask == NULL) return true;
  if (_num_camera == 0) return true;
  if (_weight_q <= 0) return true;

  ///////////////////////////////////////

  int error = 0;
  vector<int> temp(_num_camera * 2, -1);

  for (int i = 0; i < _num_camera; ++i) {
    int iq = _focal_mask[i];
    if (iq > i) {
      error = 1;
      break;
    }
    if (iq < 0) continue;
    if (iq == i) continue;
    int ip = temp[2 * iq];
    // float ratio = _camera_data[i].f / _camera_data[iq].f;
    // if(ratio < 0.01 || ratio > 100)
    //{
    //  std::cout << "Warning: constaraints on largely different camreas\n";
    //  continue;
    //}else
    if (_focal_mask[iq] != iq) {
      error = 1;
      break;
    } else if (ip == -1) {
      temp[2 * iq] = i;
      temp[2 * iq + 1] = i;
      temp[2 * i] = iq;
      temp[2 * i + 1] = iq;
    } else {
      // maintain double-linked list
      temp[2 * i] = ip;
      temp[2 * i + 1] = iq;
      temp[2 * ip + 1] = i;
      temp[2 * iq] = i;
    }
  }

  if (error) {
    std::cout << "Error: incorrect constraints\n";
    _focal_mask = NULL;
    return false;
  }

  qlist.resize(_num_camera * 2, -1);
  for (int i = 0; i < _num_camera; ++i) {
    int inext = temp[2 * i + 1];
    if (inext == -1) continue;
    qlist[2 * i] = _num_imgpt + _num_imgpt_q;
    qlist[2 * inext + 1] = _num_imgpt + _num_imgpt_q;
    qmap.push_back(i);
    qmap.push_back(inext);
    _num_imgpt_q++;
  }
  return true;
}

void SparseBundleCU::ProcessWeightCameraQ(vector<int>& cpnum, vector<int>& qmap,
                                          vector<float>& qmapw,
                                          vector<float>& qlistw) {
  // set average focal length and average radial distortion
  vector<float> qpnum(_num_camera, 0), qcnum(_num_camera, 0);
  vector<float> fs(_num_camera, 0), rs(_num_camera, 0);

  for (int i = 0; i < _num_camera; ++i) {
    int qi = _focal_mask[i];
    if (qi == -1) continue;
    // float ratio = _camera_data[i].f / _camera_data[qi].f;
    // if(ratio < 0.01 || ratio > 100)      continue;
    fs[qi] += _camera_data[i].f;
    rs[qi] += _camera_data[i].radial;
    qpnum[qi] += cpnum[i];
    qcnum[qi] += 1.0f;
  }

  // this seems not really matter..they will converge anyway
  for (int i = 0; i < _num_camera; ++i) {
    int qi = _focal_mask[i];
    if (qi == -1) continue;
    // float ratio = _camera_data[i].f / _camera_data[qi].f;
    // if(ratio < 0.01 || ratio > 100)      continue;
    _camera_data[i].f = fs[qi] / qcnum[qi];
    _camera_data[i].radial = rs[qi] / qcnum[qi];
  }

  qmapw.resize(_num_imgpt_q * 2, 0);
  qlistw.resize(_num_camera * 2, 0);
  for (int i = 0; i < _num_imgpt_q; ++i) {
    int cidx = qmap[i * 2], qi = _focal_mask[cidx];
    float wi = sqrt(qpnum[qi] / qcnum[qi]) * _weight_q;
    float wr = (__use_radial_distortion ? wi * _camera_data[qi].f : 0.0);
    qmapw[i * 2] = wi;
    qmapw[i * 2 + 1] = wr;
    qlistw[cidx * 2] = wi;
    qlistw[cidx * 2 + 1] = wr;
  }
}

void SparseBundleCU::ReleaseAllocatedData() {
  _cuCameraData.ReleaseData();
  _cuCameraDataEX.ReleaseData();
  _cuPointData.ReleaseData();
  _cuPointDataEX.ReleaseData();
  _cuMeasurements.ReleaseData();
  _cuImageProj.ReleaseData();
  _cuJacobianCamera.ReleaseData();
  _cuJacobianPoint.ReleaseData();
  _cuJacobianCameraT.ReleaseData();
  _cuProjectionMap.ReleaseData();
  _cuPointMeasurementMap.ReleaseData();
  _cuCameraMeasurementMap.ReleaseData();
  _cuCameraMeasurementList.ReleaseData();
  _cuCameraMeasurementListT.ReleaseData();
  _cuBufferData.ReleaseData();
  _cuBlockPC.ReleaseData();
  _cuVectorJtE.ReleaseData();
  _cuVectorJJ.ReleaseData();
  _cuVectorJX.ReleaseData();
  _cuVectorXK.ReleaseData();
  _cuVectorPK.ReleaseData();
  _cuVectorZK.ReleaseData();
  _cuVectorRK.ReleaseData();
  _cuVectorSJ.ReleaseData();
  _cuCameraQList.ReleaseData();
  _cuCameraQMap.ReleaseData();
  _cuCameraQMapW.ReleaseData();
  _cuCameraQListW.ReleaseData();
  ProgramCU::ResetCurrentDevice();
}

void SparseBundleCU::NormalizeDataF() {
  int incompatible_radial_distortion = 0;
  if (__focal_normalize) {
    if (__focal_scaling == 1.0f) {
      //------------------------------------------------------------------
      //////////////////////////////////////////////////////////////
      vector<float> focals(_num_camera);
      for (int i = 0; i < _num_camera; ++i) focals[i] = _camera_data[i].f;
      std::nth_element(focals.begin(), focals.begin() + _num_camera / 2,
                       focals.end());
      float median_focal_length = focals[_num_camera / 2];
      __focal_scaling = __data_normalize_median / median_focal_length;
      float radial_factor = median_focal_length * median_focal_length * 4.0f;

      ///////////////////////////////
      _imgpt_datax.resize(_num_imgpt * 2);
      for (int i = 0; i < _num_imgpt * 2; ++i)
        _imgpt_datax[i] = _imgpt_data[i] * __focal_scaling;
      for (int i = 0; i < _num_camera; ++i) {
        _camera_data[i].f *= __focal_scaling;
        if (!__use_radial_distortion) {
        } else if (__reset_initial_distortion) {
          _camera_data[i].radial = 0;
        } else if (_camera_data[i].distortion_type != __use_radial_distortion) {
          incompatible_radial_distortion++;
          _camera_data[i].radial = 0;
        } else if (__use_radial_distortion == -1) {
          _camera_data[i].radial *= radial_factor;
        }
      }
      if (__verbose_level > 2)
        std::cout << "Focal length normalized by " << __focal_scaling << '\n';
      __reset_initial_distortion = false;
    }
  } else {
    if (__use_radial_distortion) {
      for (int i = 0; i < _num_camera; ++i) {
        if (__reset_initial_distortion) {
          _camera_data[i].radial = 0;
        } else if (_camera_data[i].distortion_type != __use_radial_distortion) {
          _camera_data[i].radial = 0;
          incompatible_radial_distortion++;
        }
      }
      __reset_initial_distortion = false;
    }
    _imgpt_datax.resize(0);
  }

  if (incompatible_radial_distortion) {
    std::cout << "ERROR: incompatible radial distortion input; reset to 0;\n";
  }
}

void SparseBundleCU::NormalizeDataD() {
  if (__depth_scaling == 1.0f) {
    const float dist_bound = 1.0f;
    vector<float> oz(_num_imgpt);
    vector<float> cpdist1(_num_camera, dist_bound);
    vector<float> cpdist2(_num_camera, -dist_bound);
    vector<int> camnpj(_num_camera, 0), cambpj(_num_camera, 0);
    int bad_point_count = 0;
    for (int i = 0; i < _num_imgpt; ++i) {
      int cmidx = _camera_idx[i];
      CameraT* cam = _camera_data + cmidx;
      float* rz = cam->m[2];
      float* x = _point_data + 4 * _point_idx[i];
      oz[i] = (rz[0] * x[0] + rz[1] * x[1] + rz[2] * x[2] + cam->t[2]);

      /////////////////////////////////////////////////
      // points behind camera may causes big problem
      float ozr = oz[i] / cam->t[2];
      if (fabs(ozr) < __depth_check_epsilon) {
        bad_point_count++;
        float px = cam->f * (cam->m[0][0] * x[0] + cam->m[0][1] * x[1] +
                             cam->m[0][2] * x[2] + cam->t[0]);
        float py = cam->f * (cam->m[1][0] * x[0] + cam->m[1][1] * x[1] +
                             cam->m[1][2] * x[2] + cam->t[1]);
        float mx = _imgpt_data[i * 2], my = _imgpt_data[2 * i + 1];
        bool checkx = fabs(mx) > fabs(my);
        if ((checkx && px * oz[i] * mx < 0 && fabs(mx) > 64) ||
            (!checkx && py * oz[i] * my < 0 && fabs(my) > 64)) {
          if (__verbose_level > 3)
            std::cout << "Warning: proj of #" << cmidx
                      << " on the wrong side, oz = " << oz[i] << " ("
                      << (px / oz[i]) << ',' << (py / oz[i]) << ") (" << mx
                      << ',' << my << ")\n";
          /////////////////////////////////////////////////////////////////////////
          if (oz[i] > 0)
            cpdist2[cmidx] = 0;
          else
            cpdist1[cmidx] = 0;
        }
        if (oz[i] >= 0)
          cpdist1[cmidx] = std::min(cpdist1[cmidx], oz[i]);
        else
          cpdist2[cmidx] = std::max(cpdist2[cmidx], oz[i]);
      }
      if (oz[i] < 0) {
        __num_point_behind++;
        cambpj[cmidx]++;
      }
      camnpj[cmidx]++;
    }
    if (bad_point_count > 0 && __depth_degeneracy_fix) {
      if (!__focal_normalize || !__depth_normalize)
        std::cout << "Enable data normalization on degeneracy\n";
      __focal_normalize = true;
      __depth_normalize = true;
    }
    if (__depth_normalize) {
      std::nth_element(oz.begin(), oz.begin() + _num_imgpt / 2, oz.end());
      float oz_median = oz[_num_imgpt / 2];
      float shift_min = std::min(oz_median * 0.001f, 1.0f);
      float dist_threshold = shift_min * 0.1f;
      __depth_scaling = (1.0f / oz_median) / __data_normalize_median;
      if (__verbose_level > 2)
        std::cout << "Depth normalized by " << __depth_scaling << " ("
                  << oz_median << ")\n";

      for (int i = 0; i < _num_camera; ++i) {
        // move the camera a little bit?
        if (!__depth_degeneracy_fix) {
        } else if ((cpdist1[i] < dist_threshold ||
                    cpdist2[i] > -dist_threshold)) {
          float shift = shift_min;  //(cpdist1[i] <= -cpdist2[i] ? shift_min :
                                    //-shift_min);
          // if(cpdist1[i] < dist_bound && cpdist2[i] > - dist_bound) shift = -
          // 0.5f * (cpdist1[i] + cpdist2[i]);
          bool boths =
              cpdist1[i] < dist_threshold && cpdist2[i] > -dist_threshold;
          _camera_data[i].t[2] += shift;
          if (__verbose_level > 3)
            std::cout << "Adjust C" << std::setw(5) << i << " by "
                      << std::setw(12) << shift << " [B" << std::setw(2)
                      << cambpj[i] << "/" << std::setw(5) << camnpj[i] << "] ["
                      << (boths ? 'X' : ' ') << "][" << cpdist1[i] << ", "
                      << cpdist2[i] << "]\n";
          __num_camera_modified++;
        }
        _camera_data[i].t[0] *= __depth_scaling;
        _camera_data[i].t[1] *= __depth_scaling;
        _camera_data[i].t[2] *= __depth_scaling;
      }
      for (int i = 0; i < _num_point; ++i) {
        /////////////////////////////////
        _point_data[4 * i + 0] *= __depth_scaling;
        _point_data[4 * i + 1] *= __depth_scaling;
        _point_data[4 * i + 2] *= __depth_scaling;
      }
    }
    if (__num_point_behind > 0)
      std::cout << "WARNING: " << __num_point_behind
                << " points are behind cameras.\n";
    if (__num_camera_modified > 0)
      std::cout << "WARNING: " << __num_camera_modified
                << " camera moved to avoid degeneracy.\n";
  }
}

void SparseBundleCU::NormalizeData() {
  TimerBA timer(this, TIMER_PREPROCESSING);
  NormalizeDataD();
  NormalizeDataF();
}

void SparseBundleCU::DenormalizeData() {
  if (__focal_normalize && __focal_scaling != 1.0f) {
    float squared_focal_factor = (__focal_scaling * __focal_scaling);
    for (int i = 0; i < _num_camera; ++i) {
      _camera_data[i].f /= __focal_scaling;
      if (__use_radial_distortion == -1)
        _camera_data[i].radial *= squared_focal_factor;
      _camera_data[i].distortion_type = __use_radial_distortion;
    }
    _projection_sse /= squared_focal_factor;
    __focal_scaling = 1.0f;
    _imgpt_datax.resize(0);
  } else if (__use_radial_distortion) {
    for (int i = 0; i < _num_camera; ++i)
      _camera_data[i].distortion_type = __use_radial_distortion;
  }

  if (__depth_normalize && __depth_scaling != 1.0f) {
    for (int i = 0; i < _num_camera; ++i) {
      _camera_data[i].t[0] /= __depth_scaling;
      _camera_data[i].t[1] /= __depth_scaling;
      _camera_data[i].t[2] /= __depth_scaling;
    }
    for (int i = 0; i < _num_point; ++i) {
      _point_data[4 * i + 0] /= __depth_scaling;
      _point_data[4 * i + 1] /= __depth_scaling;
      _point_data[4 * i + 2] /= __depth_scaling;
    }
    __depth_scaling = 1.0f;
  }
}

void SparseBundleCU::TransferDataToHost() {
  TimerBA timer(this, TIMER_GPU_DOWNLOAD);
  _cuCameraData.CopyToHost(_camera_data);
  _cuPointData.CopyToHost(_point_data);
}

float SparseBundleCU::EvaluateProjection(CuTexImage& cam, CuTexImage& point,
                                         CuTexImage& proj) {
  ++__num_projection_eval;
  ConfigBA::TimerBA timer(this, TIMER_FUNCTION_PJ, true);
  ComputeProjection(cam, point, _cuMeasurements, _cuProjectionMap, proj,
                    __use_radial_distortion);
  if (_num_imgpt_q > 0)
    ComputeProjectionQ(cam, _cuCameraQMap, _cuCameraQMapW, proj, _num_imgpt);
  return (float)ComputeVectorNorm(proj, _cuBufferData);
}

float SparseBundleCU::EvaluateProjectionX(CuTexImage& cam, CuTexImage& point,
                                          CuTexImage& proj) {
  ++__num_projection_eval;
  ConfigBA::TimerBA timer(this, TIMER_FUNCTION_PJ, true);
  ComputeProjectionX(cam, point, _cuMeasurements, _cuProjectionMap, proj,
                     __use_radial_distortion);
  if (_num_imgpt_q > 0)
    ComputeProjectionQ(cam, _cuCameraQMap, _cuCameraQMapW, proj, _num_imgpt);
  return (float)ComputeVectorNorm(proj, _cuBufferData);
}

void SparseBundleCU::DebugProjections() {
  double e1 = 0, e2 = 0;
  for (int i = 0; i < _num_imgpt; ++i) {
    float* c = (float*)(_camera_data + _camera_idx[i]);
    float* p = _point_data + 4 * _point_idx[i];
    const float* m = _imgpt_datax.size() > 0 ? (&_imgpt_datax[i * 2])
                                             : (_imgpt_data + 2 * i);
    float* r = c + 4;
    float* t = c + 1;
    float dx1, dy1;
    ////////////////////////////////////////////////////////////////////////////////
    float z = r[6] * p[0] + r[7] * p[1] + r[8] * p[2] + t[2];
    float xx = (r[0] * p[0] + r[1] * p[1] + r[2] * p[2] + t[0]);
    float yy = (r[3] * p[0] + r[4] * p[1] + r[5] * p[2] + t[1]);
    float x = xx / z;
    float y = yy / z;
    if (__use_radial_distortion == -1) {
      float rn = (m[0] * m[0] + m[1] * m[1]) * c[13] + 1.0f;
      dx1 = c[0] * x - m[0] * rn;
      dy1 = c[0] * y - m[1] * rn;
      e1 += (dx1 * dx1 + dy1 * dy1);
      e2 += (dx1 * dx1 + dy1 * dy1) / (rn * rn);
    } else if (__use_radial_distortion) {
      float rn = (x * x + y * y) * c[13] + 1.0f;
      dx1 = c[0] * x * rn - m[0];
      dy1 = c[0] * y * rn - m[1];
      e1 += (dx1 * dx1 + dy1 * dy1) / (rn * rn);
      e2 += (dx1 * dx1 + dy1 * dy1);
    } else {
      dx1 = c[0] * x - m[0];
      dy1 = c[0] * y - m[1];
      e1 += (dx1 * dx1 + dy1 * dy1);
      e2 += (dx1 * dx1 + dy1 * dy1);
    }
    if (!isfinite(dx1) || !isfinite(dy1))
      std::cout << "x = " << xx << " y = " << yy << " z = " << z << '\n'
                << "t0 = " << t[0] << " t1 = " << t[1] << " t2 = " << t[2]
                << '\n' << "p0 = " << p[0] << " p1 = " << p[1]
                << " p2 = " << p[2] << '\n';
  }
  e1 = e1 / (__focal_scaling * __focal_scaling) / _num_imgpt;
  e2 = e2 / (__focal_scaling * __focal_scaling) / _num_imgpt;
  std::cout << "DEBUG: mean squared error = " << e1
            << " in undistorted domain;\n";
  std::cout << "DEBUG: mean squared error = " << e2
            << " in distorted domain.\n";
}

bool SparseBundleCU::InitializeStorageForCG() {
  bool success = true;
  size_t total_sz = 0;
  int plen = GetParameterLength();  // q = 8m + 4n

  //////////////////////////////////////////// 6q
  ALLOCATE_REQUIRED_DATA(_cuVectorJtE, plen, 1);
  ALLOCATE_REQUIRED_DATA(_cuVectorXK, plen, 1);
  ALLOCATE_REQUIRED_DATA(_cuVectorPK, plen, 1);
  ALLOCATE_REQUIRED_DATA(_cuVectorRK, plen, 1);
  ALLOCATE_REQUIRED_DATA(_cuVectorJJ, plen, 1);
  ALLOCATE_REQUIRED_DATA(_cuVectorZK, plen, 1);

  /////////////////////////////////
  unsigned int cblock_len = (__use_radial_distortion ? 64 : 56);
  ALLOCATE_REQUIRED_DATA(_cuBlockPC, _num_camera * cblock_len + 12 * _num_point,
                         1);  // 64m + 12n
  if (__accurate_gain_ratio) {
    ALLOCATE_REQUIRED_DATA(_cuVectorJX, _num_imgpt + _num_imgpt_q, 2);  // 2k
  } else {
    _cuVectorJX.SetTexture(_cuImageProj.data(), _num_imgpt + _num_imgpt_q, 2);
  }
  ALLOCATE_OPTIONAL_DATA(_cuVectorSJ, plen, 1, __jacobian_normalize);

  /////////////////////////////////////////
  __memory_usage += total_sz;
  if (__verbose_level > 1)
    std::cout << "Memory for Conjugate Gradient Solver:\t"
              << (total_sz / 1024 / 1024) << "MB\n";
  return success;
}

void SparseBundleCU::PrepareJacobianNormalization() {
  if (!_cuVectorSJ.IsValid()) return;

  if ((__jc_store_transpose || __jc_store_original) &&
      _cuJacobianPoint.IsValid() && !__bundle_current_mode) {
    CuTexImage null;
    null.SwapData(_cuVectorSJ);
    EvaluateJacobians();
    null.SwapData(_cuVectorSJ);
    ComputeDiagonal(_cuVectorJJ, _cuVectorSJ);
    ComputeSQRT(_cuVectorSJ);
  } else {
    CuTexImage null;
    null.SwapData(_cuVectorSJ);
    EvaluateJacobians();
    ComputeBlockPC(0, true);
    null.SwapData(_cuVectorSJ);
    _cuVectorJJ.SwapData(_cuVectorSJ);
    ProgramCU::ComputeRSQRT(_cuVectorSJ);
  }
}

void SparseBundleCU::EvaluateJacobians(bool shuffle) {
  if (__no_jacobian_store) return;
  if (__bundle_current_mode == BUNDLE_ONLY_MOTION && !__jc_store_original &&
      !__jc_store_transpose)
    return;
  ConfigBA::TimerBA timer(this, TIMER_FUNCTION_JJ, true);

  if (__jc_store_original || !__jc_store_transpose) {
    ComputeJacobian(_cuCameraData, _cuPointData, _cuJacobianCamera,
                    _cuJacobianPoint, _cuProjectionMap, _cuVectorSJ,
                    _cuMeasurements, _cuCameraMeasurementList,
                    __fixed_intrinsics, __use_radial_distortion, false);
    if (shuffle && __jc_store_transpose && _cuJacobianCameraT.IsValid())
      ShuffleCameraJacobian(_cuJacobianCamera, _cuCameraMeasurementList,
                            _cuJacobianCameraT);
  } else {
    ComputeJacobian(_cuCameraData, _cuPointData, _cuJacobianCameraT,
                    _cuJacobianPoint, _cuProjectionMap, _cuVectorSJ,
                    _cuMeasurements, _cuCameraMeasurementListT,
                    __fixed_intrinsics, __use_radial_distortion, true);
  }
  ++__num_jacobian_eval;
}

void SparseBundleCU::ComputeJtE(CuTexImage& E, CuTexImage& JtE, int mode) {
  ConfigBA::TimerBA timer(this, TIMER_FUNCTION_JTE, true);
  if (mode == 0) mode = __bundle_current_mode;
  if (__no_jacobian_store || (!__jc_store_original && !__jc_store_transpose)) {
    ProgramCU::ComputeJtE_(E, JtE, _cuCameraData, _cuPointData, _cuMeasurements,
                           _cuCameraMeasurementMap, _cuCameraMeasurementList,
                           _cuPointMeasurementMap, _cuProjectionMap,
                           _cuJacobianPoint, __fixed_intrinsics,
                           __use_radial_distortion, mode);

    ////////////////////////////////////////////////////////////////////////////////////
    if (!_cuVectorSJ.IsValid()) {
    } else if (mode == 2) {
      if (!_cuJacobianPoint.IsValid())
        ComputeVXY(JtE, _cuVectorSJ, JtE, _num_point * 4, _num_camera * 8);
    } else if (mode == 1)
      ComputeVXY(JtE, _cuVectorSJ, JtE, _num_camera * 8);
    else
      ComputeVXY(JtE, _cuVectorSJ, JtE,
                 _cuJacobianPoint.IsValid() ? _num_camera * 8 : 0);

  } else if (__jc_store_transpose) {
    ProgramCU::ComputeJtE(E, _cuJacobianCameraT, _cuCameraMeasurementMap,
                          _cuCameraMeasurementList, _cuJacobianPoint,
                          _cuPointMeasurementMap, JtE, true, mode);
  } else {
    ProgramCU::ComputeJtE(E, _cuJacobianCamera, _cuCameraMeasurementMap,
                          _cuCameraMeasurementList, _cuJacobianPoint,
                          _cuPointMeasurementMap, JtE, false, mode);
  }

  if (mode != 2 && _num_imgpt_q > 0)
    ProgramCU::ComputeJQtEC(E, _cuCameraQList, _cuCameraQListW, _cuVectorSJ,
                            JtE);
}

void SparseBundleCU::SaveBundleRecord(int iter, float res, float damping,
                                      float& g_norm, float& g_inf) {
  // do not really compute if parameter not specified...
  // for large dataset, it never converges..
  g_inf =
      __lm_check_gradient ? ComputeVectorMax(_cuVectorJtE, _cuBufferData) : 0;
  g_norm = __save_gradient_norm
               ? float(ComputeVectorNorm(_cuVectorJtE, _cuBufferData))
               : g_inf;
  ConfigBA::SaveBundleRecord(iter, res, damping, g_norm, g_inf);
}

void SparseBundleCU::ComputeJX(CuTexImage& X, CuTexImage& JX, int mode) {
  ConfigBA::TimerBA timer(this, TIMER_FUNCTION_JX, true);
  if (__no_jacobian_store || (__multiply_jx_usenoj && mode != 2) ||
      !__jc_store_original) {
    if (_cuVectorSJ.IsValid()) {
      if (mode == 0)
        ProgramCU::ComputeVXY(X, _cuVectorSJ, _cuVectorZK);
      else if (mode == 1)
        ProgramCU::ComputeVXY(X, _cuVectorSJ, _cuVectorZK, _num_camera * 8);
      else if (mode == 2)
        ProgramCU::ComputeVXY(X, _cuVectorSJ, _cuVectorZK, _num_point * 4,
                              _num_camera * 8);
      ProgramCU::ComputeJX_(_cuVectorZK, JX, _cuCameraData, _cuPointData,
                            _cuMeasurements, _cuProjectionMap,
                            __fixed_intrinsics, __use_radial_distortion, mode);
    } else {
      ProgramCU::ComputeJX_(X, JX, _cuCameraData, _cuPointData, _cuMeasurements,
                            _cuProjectionMap, __fixed_intrinsics,
                            __use_radial_distortion, mode);
    }
  } else {
    ProgramCU::ComputeJX(_num_camera * 2, X, _cuJacobianCamera,
                         _cuJacobianPoint, _cuProjectionMap, JX, mode);
  }

  if (_num_imgpt_q > 0 && mode != 2) {
    ProgramCU::ComputeJQX(X, _cuCameraQMap, _cuCameraQMapW, _cuVectorSJ, JX,
                          _num_imgpt);
  }
}

void SparseBundleCU::ComputeBlockPC(float lambda, bool dampd) {
  ConfigBA::TimerBA timer(this, TIMER_FUNCTION_BC, true);

  bool use_diagonal_q = _cuCameraQListW.IsValid() && __bundle_current_mode != 2;
  if (use_diagonal_q)
    ComputeDiagonalQ(_cuCameraQListW, _cuVectorSJ, _cuVectorJJ);

  if (__no_jacobian_store || (!__jc_store_original && !__jc_store_transpose)) {
    ComputeDiagonalBlock_(
        lambda, dampd, _cuCameraData, _cuPointData, _cuMeasurements,
        _cuCameraMeasurementMap, _cuCameraMeasurementList,
        _cuPointMeasurementMap, _cuProjectionMap, _cuJacobianPoint, _cuVectorSJ,
        _cuVectorJJ, _cuBlockPC, __fixed_intrinsics, __use_radial_distortion,
        use_diagonal_q, __bundle_current_mode);
  } else if (__jc_store_transpose) {
    ComputeDiagonalBlock(lambda, dampd, _cuJacobianCameraT,
                         _cuCameraMeasurementMap, _cuJacobianPoint,
                         _cuPointMeasurementMap, _cuCameraMeasurementList,
                         _cuVectorJJ, _cuBlockPC, __use_radial_distortion, true,
                         use_diagonal_q, __bundle_current_mode);
  } else {
    ComputeDiagonalBlock(lambda, dampd, _cuJacobianCamera,
                         _cuCameraMeasurementMap, _cuJacobianPoint,
                         _cuPointMeasurementMap, _cuCameraMeasurementList,
                         _cuVectorJJ, _cuBlockPC, __use_radial_distortion,
                         false, use_diagonal_q, __bundle_current_mode);
  }
}

void SparseBundleCU::ApplyBlockPC(CuTexImage& v, CuTexImage& pv, int mode) {
  ConfigBA::TimerBA timer(this, TIMER_FUNCTION_MP, true);
  MultiplyBlockConditioner(_num_camera, _num_point, _cuBlockPC, v, pv,
                           __use_radial_distortion, mode);
}

void SparseBundleCU::ComputeDiagonal(CuTexImage& JJ, CuTexImage& JJI) {
  ////////////////////checking the impossible.
  if (__no_jacobian_store) return;
  if (!__jc_store_transpose && !__jc_store_original) return;

  ConfigBA::TimerBA timer(this, TIMER_FUNCTION_DD, true);
  bool use_diagonal_q = _cuCameraQListW.IsValid();
  if (use_diagonal_q) {
    CuTexImage null;
    ComputeDiagonalQ(_cuCameraQListW, null, JJ);
  }
  if (__jc_store_transpose) {
    ProgramCU::ComputeDiagonal(_cuJacobianCameraT, _cuCameraMeasurementMap,
                               _cuJacobianPoint, _cuPointMeasurementMap,
                               _cuCameraMeasurementList, JJ, JJI, true,
                               __use_radial_distortion, use_diagonal_q);
  } else {
    ProgramCU::ComputeDiagonal(_cuJacobianCamera, _cuCameraMeasurementMap,
                               _cuJacobianPoint, _cuPointMeasurementMap,
                               _cuCameraMeasurementList, JJ, JJI, false,
                               __use_radial_distortion, use_diagonal_q);
  }
}

int SparseBundleCU::SolveNormalEquationPCGX(float lambda) {
  //----------------------------------------------------------
  //(Jt * J + lambda * diag(Jt * J)) X = Jt * e
  //-------------------------------------------------------------
  TimerBA timer(this, TIMER_CG_ITERATION);
  __recent_cg_status = ' ';

  // diagonal for jacobian preconditioning...
  int plen = GetParameterLength();
  CuTexImage null;
  CuTexImage& VectorDP =
      __lm_use_diagonal_damp ? _cuVectorJJ : null;  // diagonal
  ComputeBlockPC(lambda, __lm_use_diagonal_damp);

  ///////////////////////////////////////////////////////
  // B = [BC 0 ; 0 BP]
  // m = [mc 0; 0 mp];
  // A x= BC * x - JcT * Jp * mp * JpT * Jc * x
  //   = JcT * Jc x + lambda * D * x + ........
  ////////////////////////////////////////////////////////////

  CuTexImage r;
  r.SetTexture(_cuVectorRK.data(), 8 * _num_camera);
  CuTexImage p;
  p.SetTexture(_cuVectorPK.data(), 8 * _num_camera);
  CuTexImage z;
  z.SetTexture(_cuVectorZK.data(), 8 * _num_camera);
  CuTexImage x;
  x.SetTexture(_cuVectorXK.data(), 8 * _num_camera);
  CuTexImage d;
  d.SetTexture(VectorDP.data(), 8 * _num_camera);

  CuTexImage& u = _cuVectorRK;
  CuTexImage& v = _cuVectorPK;
  CuTexImage up;
  up.SetTexture(u.data() + 8 * _num_camera, 4 * _num_point);
  CuTexImage vp;
  vp.SetTexture(v.data() + 8 * _num_camera, 4 * _num_point);
  CuTexImage uc;
  uc.SetTexture(z.data(), 8 * _num_camera);

  CuTexImage& e = _cuVectorJX;
  CuTexImage& e2 = _cuImageProj;

  ApplyBlockPC(_cuVectorJtE, u, 2);
  ComputeJX(u, e, 2);
  ComputeJtE(e, uc, 1);
  ComputeSAXPY(-1.0f, uc, _cuVectorJtE, r);  // r
  ApplyBlockPC(r, p, 1);  // z = p = M r

  float_t rtz0 = (float_t)ComputeVectorDot(r, p, _cuBufferData);  // r(0)' *
                                                                  // z(0)
  ComputeJX(p, e, 1);  // Jc * x
  ComputeJtE(e, u, 2);  // JpT * jc * x
  ApplyBlockPC(u, v, 2);
  float_t qtq0 = (float_t)ComputeVectorNorm(e, _cuBufferData);  // q(0)' * q(0)
  float_t pdp0 =
      (float_t)ComputeVectorNormW(p, d, _cuBufferData);  // p(0)' * DDD * p(0)
  float_t uv0 = (float_t)ComputeVectorDot(up, vp, _cuBufferData);
  float_t alpha0 = rtz0 / (qtq0 + lambda * pdp0 - uv0);

  if (__verbose_cg_iteration)
    std::cout << " --0,\t alpha = " << alpha0
              << ", t = " << BundleTimerGetNow(TIMER_CG_ITERATION) << "\n";
  if (!isfinite(alpha0)) {
    return 0;
  }
  if (alpha0 == 0) {
    __recent_cg_status = 'I';
    return 1;
  }

  ////////////////////////////////////////////////////////////
  ComputeSAX((float)alpha0, p, x);  // x(k+1) = x(k) + a(k) * p(k)
  ComputeJX(v, e2, 2);  //                          //Jp * mp * JpT * JcT * p
  ComputeSAXPY(-1.0f, e2, e, e);
  ComputeJtE(e, uc, 1);  // JcT * ....
  ComputeSXYPZ(lambda, d, p, uc, uc);
  ComputeSAXPY((float)-alpha0, uc, r, r);  // r(k + 1) = r(k) - a(k) * A * pk

  //////////////////////////////////////////////////////////////////////////
  float_t rtzk = rtz0, rtz_min = rtz0, betak;
  int iteration = 1;
  ++__num_cg_iteration;

  while (true) {
    ApplyBlockPC(r, z, 1);

    ///////////////////////////////////////////////////////////////////////////
    float_t rtzp = rtzk;
    rtzk = (float_t)ComputeVectorDot(
        r, z, _cuBufferData);  //[r(k + 1) = M^(-1) * z(k + 1)] * z(k+1)
    float_t rtz_ratio = sqrt(fabs(rtzk / rtz0));

    if (rtz_ratio < __cg_norm_threshold) {
      if (__recent_cg_status == ' ')
        __recent_cg_status = iteration < std::min(10, __cg_min_iteration)
                                 ? '0' + iteration
                                 : 'N';
      if (iteration >= __cg_min_iteration) break;
    }
    ////////////////////////////////////////////////////////////////////////////
    betak = rtzk / rtzp;  // beta
    rtz_min = std::min(rtz_min, rtzk);

    ComputeSAXPY((float)betak, p, z, p);  // p(k) = z(k) + b(k) * p(k - 1)
    ComputeJX(p, e, 1);  // Jc * p
    ComputeJtE(e, u, 2);  // JpT * jc * p
    ApplyBlockPC(u, v, 2);
    //////////////////////////////////////////////////////////////////////

    float_t qtqk = (float_t)ComputeVectorNorm(e, _cuBufferData);  // q(k)' q(k)
    float_t pdpk =
        (float_t)ComputeVectorNormW(p, d, _cuBufferData);  // p(k)' * DDD * p(k)
    float_t uvk = (float_t)ComputeVectorDot(up, vp, _cuBufferData);
    float_t alphak = rtzk / (qtqk + lambda * pdpk - uvk);

    /////////////////////////////////////////////////////
    if (__verbose_cg_iteration)
      std::cout << " --" << iteration << ",\t alpha= " << alphak
                << ", rtzk/rtz0 = " << rtz_ratio
                << ", t = " << BundleTimerGetNow(TIMER_CG_ITERATION) << "\n";

    ///////////////////////////////////////////////////
    if (!isfinite(alphak) || rtz_ratio > __cg_norm_guard) {
      __recent_cg_status = 'X';
      break;
    }  // something doesn't converge..

    ////////////////////////////////////////////////
    ComputeSAXPY((float)alphak, p, x, x);  // x(k+1) = x(k) + a(k) * p(k)

    /////////////////////////////////////////////////
    ++iteration;
    ++__num_cg_iteration;
    if (iteration >= std::min(__cg_max_iteration, plen)) break;

    ComputeJX(v, e2, 2);  //                          //Jp * mp * JpT * JcT * p
    ComputeSAXPY(-1.0f, e2, e, e);
    ComputeJtE(e, uc, 1);  // JcT * ....
    ComputeSXYPZ(lambda, d, p, uc, uc);
    ComputeSAXPY((float)-alphak, uc, r, r);  // r(k + 1) = r(k) - a(k) * A * pk
  }

  // if(__recent_cg_status == 'X')     return iteration;

  ComputeJX(x, e, 1);
  ComputeJtE(e, u, 2);
  CuTexImage jte_p;
  jte_p.SetTexture(_cuVectorJtE.data() + 8 * _num_camera, _num_point * 4);
  ComputeSAXPY(-1.0f, up, jte_p, vp);
  ApplyBlockPC(v, _cuVectorXK, 2);
  return iteration;
}
int SparseBundleCU::SolveNormalEquationPCGB(float lambda) {
  //----------------------------------------------------------
  //(Jt * J + lambda * diag(Jt * J)) X = Jt * e
  //-------------------------------------------------------------
  TimerBA timer(this, TIMER_CG_ITERATION);
  __recent_cg_status = ' ';

  // diagonal for jacobian preconditioning...
  int plen = GetParameterLength();
  CuTexImage null;
  CuTexImage& VectorDP =
      __lm_use_diagonal_damp ? _cuVectorJJ : null;  // diagonal
  CuTexImage& VectorQK = _cuVectorZK;  // temporary
  ComputeBlockPC(lambda, __lm_use_diagonal_damp);

  ////////////////////////////////////////////////////////
  ApplyBlockPC(_cuVectorJtE,
               _cuVectorPK);  // z(0) = p(0) = M * r(0)//r(0) = Jt * e
  ComputeJX(_cuVectorPK, _cuVectorJX);  // q(0) = J * p(0)

  //////////////////////////////////////////////////
  float_t rtz0 = (float_t)ComputeVectorDot(_cuVectorJtE, _cuVectorPK,
                                           _cuBufferData);  // r(0)' * z(0)
  float_t qtq0 =
      (float_t)ComputeVectorNorm(_cuVectorJX, _cuBufferData);  // q(0)' * q(0)
  float_t ptdp0 = (float_t)ComputeVectorNormW(
      _cuVectorPK, VectorDP, _cuBufferData);  // p(0)' * DDD * p(0)
  float_t alpha0 = rtz0 / (qtq0 + lambda * ptdp0);

  if (__verbose_cg_iteration)
    std::cout << " --0,\t alpha = " << alpha0
              << ", t = " << BundleTimerGetNow(TIMER_CG_ITERATION) << "\n";
  if (!isfinite(alpha0)) {
    return 0;
  }
  if (alpha0 == 0) {
    __recent_cg_status = 'I';
    return 1;
  }

  ////////////////////////////////////////////////////////////
  ComputeSAX((float)alpha0, _cuVectorPK,
             _cuVectorXK);  // x(k+1) = x(k) + a(k) * p(k)
  ComputeJtE(_cuVectorJX, VectorQK);  // Jt * (J * p0)

  ComputeSXYPZ(lambda, VectorDP, _cuVectorPK, VectorQK,
               VectorQK);  // Jt * J * p0 + lambda * DDD * p0
  ComputeSAXPY(
      (float)-alpha0, VectorQK, _cuVectorJtE,
      _cuVectorRK);  // r(k+1) = r(k) - a(k) * (Jt * q(k)  + DDD * p(k)) ;

  float_t rtzk = rtz0, rtz_min = rtz0, betak;
  int iteration = 1;
  ++__num_cg_iteration;

  while (true) {
    ApplyBlockPC(_cuVectorRK, _cuVectorZK);

    ///////////////////////////////////////////////////////////////////////////
    float_t rtzp = rtzk;
    rtzk = (float_t)ComputeVectorDot(
        _cuVectorRK, _cuVectorZK,
        _cuBufferData);  //[r(k + 1) = M^(-1) * z(k + 1)] * z(k+1)
    float_t rtz_ratio = sqrt(fabs(rtzk / rtz0));
    if (rtz_ratio < __cg_norm_threshold) {
      if (__recent_cg_status == ' ')
        __recent_cg_status = iteration < std::min(10, __cg_min_iteration)
                                 ? '0' + iteration
                                 : 'N';
      if (iteration >= __cg_min_iteration) break;
    }

    ////////////////////////////////////////////////////////////////////////////
    betak = rtzk / rtzp;  // beta
    rtz_min = std::min(rtz_min, rtzk);

    ComputeSAXPY((float)betak, _cuVectorPK, _cuVectorZK,
                 _cuVectorPK);  // p(k) = z(k) + b(k) * p(k - 1)
    ComputeJX(_cuVectorPK, _cuVectorJX);  // q(k) = J * p(k)
    //////////////////////////////////////////////////////////////////////

    float_t qtqk =
        (float_t)ComputeVectorNorm(_cuVectorJX, _cuBufferData);  // q(k)' q(k)
    float_t ptdpk = (float_t)ComputeVectorNormW(
        _cuVectorPK, VectorDP, _cuBufferData);  // p(k)' * DDD * p(k)
    float_t alphak = rtzk / (qtqk + lambda * ptdpk);

    /////////////////////////////////////////////////////
    if (__verbose_cg_iteration)
      std::cout << " --" << iteration << ",\t alpha= " << alphak
                << ", rtzk/rtz0 = " << rtz_ratio
                << ", t = " << BundleTimerGetNow(TIMER_CG_ITERATION) << "\n";

    ///////////////////////////////////////////////////
    if (!isfinite(alphak) || rtz_ratio > __cg_norm_guard) {
      __recent_cg_status = 'X';
      break;
    }  // something doesn't converge..

    ////////////////////////////////////////////////
    ComputeSAXPY((float)alphak, _cuVectorPK, _cuVectorXK,
                 _cuVectorXK);  // x(k+1) = x(k) + a(k) * p(k)

    /////////////////////////////////////////////////
    ++iteration;
    ++__num_cg_iteration;
    if (iteration >= std::min(__cg_max_iteration, plen)) break;

    // if(iteration == 2 && rtz_ratio < __cg_norm_threshold)
    if (__cg_recalculate_freq > 0 && iteration % __cg_recalculate_freq == 0) {
      ////r = JtE - (Jt J + lambda * D) x
      ComputeJX(_cuVectorXK, _cuVectorJX);
      ComputeJtE(_cuVectorJX, VectorQK);
      ComputeSXYPZ(lambda, VectorDP, _cuVectorXK, VectorQK, VectorQK);
      ComputeSAXPY(-1.0f, VectorQK, _cuVectorJtE, _cuVectorRK);
    } else {
      ComputeJtE(_cuVectorJX, VectorQK);
      ComputeSXYPZ(lambda, VectorDP, _cuVectorPK, VectorQK, VectorQK);  //
      ComputeSAXPY(
          (float)-alphak, VectorQK, _cuVectorRK,
          _cuVectorRK);  // r(k+1) = r(k) - a(k) * (Jt * q(k)  + DDD * p(k)) ;
    }
  }
  return iteration;
}

int SparseBundleCU::SolveNormalEquation(float lambda) {
  if (__bundle_current_mode == BUNDLE_ONLY_MOTION) {
    ComputeBlockPC(lambda, __lm_use_diagonal_damp);
    ApplyBlockPC(_cuVectorJtE, _cuVectorXK, 1);
    return 1;
  } else if (__bundle_current_mode == BUNDLE_ONLY_STRUCTURE) {
    ComputeBlockPC(lambda, __lm_use_diagonal_damp);
    ApplyBlockPC(_cuVectorJtE, _cuVectorXK, 2);
    return 1;
  } else {
    ////solve linear system using Conjugate Gradients
    return __cg_schur_complement ? SolveNormalEquationPCGX(lambda)
                                 : SolveNormalEquationPCGB(lambda);
  }
}

void SparseBundleCU::RunTestIterationLM(bool reduced) {
  EvaluateProjection(_cuCameraData, _cuPointData, _cuImageProj);
  EvaluateJacobians();
  ComputeJtE(_cuImageProj, _cuVectorJtE);
  if (reduced)
    SolveNormalEquationPCGX(__lm_initial_damp);
  else
    SolveNormalEquationPCGB(__lm_initial_damp);
  UpdateCameraPoint(_cuVectorZK, _cuImageProj);
  ComputeVectorDot(_cuVectorXK, _cuVectorJtE, _cuBufferData);
  ComputeJX(_cuVectorXK, _cuVectorJX);
  ComputeVectorNorm(_cuVectorJX, _cuBufferData);
}

float SparseBundleCU::UpdateCameraPoint(CuTexImage& dx,
                                        CuTexImage& cuImageTempProj) {
  ConfigBA::TimerBA timer(this, TIMER_FUNCTION_UP, true);
  if (__bundle_current_mode == BUNDLE_ONLY_MOTION) {
    if (__jacobian_normalize)
      ComputeVXY(_cuVectorXK, _cuVectorSJ, dx, 8 * _num_camera);
    ProgramCU::UpdateCameraPoint(_num_camera, _cuCameraData, _cuPointData, dx,
                                 _cuCameraDataEX, _cuPointDataEX,
                                 __bundle_current_mode);
    return EvaluateProjection(_cuCameraDataEX, _cuPointData, cuImageTempProj);
  } else if (__bundle_current_mode == BUNDLE_ONLY_STRUCTURE) {
    if (__jacobian_normalize)
      ComputeVXY(_cuVectorXK, _cuVectorSJ, dx, 4 * _num_point, 8 * _num_camera);
    ProgramCU::UpdateCameraPoint(_num_camera, _cuCameraData, _cuPointData, dx,
                                 _cuCameraDataEX, _cuPointDataEX,
                                 __bundle_current_mode);
    return EvaluateProjection(_cuCameraData, _cuPointDataEX, cuImageTempProj);
  } else {
    if (__jacobian_normalize) ComputeVXY(_cuVectorXK, _cuVectorSJ, dx);
    ProgramCU::UpdateCameraPoint(_num_camera, _cuCameraData, _cuPointData, dx,
                                 _cuCameraDataEX, _cuPointDataEX,
                                 __bundle_current_mode);
    return EvaluateProjection(_cuCameraDataEX, _cuPointDataEX, cuImageTempProj);
  }
}

float SparseBundleCU::SaveUpdatedSystem(float residual_reduction,
                                        float dx_sqnorm, float damping) {
  float expected_reduction;
  if (__bundle_current_mode == BUNDLE_ONLY_MOTION) {
    CuTexImage xk;
    xk.SetTexture(_cuVectorXK.data(), 8 * _num_camera);
    CuTexImage jte;
    jte.SetTexture(_cuVectorJtE.data(), 8 * _num_camera);
    float dxtg = (float)ComputeVectorDot(xk, jte, _cuBufferData);
    if (__lm_use_diagonal_damp) {
      CuTexImage jj;
      jj.SetTexture(_cuVectorJJ.data(), 8 * _num_camera);
      float dq = (float)ComputeVectorNormW(xk, jj, _cuBufferData);
      expected_reduction = damping * dq + dxtg;
    } else {
      expected_reduction = damping * dx_sqnorm + dxtg;
    }
    _cuCameraData.SwapData(_cuCameraDataEX);
  } else if (__bundle_current_mode == BUNDLE_ONLY_STRUCTURE) {
    CuTexImage xk;
    xk.SetTexture(_cuVectorXK.data() + 8 * _num_camera, 4 * _num_point);
    CuTexImage jte;
    jte.SetTexture(_cuVectorJtE.data() + 8 * _num_camera, 4 * _num_point);
    float dxtg = (float)ComputeVectorDot(xk, jte, _cuBufferData);
    if (__lm_use_diagonal_damp) {
      CuTexImage jj;
      jj.SetTexture(_cuVectorJJ.data() + 8 * _num_camera, 4 * _num_point);
      float dq = (float)ComputeVectorNormW(xk, jj, _cuBufferData);
      expected_reduction = damping * dq + dxtg;
    } else {
      expected_reduction = damping * dx_sqnorm + dxtg;
    }
    _cuPointData.SwapData(_cuPointDataEX);
  } else {
    float dxtg =
        (float)ComputeVectorDot(_cuVectorXK, _cuVectorJtE, _cuBufferData);

    if (__accurate_gain_ratio) {
      ComputeJX(_cuVectorXK, _cuVectorJX);
      float njx = (float)ComputeVectorNorm(_cuVectorJX, _cuBufferData);
      expected_reduction = 2.0f * dxtg - njx;
      // could the expected reduction be negative??? not sure
      if (expected_reduction <= 0)
        expected_reduction = 0.001f * residual_reduction;
    } else if (__lm_use_diagonal_damp) {
      float dq =
          (float)ComputeVectorNormW(_cuVectorXK, _cuVectorJJ, _cuBufferData);
      expected_reduction = damping * dq + dxtg;
    } else {
      expected_reduction = damping * dx_sqnorm + dxtg;
    }

    /// save the new motion/struture
    _cuCameraData.SwapData(_cuCameraDataEX);
    _cuPointData.SwapData(_cuPointDataEX);

    //_cuCameraData.CopyToHost(_camera_data);
    //_cuPointData.CopyToHost(_point_data);
    // DebugProjections();
  }
  ////////////////////////////////////////////
  return float(residual_reduction / expected_reduction);
}

void SparseBundleCU::AdjustBundleAdjsutmentMode() {
  if (__bundle_current_mode == BUNDLE_ONLY_STRUCTURE) {
    _cuJacobianCamera.InitTexture(0, 0);
    _cuJacobianCameraT.InitTexture(0, 0);
  }
}

float SparseBundleCU::EvaluateDeltaNorm() {
  if (__bundle_current_mode == BUNDLE_ONLY_MOTION) {
    CuTexImage temp;
    temp.SetTexture(_cuVectorXK.data(), 8 * _num_camera);
    return ComputeVectorNorm(temp, _cuBufferData);

  } else if (__bundle_current_mode == BUNDLE_ONLY_STRUCTURE) {
    CuTexImage temp;
    temp.SetTexture(_cuVectorXK.data() + 8 * _num_camera, 4 * _num_point);
    return ComputeVectorNorm(temp, _cuBufferData);
  } else {
    return (float)ComputeVectorNorm(_cuVectorXK, _cuBufferData);
  }
}

void SparseBundleCU::NonlinearOptimizeLM() {
  ////////////////////////////////////////
  TimerBA timer(this, TIMER_OPTIMIZATION);

  ////////////////////////////////////////////////
  float mse_convert_ratio =
      1.0f / (_num_imgpt * __focal_scaling * __focal_scaling);
  float error_display_ratio = __verbose_sse ? _num_imgpt : 1.0f;
  const int edwidth = __verbose_sse ? 12 : 8;
  _projection_sse =
      EvaluateProjection(_cuCameraData, _cuPointData, _cuImageProj);
  __initial_mse = __final_mse = _projection_sse * mse_convert_ratio;

  // compute jacobian diagonals for normalization
  if (__jacobian_normalize) PrepareJacobianNormalization();

  // evalaute jacobian
  EvaluateJacobians();
  ComputeJtE(_cuImageProj, _cuVectorJtE);
  ///////////////////////////////////////////////////////////////
  if (__verbose_level)
    std::cout << "Initial " << (__verbose_sse ? "sumed" : "mean")
              << " squared error = " << __initial_mse * error_display_ratio
              << "\n----------------------------------------------\n";

  //////////////////////////////////////////////////
  CuTexImage& cuImageTempProj = _cuVectorJX;
  // CuTexImage& cuVectorTempJX  =   _cuVectorJX;
  CuTexImage& cuVectorDX = _cuVectorSJ.IsValid() ? _cuVectorZK : _cuVectorXK;

  //////////////////////////////////////////////////
  float damping_adjust = 2.0f, damping = __lm_initial_damp, g_norm, g_inf;
  SaveBundleRecord(0, _projection_sse * mse_convert_ratio, damping, g_norm,
                   g_inf);

  ////////////////////////////////////
  std::cout << std::left;
  for (int i = 0; i < __lm_max_iteration && !__abort_flag;
       __current_iteration = (++i)) {
    ////solve linear system
    int num_cg_iteration = SolveNormalEquation(damping);

    // there must be NaN somewhere
    if (num_cg_iteration == 0) {
      if (__verbose_level)
        std::cout << "#" << std::setw(3) << i << " quit on numeric errors\n";
      __pba_return_code = 'E';
      break;
    }

    // there must be infinity somewhere
    if (__recent_cg_status == 'I') {
      std::cout << "#" << std::setw(3) << i << " 0  I e=" << std::setw(edwidth)
                << "------- "
                << " u=" << std::setprecision(3) << std::setw(9) << damping
                << '\n' << std::setprecision(6);
      /////////////increase damping factor
      damping = damping * damping_adjust;
      damping_adjust = 2.0f * damping_adjust;
      --i;
      continue;
    }

    /////////////////////
    ++__num_lm_iteration;

    ////////////////////////////////////
    float dx_sqnorm = EvaluateDeltaNorm(), dx_norm = sqrt(dx_sqnorm);

    // In this library, we check absolute difference instead of realtive
    // difference
    if (dx_norm <= __lm_delta_threshold) {
      // damping factor must be way too big...or it converges
      if (__verbose_level > 1)
        std::cout << "#" << std::setw(3) << i << " " << std::setw(3)
                  << num_cg_iteration << char(__recent_cg_status)
                  << " quit on too small change (" << dx_norm << "  < "
                  << __lm_delta_threshold << ")\n";
      __pba_return_code = 'S';
      break;
    }
    ///////////////////////////////////////////////////////////////////////
    // update structure and motion, check reprojection error
    float new_residual = UpdateCameraPoint(cuVectorDX, cuImageTempProj);
    float average_residual = new_residual * mse_convert_ratio;
    float residual_reduction = _projection_sse - new_residual;

    // do we find a better solution?
    if (isfinite(new_residual) && residual_reduction > 0) {
      ////compute relative norm change
      float relative_reduction = 1.0f - (new_residual / _projection_sse);

      ////////////////////////////////////
      __num_lm_success++;  // increase counter
      _projection_sse = new_residual;  // save the new residual
      _cuImageProj.SwapData(cuImageTempProj);  // save the new projection

      ///////////////gain ratio////////////////////
      float gain_ratio =
          SaveUpdatedSystem(residual_reduction, dx_sqnorm, damping);

      /////////////////////////////////////
      SaveBundleRecord(i + 1, _projection_sse * mse_convert_ratio, damping,
                       g_norm, g_inf);

      /////////////////////////////////////////////
      if (__verbose_level > 1)
        std::cout << "#" << std::setw(3) << i << " " << std::setw(3)
                  << num_cg_iteration << char(__recent_cg_status)
                  << " e=" << std::setw(edwidth)
                  << average_residual * error_display_ratio
                  << " u=" << std::setprecision(3) << std::setw(9) << damping
                  << " r=" << std::setw(6)
                  << floor(gain_ratio * 1000.f) * 0.001f
                  << " g=" << std::setw(g_norm > 0 ? 9 : 1) << g_norm << " "
                  << std::setw(9) << relative_reduction << ' ' << std::setw(9)
                  << dx_norm << " t=" << int(BundleTimerGetNow()) << "\n"
                  << std::setprecision(6);

      /////////////////////////////
      if (!IsTimeBudgetAvailable()) {
        if (__verbose_level > 1)
          std::cout << "#" << std::setw(3) << i << " used up time budget.\n";
        __pba_return_code = 'T';
        break;
      } else if (__lm_check_gradient && g_inf < __lm_gradient_threshold) {
        if (__verbose_level > 1)
          std::cout << "#" << std::setw(3) << i
                    << " converged with small gradient\n";
        __pba_return_code = 'G';
        break;
      } else if (average_residual * error_display_ratio <= __lm_mse_threshold) {
        if (__verbose_level > 1)
          std::cout << "#" << std::setw(3) << i << " satisfies MSE threshold\n";
        __pba_return_code = 'M';
        break;
      } else {
        /////////////////////////////adjust damping factor
        float temp = gain_ratio * 2.0f - 1.0f;
        float adaptive_adjust = 1.0f - temp * temp * temp;  // powf(, 3.0f); //
        float auto_adjust = std::max(1.0f / 3.0f, adaptive_adjust);

        //////////////////////////////////////////////////
        damping = damping * auto_adjust;
        damping_adjust = 2.0f;
        if (damping < __lm_minimum_damp)
          damping = __lm_minimum_damp;
        else if (__lm_damping_auto_switch == 0 && damping > __lm_maximum_damp &&
                 __lm_use_diagonal_damp)
          damping = __lm_maximum_damp;

        EvaluateJacobians();
        ComputeJtE(_cuImageProj, _cuVectorJtE);
      }
    } else {
      if (__verbose_level > 1)
        std::cout << "#" << std::setw(3) << i << " " << std::setw(3)
                  << num_cg_iteration << char(__recent_cg_status)
                  << " e=" << std::setw(edwidth) << std::left
                  << average_residual * error_display_ratio
                  << " u=" << std::setprecision(3) << std::setw(9) << damping
                  << " r=----- " << (__lm_check_gradient || __save_gradient_norm
                                         ? " g=---------"
                                         : " g=0")
                  << " --------- " << std::setw(9) << dx_norm
                  << " t=" << int(BundleTimerGetNow()) << "\n"
                  << std::setprecision(6);

      if (__lm_damping_auto_switch > 0 && __lm_use_diagonal_damp &&
          damping > __lm_damping_auto_switch) {
        __lm_use_diagonal_damp = false;
        damping = __lm_damping_auto_switch;
        damping_adjust = 2.0f;
        if (__verbose_level > 1)
          std::cout << "NOTE: switch to damping with an identity matix\n";
      } else {
        /////////////increase damping factor
        damping = damping * damping_adjust;
        damping_adjust = 2.0f * damping_adjust;
      }
    }

    if (__verbose_level == 1) std::cout << '.';
  }

  __final_mse = float(_projection_sse * mse_convert_ratio);
  __final_mse_x =
      __use_radial_distortion
          ? EvaluateProjectionX(_cuCameraData, _cuPointData, _cuImageProj) *
                mse_convert_ratio
          : __final_mse;
}

#define PROFILE_(A, B)                    \
  BundleTimerStart(TIMER_PROFILE_STEP);   \
  for (int i = 0; i < repeat; ++i) {      \
    B;                                    \
    FinishWorkCUDA();                     \
  }                                       \
  BundleTimerSwitch(TIMER_PROFILE_STEP);  \
  std::cout << std::setw(24) << A << ": " \
            << (BundleTimerGet(TIMER_PROFILE_STEP) / repeat) << "\n";

#define PROFILE(A, B) PROFILE_(#A, A B)
#define PROXILE(A, B) PROFILE_(A, B)

void SparseBundleCU::RunProfileSteps() {
  const int repeat = __profile_pba;
  std::cout << "---------------------------------\n"
               "|    Run profiling steps ("
            << repeat << ")  |\n"
                         "---------------------------------\n"
            << std::left;
  ;

  ///////////////////////////////////////////////
  PROXILE("Upload Measurements",
          _cuMeasurements.CopyFromHost(
              _imgpt_datax.size() > 0 ? &_imgpt_datax[0] : _imgpt_data));
  PROXILE("Upload Point Data", _cuPointData.CopyToHost(_point_data));
  std::cout << "---------------------------------\n";

  /////////////////////////////////////////////
  EvaluateProjection(_cuCameraData, _cuPointData, _cuImageProj);
  PrepareJacobianNormalization();
  EvaluateJacobians();
  ComputeJtE(_cuImageProj, _cuVectorJtE);
  ComputeBlockPC(__lm_initial_damp, true);
  FinishWorkCUDA();

  do {
    if (SolveNormalEquationPCGX(__lm_initial_damp) == 10 &&
        SolveNormalEquationPCGB(__lm_initial_damp) == 10)
      break;
    __lm_initial_damp *= 2.0f;
  } while (__lm_initial_damp < 1024.0f);
  std::cout << "damping set to " << __lm_initial_damp << " for profiling\n"
            << "---------------------------------\n";

  {
    int repeat = 10, cgmin = __cg_min_iteration, cgmax = __cg_max_iteration;
    __cg_max_iteration = __cg_min_iteration = 10;
    __num_cg_iteration = 0;
    PROFILE(SolveNormalEquationPCGX, (__lm_initial_damp));
    if (__num_cg_iteration != 100)
      std::cout << __num_cg_iteration << " cg iterations in all\n";

    /////////////////////////////////////////////////////////////////////
    __num_cg_iteration = 0;
    PROFILE(SolveNormalEquationPCGB, (__lm_initial_damp));
    if (__num_cg_iteration != 100)
      std::cout << __num_cg_iteration << " cg iterations in all\n";
    std::cout << "---------------------------------\n";
    //////////////////////////////////////////////////////
    __num_cg_iteration = 0;
    PROXILE("Single iteration LMX", RunTestIterationLM(true));
    if (__num_cg_iteration != 100)
      std::cout << __num_cg_iteration << " cg iterations in all\n";
    ////////////////////////////////////////////////////////
    __num_cg_iteration = 0;
    PROXILE("Single iteration LMB", RunTestIterationLM(false));
    if (__num_cg_iteration != 100)
      std::cout << __num_cg_iteration << " cg iterations in all\n";
    std::cout << "---------------------------------\n";
    __cg_max_iteration = cgmax;
    __cg_min_iteration = cgmin;
  }
  /////////////////////////////////////////////////////
  PROFILE(UpdateCameraPoint, (_cuVectorZK, _cuImageProj));
  PROFILE(ComputeVectorNorm, (_cuVectorXK, _cuBufferData));
  PROFILE(ComputeVectorDot, (_cuVectorXK, _cuVectorRK, _cuBufferData));
  PROFILE(ComputeVectorNormW, (_cuVectorXK, _cuVectorRK, _cuBufferData));
  PROFILE(ComputeSAXPY, (0.01f, _cuVectorXK, _cuVectorRK, _cuVectorZK));
  PROFILE(ComputeSXYPZ,
          (0.01f, _cuVectorXK, _cuVectorPK, _cuVectorRK, _cuVectorZK));
  std::cout << "---------------------------------\n";
  PROFILE(ComputeVectorNorm, (_cuImageProj, _cuBufferData));
  PROFILE(ComputeSAXPY, (0.000f, _cuImageProj, _cuVectorJX, _cuVectorJX));
  std::cout << "---------------------------------\n";

  __multiply_jx_usenoj = false;
  ///////////////////////////////////////////////////////
  PROFILE(EvaluateProjection, (_cuCameraData, _cuPointData, _cuImageProj));
  PROFILE(ApplyBlockPC, (_cuVectorJtE, _cuVectorPK));
  /////////////////////////////////////////////////
  if (!__no_jacobian_store) {
    if (__jc_store_original) {
      PROFILE(ComputeJX, (_cuVectorJtE, _cuVectorJX));
      PROFILE(EvaluateJacobians, (false));

      if (__jc_store_transpose) {
        PROFILE(
            ShuffleCameraJacobian,
            (_cuJacobianCamera, _cuCameraMeasurementList, _cuJacobianCameraT));
        PROFILE(ComputeDiagonal, (_cuVectorJJ, _cuVectorPK));
        PROFILE(ComputeJtE, (_cuImageProj, _cuVectorJtE));
        PROFILE(ComputeBlockPC, (0.001f, true));

        std::cout << "---------------------------------\n"
                     "|   Not storing original  JC    | \n"
                     "---------------------------------\n";
        __jc_store_original = false;
        PROFILE(EvaluateJacobians, ());
        __jc_store_original = true;
      }
      //////////////////////////////////////////////////

      std::cout << "---------------------------------\n"
                   "|   Not storing transpose JC    | \n"
                   "---------------------------------\n";
      __jc_store_transpose = false;
      PROFILE(ComputeDiagonal, (_cuVectorJJ, _cuVectorPK));
      PROFILE(ComputeJtE, (_cuImageProj, _cuVectorJtE));
      PROFILE(ComputeBlockPC, (0.001f, true));

      //////////////////////////////////////

    } else if (__jc_store_transpose) {
      PROFILE(ComputeDiagonal, (_cuVectorJJ, _cuVectorPK));
      PROFILE(ComputeJtE, (_cuImageProj, _cuVectorJtE));
      PROFILE(ComputeBlockPC, (0.001f, true));
      std::cout << "---------------------------------\n"
                   "|   Not storing original  JC    | \n"
                   "---------------------------------\n";
      PROFILE(EvaluateJacobians, ());
    }
  }

  if (!__no_jacobian_store) {
    std::cout << "---------------------------------\n"
                 "| Not storing Camera Jacobians  | \n"
                 "---------------------------------\n";
    __jc_store_transpose = false;
    __jc_store_original = false;
    _cuJacobianCamera.ReleaseData();
    _cuJacobianCameraT.ReleaseData();
    PROFILE(EvaluateJacobians, ());
    PROFILE(ComputeJtE, (_cuImageProj, _cuVectorJtE));
    PROFILE(ComputeBlockPC, (0.001f, true));
  }

  ///////////////////////////////////////////////

  std::cout << "---------------------------------\n"
               "|   Not storing any jacobians   |\n"
               "---------------------------------\n";
  __no_jacobian_store = true;
  _cuJacobianPoint.ReleaseData();
  PROFILE(ComputeJX, (_cuVectorJtE, _cuVectorJX));
  PROFILE(ComputeJtE, (_cuImageProj, _cuVectorJtE));
  PROFILE(ComputeBlockPC, (0.001f, true));

  std::cout << "---------------------------------\n";
}

void SparseBundleCU::RunDebugSteps() {
  EvaluateProjection(_cuCameraData, _cuPointData, _cuImageProj);
  EvaluateJacobians();
  ComputeJtE(_cuImageProj, _cuVectorJtE);
  // DEBUG_FUNCN(_cuVectorXK, SolveNormalEquationPCGB, (0.001f), 100);
  DEBUG_FUNCN(_cuVectorJtE, ComputeJtE, (_cuImageProj, _cuVectorJtE), 100);
  DEBUG_FUNCN(_cuVectorJX, ComputeJX, (_cuVectorJtE, _cuVectorJX), 100);
}

void SparseBundleCU::SaveNormalEquation(float lambda) {
  ofstream out1("../../matlab/cg_j.txt");
  ofstream out2("../../matlab/cg_b.txt");
  ofstream out3("../../matlab/cg_x.txt");

  out1 << std::setprecision(20);
  out2 << std::setprecision(20);
  out3 << std::setprecision(20);

  int plen = GetParameterLength();
  vector<float> jc(16 * _num_imgpt);
  vector<float> jp(8 * _num_imgpt);
  vector<float> ee(2 * _num_imgpt);
  vector<float> dx(plen);

  _cuJacobianCamera.CopyToHost(&jc[0]);
  _cuJacobianPoint.CopyToHost(&jp[0]);
  _cuImageProj.CopyToHost(&ee[0]);
  _cuVectorXK.CopyToHost(&dx[0]);

  for (int i = 0; i < _num_imgpt; ++i) {
    out2 << ee[i * 2] << ' ' << ee[i * 2 + 1] << ' ';
    int cidx = _camera_idx[i], pidx = _point_idx[i];
    float *cp = &jc[i * 16], *pp = &jp[i * 8];
    int cmin = cidx * 8, pmin = 8 * _num_camera + pidx * 4;
    for (int j = 0; j < 8; ++j)
      out1 << (i * 2 + 1) << ' ' << (cmin + j + 1) << ' ' << cp[j] << '\n';
    for (int j = 0; j < 8; ++j)
      out1 << (i * 2 + 2) << ' ' << (cmin + j + 1) << ' ' << cp[j + 8] << '\n';
    for (int j = 0; j < 4; ++j)
      out1 << (i * 2 + 1) << ' ' << (pmin + j + 1) << ' ' << pp[j] << '\n';
    for (int j = 0; j < 4; ++j)
      out1 << (i * 2 + 2) << ' ' << (pmin + j + 1) << ' ' << pp[j + 4] << '\n';
  }

  for (size_t i = 0; i < dx.size(); ++i) out3 << dx[i] << ' ';

  std::cout << "lambda = " << std::setprecision(20) << lambda << '\n';
}

}  // namespace pba