/******************************************************************************
 * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#include "stdafx.h"
#ifndef CFG_DEBUG
//#define CFG_DEBUG
#endif
#include "IBA.h"
#include "IBA_internal.h"

#ifndef _WIN32
#include <boost/filesystem.hpp>
#endif

namespace IBA {

static inline void ConvertCamera(const CameraIMUState &Ci, Camera *Co) {
  if (Ci.C.R[0][0] == FLT_MAX) {
    Co->Invalidate();
    return;
  }
  Co->m_T.LA::AlignedMatrix3x3f::Set(Ci.C.R);
  Co->m_p.Set(Ci.C.p);
  Co->m_T.SetPosition(Co->m_p);
  Co->m_v.Set(Ci.v);
  Co->m_ba.Set(Ci.ba);
  Co->m_bw.Set(Ci.bw);
}

static inline void ConvertCamera(const CameraPose &Ci, Rigid3D *Co) {
  if (Ci.R[0][0] == FLT_MAX) {
    Co->Invalidate();
    return;
  }
  Co->LA::AlignedMatrix3x3f::Set(Ci.R);
  const ::Point3D p = Ci.p;
  Co->SetPosition(p);
}

static inline void ConvertCamera(const Camera &Ci, CameraIMUState *Co) {
  Ci.m_T.LA::AlignedMatrix3x3f::Get(Co->C.R);
  Ci.m_p.Get(Co->C.p);
  Ci.m_v.Get(Co->v);
  Ci.m_ba.Get(Co->ba);
  Ci.m_bw.Get(Co->bw);
}

static inline void ConvertDepth(const Depth &di, ::Depth::InverseGaussian *_do) {
  _do->u() = di.d;
  _do->s2() = di.s2;
}

static inline void ConvertDepth(const ::Depth::InverseGaussian &di, Depth *_do) {
  _do->d = di.u();
  _do->s2 = di.s2();
}

Solver::Solver() {
  m_internal = NULL;
}

Solver::~Solver() {
#ifdef CFG_DEBUG
  UT_ASSERT(m_internal == NULL);
#endif
}

void Solver::Create(const Calibration &K, const int serial, const int verbose, const int debug,
                    const int history, const std::string param, const std::string dir,
                    const std::vector<CameraIMUState> *XsGT, const std::vector<Depth> *dsGT) {
#ifdef CFG_DEBUG
  UT_ASSERT(m_internal == NULL);
#endif
  m_internal = new Internal();
#ifdef CFG_DEBUG
  UT_ASSERT(m_internal != NULL);
#ifndef WIN32
  UT_ASSERT((reinterpret_cast<std::uintptr_t>(m_internal) & (SIMD_ALIGN - 1)) == 0);
#endif
#endif
  Camera::Calibration &_K = m_internal->m_K;
  _K.m_K.Set(K.w, K.h, K.K.fx, K.K.fy, K.K.cx, K.K.cy, K.K.ds, K.fishEye);
  const Rigid3D Tu(K.Tu);
  _K.m_Ru = Tu;
  _K.m_pu = Tu.GetTranslation();
#ifdef CFG_STEREO
  const Rigid3D Tr(K.Tr);
  _K.m_Kr.Set(K.w, K.h, K.Kr.fx, K.Kr.fy, K.Kr.cx, K.Kr.cy, K.Kr.ds, K.fishEye,
              //K.K.fx, K.K.fy,
              _K.m_K.fx(), _K.m_K.fy(),
              &Tr);
  _K.m_Rr = Tr;
  _K.m_br = Tr.GetTranslation().GetMinus();
#endif
  _K.m_ba.Set(K.ba);
  _K.m_bw.Set(K.bw);
  //_K.m_sa.Set(K.sa);
  LoadParameters(param);
  AlignedVector<Camera> &CsGT = m_internal->m_CsGT;
  if (XsGT) {
    const int N = static_cast<int>(XsGT->size());
    CsGT.Resize(N);
    for (int i = 0; i < N; ++i) {
      Camera &C = CsGT[i];
      ConvertCamera(XsGT->at(i), &C);
      C.m_ba -= _K.m_ba;
      C.m_bw -= _K.m_bw;
    }
  } else {
    CsGT.Clear();
  }
  std::vector<::Depth::InverseGaussian> &DsGT = m_internal->m_DsGT;
  if (dsGT) {
    const int N = static_cast<int>(dsGT->size());
    DsGT.resize(N);
    for (int i = 0; i < N; ++i) {
      ConvertDepth(dsGT->at(i), &DsGT[i]);
    }
  } else {
    DsGT.clear();
  }
  m_internal->m_dir = dir;
  m_internal->m_LBA.Initialize(this, serial & 255, verbose & 255,
                               static_cast<int>(static_cast<char>(debug & 255)),
                               static_cast<int>(static_cast<char>(history & 255)));
  m_internal->m_GBA.Initialize(this, (serial >> 8) & 255, (verbose >> 8) & 255,
                               static_cast<int>(static_cast<char>((debug >> 8) & 255)),
                               static_cast<int>(static_cast<char>((history >> 8) & 255)));
}

void Solver::Destroy() {
  delete m_internal;
  m_internal = NULL;
}

void Solver::Start() {
  m_internal->m_LBA.Start();
  m_internal->m_GBA.Start();
  m_internal->m_nFrms = 0;
  m_internal->m_Ts.resize(0);
  m_internal->m_LM.IBA_Reset();
  m_internal->m_iFrmsKF.resize(0);
  m_internal->m_iKF2d.assign(1, 0);
  m_internal->m_id2X.resize(0);
  m_internal->m_iX2d.resize(0);
  m_internal->m_id2idx.resize(0);
  m_internal->m_idx2iX.resize(0);
  m_internal->m_CsKF.Resize(0);
  m_internal->m_xs.resize(0);
  m_internal->m_ds.resize(0);
#ifdef CFG_GROUND_TRUTH
  m_internal->m_dsGT.resize(0);
  m_internal->m_tsGT.Resize(0);
  m_internal->m_zsGT.resize(0);
#endif
}

void Solver::Stop() {
  m_internal->m_LBA.Stop();
  m_internal->m_GBA.Stop();
}

void Solver::PushCurrentFrame(const CurrentFrame &CF, const KeyFrame *KF) {
  if (KF) {
    //UT::Print("[%d]\n", KF->iFrm);
    if (KF->iFrm == CF.iFrm) {
      m_internal->PushCurrentFrame(CF);
      m_internal->PushKeyFrame(*KF, &m_internal->m_ILF.m_C);
    } else {
      m_internal->PushKeyFrame(*KF);
      m_internal->PushCurrentFrame(CF);
    }
    m_internal->m_LM.IBA_PushCurrentFrame(LocalMap::CameraLF(m_internal->m_ILF.m_C, CF.iFrm),
                                          m_internal->m_IKF);
    m_internal->m_LBA.PushCurrentFrame(m_internal->m_ILF, m_internal->m_IKF);
  } else {
    const LocalBundleAdjustor::InputLocalFrame &ILF = m_internal->PushCurrentFrame(CF);
    m_internal->m_LM.IBA_PushCurrentFrame(LocalMap::CameraLF(ILF.m_C, CF.iFrm));
    m_internal->m_LBA.PushCurrentFrame(ILF);
  }
  m_internal->m_LBA.WakeUp();
}

bool Solver::PushRelativeConstraint(const RelativeConstraint &Z) {
  if (Z.iFrm1 == -1 || Z.iFrm2 == -1) {
    return false;
  }
#ifdef CFG_DEBUG
  UT_ASSERT(Z.iFrm1 < Z.iFrm2);
#endif
  const std::vector<int> &iFrmsKF = m_internal->m_iFrmsKF;
  const std::vector<int>::const_iterator i1 = std::lower_bound(iFrmsKF.begin(), iFrmsKF.end(),
                                                               Z.iFrm1);
  if (i1 == iFrmsKF.end() || *i1 != Z.iFrm1) {
    return false;
  }
  const std::vector<int>::const_iterator i2 = std::lower_bound(i1, iFrmsKF.end(), Z.iFrm2);
  if (i2 == iFrmsKF.end() || *i2 != Z.iFrm2) {
    return false;
  }
  Rigid3D T;
  LA::AlignedVector3f p;
  T.Rotation3D::Set(Z.T.R);
  p.Set(Z.T.p);
  T.SetPosition(p);
  LA::AlignedMatrix6x6f S;
  S.Set(Z.S);
  CameraPrior::Pose _Z;
  if (!_Z.Initialize(BA_WEIGHT_PRIOR_CAMERA_RELATIVE_CONSTRAINT,
                     static_cast<int>(i1 - iFrmsKF.begin()),
                     static_cast<int>(i2 - iFrmsKF.begin()), T, S)) {
    return false;
  }
  //////////////////////////////////////////////////////////////////////////
  //const int verboseBkp = m_internal->m_GBA.m_verbose;
  //m_internal->m_GBA.m_verbose = 2;
  //////////////////////////////////////////////////////////////////////////
  m_internal->m_GBA.PushCameraPriorPose(Z.iFrm2, _Z);
  m_internal->m_GBA.WakeUp();
  //////////////////////////////////////////////////////////////////////////
  //m_internal->m_GBA.m_verbose = verboseBkp;
  //////////////////////////////////////////////////////////////////////////
  return true;
}

#ifdef CFG_GROUND_TRUTH
static inline void OffsetPointer(const LA::AlignedVector3f *t0,
                                 const AlignedVector<LA::AlignedVector3f> &ts,
                                 std::vector<std::vector<::Depth::Measurement> > *zs) {
  if (ts.Data() == t0) {
    return;
  }
  const int NX = static_cast<int>(zs->size());
  for (int iX = 0; iX < NX; ++iX) {
    std::vector<::Depth::Measurement> &_zs = zs->at(iX);
    const int N = static_cast<int>(_zs.size());
    for (int i = 0; i < N; ++i) {
      ::Depth::Measurement &z = _zs[i];
      //const int it1 = static_cast<int>(z.m_t - t0);
      z.m_t = ts.Data() + static_cast<int>(z.m_t - t0);
      //const int it2 = static_cast<int>(z.m_t - ts.Data());
#ifdef CFG_DEBUG
      UT_ASSERT(z.m_t >= ts.Data() && z.m_t < ts.End());
#endif
    }
  }
}
static inline int PushTranslation(const LA::AlignedVector3f &t,
                                  AlignedVector<LA::AlignedVector3f> *ts,
                                  std::vector<std::vector<::Depth::Measurement> > *zs) {
  const int it = ts->Size();
  if (it + 1 > ts->Capacity()) {
    const LA::AlignedVector3f *t0 = ts->Data();
    ts->Push(t);
    OffsetPointer(t0, *ts, zs);
  } else {
    ts->Push(t);
  }
  return it;
}

void Solver::PushDepthMeasurementsGT(const CurrentFrame &CF, const KeyFrame *KF,
                                     const bool keyframeOnly) {
  const AlignedVector<Camera> &CsGT = m_internal->m_CsGT;
  if (CsGT.Empty()) {
    return;
  }
  const LocalBundleAdjustor::InputLocalFrame &ILF = m_internal->PushCurrentFrame(CF);
  if (KF) {
    //UT::Print("[%d]\n", KF->iFrm);
    const GlobalMap::InputKeyFrame &IKF = m_internal->PushKeyFrame(*KF);

    //float e;
    int it;
    LA::Vector3f Rx;
    const std::vector<int> &iFrmsKF = m_internal->m_iFrmsKF, &iKF2d = m_internal->m_iKF2d;
    std::vector<::Depth::InverseGaussian> &dsGT = m_internal->m_dsGT;
    AlignedVector<LA::AlignedVector3f> &tsGT = m_internal->m_tsGT;
    std::vector<std::vector<::Depth::Measurement> > &zsGT = m_internal->m_zsGT;
    AlignedVector<float> &work = m_internal->m_work;
    AlignedVector<Rotation3D> &Rs = m_internal->m_RsGT;
    std::vector<int> &its = m_internal->m_idxsTmp;
#ifdef CFG_STEREO
    const LA::AlignedVector3f &br = m_internal->m_K.m_br;
    if (tsGT.Empty()) {
      tsGT.Push(br);
    }
#endif
    const int N = static_cast<int>(IKF.m_Xs.size());
    for (int i1 = 0, i2 = 0; i1 < N; i1 = i2) {
      const int iKF = IKF.m_Xs[i1].m_iKF;
      const Rigid3D &C = CsGT[iFrmsKF[iKF]].m_T;
      const int it0 = tsGT.Size();
      Rs.Resize(0);
      const int nKFs = static_cast<int>(iFrmsKF.size());
      its.assign(nKFs, -1);
      for (i2 = i1 + 1; i2 < N && IKF.m_Xs[i2].m_iKF == iKF; ++i2) {}
      const GlobalMap::Point *Xs = IKF.m_Xs.data() + i1;
      const int Nx = i2 - i1, id = iKF2d[iKF + 1] - Nx;
      dsGT.insert(dsGT.begin() + id, Nx, ::Depth::InverseGaussian());
      ::Depth::InverseGaussian *ds = dsGT.data() + id;
      zsGT.insert(zsGT.begin() + id, Nx, std::vector<::Depth::Measurement>());
      std::vector<::Depth::Measurement> *zs = zsGT.data() + id;
      for (int i = 0; i < Nx; ++i) {
        const GlobalMap::Point &X = Xs[i];
        ::Depth::InverseGaussian &d = ds[i];
        std::vector<::Depth::Measurement> &_zs = zs[i];
        d.Initialize();
#ifdef CFG_STEREO
        const FTR::Source &x = X.m_x;
        if (x.m_xr.Valid()) {
          Rx.Set(x.m_x.x(), x.m_x.y(), 1.0f);
          _zs.push_back(::Depth::Measurement(tsGT[0], Rx, x.m_xr, x.m_Wr));
          ::Depth::Triangulate(1.0f, static_cast<int>(_zs.size()), _zs.data(), &d, &work);
        }
#endif
        const int Nz = static_cast<int>(X.m_zs.size());
        for (int j = 0; j < Nz; ++j) {
          const FTR::Measurement &z = X.m_zs[j];
          if ((it = its[z.m_iKF]) == -1) {
            const Rigid3D T = CsGT[iFrmsKF[z.m_iKF]].m_T / C;
            Rs.Push(T);
            const LA::AlignedVector3f t = T.GetTranslation();
            it = its[z.m_iKF] = PushTranslation(t, &tsGT, &zsGT);
#ifdef CFG_STEREO
            const LA::AlignedVector3f tr = t + br;
            PushTranslation(tr, &tsGT, &zsGT);
#endif
          }
          const int iR = (it - it0)
#ifdef CFG_STEREO
                       >> 1;
#endif
#ifdef CFG_DEBUG
//#if 0
          const Rigid3D T = CsGT[iFrmsKF[z.m_iKF]].m_T / C;
          const LA::AlignedVector3f t = T.GetTranslation();
          const LA::AlignedVector3f tr = t + br;
          Rs[iR].AssertEqual(T);
          tsGT[it].AssertEqual(t);
          tsGT[it + 1].AssertEqual(tr);
#endif
          Rs[iR].Apply(x.m_x, Rx);
#ifdef CFG_STEREO
          if (z.m_z.Valid()) {
            _zs.push_back(::Depth::Measurement(tsGT[it], Rx, z.m_z, z.m_W));
          }
          if (z.m_zr.Valid()) {
            _zs.push_back(::Depth::Measurement(tsGT[it + 1], Rx, z.m_zr, z.m_Wr));
          }
#else
          _zs.push_back(::Depth::Measurement(tsGT[it], Rx, z.m_z, z.m_W));
#endif
          ::Depth::Triangulate(1.0f, static_cast<int>(_zs.size()), _zs.data(),
                               &d, &work, true, true/*, &e*/);
          //const int iX = DsGT.size() - 1;
          //UT::Print("iX = %d d = %f e = %f [%d]\n", iX, d.u(),
          //          e * sqrtf(m_internal->m_K.m_K.fxy()), iFrmsKF[z.m_iKF]);
        }
      }
    }
    m_internal->PushDepthMeasurementsGT(IKF);
  }
  if (!keyframeOnly) {
    m_internal->PushDepthMeasurementsGT(ILF);
  }
}

void Internal::PushDepthMeasurementsGT(const FRM::Frame &F) {
  //float e;
  LA::Vector3f Rx;
  const int NZ = static_cast<int>(F.m_Zs.size());
  for (int iZ = 0; iZ < NZ; ++iZ) {
    const FRM::Measurement &Z = F.m_Zs[iZ];
    const int iFrmKF = m_iFrmsKF[Z.m_iKF];
#ifdef CFG_DEBUG
    UT_ASSERT(iFrmKF < F.m_T.m_iFrm);
#endif
    const Rigid3D T = m_CsGT[F.m_T.m_iFrm].m_T / m_CsGT[iFrmKF].m_T;
    const LA::AlignedVector3f t = T.GetTranslation();
    const int it = PushTranslation(t, &m_tsGT, &m_zsGT);
#ifdef CFG_STEREO
    const LA::AlignedVector3f tr = t + m_K.m_br;
    PushTranslation(tr, &m_tsGT, &m_zsGT);
#endif
    const int id = m_iKF2d[Z.m_iKF];
    const ::Point2D *xs = m_xs.data() + id;
    ::Depth::InverseGaussian *ds = m_dsGT.data() + id;
    std::vector<::Depth::Measurement> *zs = m_zsGT.data() + id;
    for (int iz = Z.m_iz1; iz < Z.m_iz2; ++iz) {
      const FTR::Measurement &z = F.m_zs[iz];
      const int ix = z.m_ix;
      std::vector<::Depth::Measurement> &_zs = zs[ix];
      T.ApplyRotation(xs[ix], Rx);
#ifdef CFG_STEREO
      if (z.m_z.Valid()) {
        _zs.push_back(::Depth::Measurement(m_tsGT[it], Rx, z.m_z, z.m_W));
      }
      if (z.m_zr.Valid()) {
        _zs.push_back(::Depth::Measurement(m_tsGT[it + 1], Rx, z.m_zr, z.m_Wr));
      }
#else
      _zs.push_back(::Depth::Measurement(m_tsGT[it], Rx, z.m_z, z.m_W));
#endif
      ::Depth::Triangulate(1.0f, static_cast<int>(_zs.size()), _zs.data(),
                           &ds[ix], &m_work, true, true/*, &e*/);
      //UT::Print("iX = %d d = %f e = %f [%d]\n", iX, m_DsGT[iX].u(),
      //          e * sqrtf(m_K.m_K.fxy()), F.m_T.m_iFrm);
    }
  }
}

void Solver::TriangulateDepthsGT(std::vector<Depth> *dsGT) {
  float e;
  LA::AlignedVectorXf es;
  const int N = static_cast<int>(m_internal->m_idx2iX.size());
  dsGT->resize(N);
  es.Reserve(N);
  int SNd = 0;
  for (int idx = 0; idx < N; ++idx) {
    Depth &d = dsGT->at(idx);
    const int iX = m_internal->m_idx2iX[idx];
    if (iX == -1) {
      d.d = d.s2 = 0.0f;
    } else {
      const int id = m_internal->m_iX2d[iX];
      ::Depth::InverseGaussian &_d = m_internal->m_dsGT[id];
      const std::vector<::Depth::Measurement> &zs = m_internal->m_zsGT[id];
      const int Nz = static_cast<int>(zs.size());
      if (::Depth::Triangulate(1.0f, Nz, zs.data(), &_d, &m_internal->m_work, true, true, &e)) {
        es.Push(e);
        ConvertDepth(_d, &d);
      } else {
        d.d = d.s2 = 0.0f;
      }
      if (Nz > 0) {
        ++SNd;
      }
    }
  }
  es *= sqrtf(m_internal->m_K.m_K.fxy());
  const int Nv = es.Size();
  UT::Print("Success %d / %d = %f%%\n", Nv, SNd, UT::Percentage(Nv, SNd));
  UT::Print("Error = %f +- %f <= %f\n", es.Mean(), sqrtf(es.Variance()), es.Maximal());
  //#ifdef CFG_DEBUG
#if 0
  es.Save("D:/tmp/es.txt");
  //std::sort(es.Data(), es.End());
#endif
}
#endif

void Solver::SetCallbackLBA(const IbaCallback& iba_callback) {
  m_internal->m_LBA.SetCallback(iba_callback);
}

void Solver::SetCallbackGBA(const IbaCallback& iba_callback) {
  m_internal->m_GBA.SetCallback(iba_callback);
}

void Solver::GetSlidingWindow(SlidingWindow *SW) {
  const ubyte Uc = m_internal->m_LM.IBA_Synchronize(m_internal->m_nFrms,
                                                    m_internal->m_CsLF, m_internal->m_CsKF,
                                                    m_internal->m_ucsKF, m_internal->m_ds,
                                                    m_internal->m_uds);
  SW->iFrms.resize(0);
  SW->CsLF.resize(0);
  if (Uc & LM_FLAG_FRAME_UPDATE_CAMERA_LF) {
    CameraIMUState C;
    for (std::list<LocalMap::CameraLF>::const_iterator i = m_internal->m_CsLF.begin();
         i != m_internal->m_CsLF.end(); ++i) {
      if (!i->m_uc) {
        continue;
      }
      SW->iFrms.push_back(i->m_iFrm);
      ConvertCamera(i->m_C, &C);
      SW->CsLF.push_back(C);
    }
  }
  SW->iFrmsKF.resize(0);
  SW->CsKF.resize(0);
  if (Uc & LM_FLAG_FRAME_UPDATE_CAMERA_KF) {
    CameraPose C;
    const int nKFs = m_internal->m_CsKF.Size();
    for (int iKF = 0; iKF < nKFs; ++iKF) {
      if (!(m_internal->m_ucsKF[iKF] & LM_FLAG_FRAME_UPDATE_CAMERA_KF)) {
        continue;
      }
      const Rigid3D &_C = m_internal->m_CsKF[iKF];
      _C.LA::AlignedMatrix3x3f::Get(C.R);
      _C.GetPosition().Get(C.p);
      SW->iFrmsKF.push_back(m_internal->m_iFrmsKF[iKF]);
      SW->CsKF.push_back(C);
    }
  }
  SW->Xs.resize(0);
  if (Uc & LM_FLAG_FRAME_UPDATE_DEPTH) {
    ::Point3D X1;
    Point3D X2;
    const int nKFs = static_cast<int>(m_internal->m_ucsKF.size());
    for (int iKF = 0; iKF < nKFs; ++iKF) {
      if (!(m_internal->m_ucsKF[iKF] & LM_FLAG_FRAME_UPDATE_DEPTH)) {
        continue;
      }
      const int id1 = m_internal->m_iKF2d[iKF], id2 = m_internal->m_iKF2d[iKF + 1];
      const ::Point2D *xs = m_internal->m_xs.data() + id1;
      const ::Depth::InverseGaussian *ds = m_internal->m_ds.data() + id1;
      const ubyte *uds = m_internal->m_uds.data() + id1;
      const int *idxs = m_internal->m_id2idx.data() + id1;
      const int Nx = id2 - id1;
      const Rigid3D &C = m_internal->m_CsKF[iKF];
      for (int ix = 0; ix < Nx; ++ix) {
        if (!(uds[ix] & LM_FLAG_TRACK_UPDATE_DEPTH)) {
          continue;
        }
        C.ApplyInversely(xs[ix], 1.0f / ds[ix].u(), X1);
        X2.idx = idxs[ix];
        X1.Get(X2.X);
        SW->Xs.push_back(X2);
      }
    }
  }
}

const std::vector<int>& Solver::GetKeyFrameIndexes() {
  return m_internal->m_iFrmsKF;
}

int Solver::SearchKeyFrame(const int iFrm) {
  const std::vector<int> &iFrmsKF = m_internal->m_iFrmsKF;
  const std::vector<int>::const_iterator it = std::lower_bound(iFrmsKF.begin(),
                                                               iFrmsKF.end(), iFrm);
  if (it == iFrmsKF.end() || *it != iFrm) {
    return -1;
  }
  return static_cast<int>(it - iFrmsKF.begin());
}

bool Solver::DeleteKeyFrame(const int iFrm) {
  const int iKF = SearchKeyFrame(iFrm);
  if (iKF == -1) {
    return false;
  }
#ifdef CFG_DEBUG
//#if 0
  UT::Print("\nDeleting KF%d [%d]...", iKF, iFrm);
#endif
  std::vector<int> &iFrmsKF = m_internal->m_iFrmsKF;
  const int nKFs = static_cast<int>(iFrmsKF.size());
  iFrmsKF.erase(iFrmsKF.begin() + iKF);

  std::vector<int> &iKF2d = m_internal->m_iKF2d, &id2X = m_internal->m_id2X;
  std::vector<int> &iX2d = m_internal->m_iX2d, &id2idx = m_internal->m_id2idx;
  const int id1 = iKF2d[iKF], id2 = iKF2d[iKF + 1], Nd = id2 - id1;
  for (int jKF = iKF + 1; jKF <= nKFs; ++jKF) {
    iKF2d[jKF] -= Nd;
  }
  iKF2d.erase(iKF2d.begin() + iKF);
  for (int id = id1; id < id2; ++id) {
    iX2d[id2X[id]] = -1;
  }
  const int ND = static_cast<int>(id2X.size());
  for (int id = id2; id < ND; ++id) {
    iX2d[id2X[id]] -= Nd;
  }
  id2X.erase(id2X.begin() + id1, id2X.begin() + id2);
  id2idx.erase(id2idx.begin() + id1, id2idx.begin() + id2);
  m_internal->m_CsKF.Erase(iKF);
  std::vector<::Point2D> &xs = m_internal->m_xs;
  xs.erase(xs.begin() + id1, xs.begin() + id2);
  std::vector<::Depth::InverseGaussian> &ds = m_internal->m_ds;
  ds.erase(ds.begin() + id1, ds.begin() + id2);
#ifdef CFG_GROUND_TRUTH
  std::vector<::Depth::InverseGaussian> &dsGT = m_internal->m_dsGT;
  if (!dsGT.empty()) {
    dsGT.erase(dsGT.begin() + id1, dsGT.begin() + id2);
  }
#endif
  //m_internal->m_LBA.Synchronize();
  //m_internal->m_GBA.Synchronize();
  m_internal->m_LBA.DeleteKeyFrame(iKF);
  m_internal->m_LM.IBA_DeleteKeyFrame(iKF);
#ifdef CFG_DEBUG
//#if 0
  UT::Print("done!\n");
#endif
  return true;
}

void Solver::UpdateCameras(const std::vector<int> &iFrms, const std::vector<CameraPose> &Cs) {
  Rigid3D C;
  std::vector<GlobalBundleAdjustor::InputCamera> &ICs = m_internal->m_ICs;
  ICs.resize(0);
  const std::vector<int> &iFrmsKF = m_internal->m_iFrmsKF;
  std::vector<int>::const_iterator i = iFrmsKF.begin();
  const int N = static_cast<int>(iFrms.size());
#ifdef CFG_DEBUG
  UT_ASSERT(static_cast<int>(Cs.size()) == N);
#endif
  for (int j = 0; j < N; ++j) {
    const int iFrm = iFrms[j];
#ifdef CFG_DEBUG
    if (j > 0) {
      UT_ASSERT(iFrm > iFrms[j - 1]);
    }
#endif
    i = std::lower_bound(i, iFrmsKF.end(), iFrm);
    if (i == iFrmsKF.end()) {
      break;
    } else if (*i != iFrm) {
      continue;
    }
    const int iKF = static_cast<int>(i - iFrmsKF.begin());
    ConvertCamera(Cs[j], &C);
    ICs.push_back(GlobalBundleAdjustor::InputCamera(iKF, C));
  }
#ifdef CFG_DEBUG
  const int _N = static_cast<int>(ICs.size());
  UT::Print("\nUpdating %d / %d cameras {", _N, iFrmsKF.size());
  for (int j = 0; j < _N; ++j) {
    UT::Print(" %d", ICs[j].m_iKF);
  }
  UT::Print(" }\n");
#endif
  m_internal->m_GBA.UpdateCameras(m_internal->m_nFrms, ICs);
  m_internal->m_GBA.WakeUp();
}

void Solver::PropagateState(const std::vector<IMUMeasurement> &us, const float t1, const float t2,
                            const CameraIMUState &X1, CameraIMUState *X2) {
  AlignedVector<IMU::Measurement> _us;
  const int N = static_cast<int>(us.size());
  _us.Resize(N);
  const Rotation3D Ru = m_internal->m_K.m_Ru;
  const ::Point3D pu = m_internal->m_K.m_pu;
  const LA::AlignedVector3f ba = m_internal->m_K.m_ba;
  const LA::AlignedVector3f bw = m_internal->m_K.m_bw;
  for (int i = 0; i < N; ++i) {
    const IMUMeasurement &u = us[i];
    IMU::Measurement &_u = _us[i];
    _u.m_a.Set(u.a);
    _u.m_w.Set(u.w);
    if (Ru.Valid()) {
      _u.m_a = Ru.GetApplied(_u.m_a);
      _u.m_w = Ru.GetApplied(_u.m_w);
    }
    _u.m_a -= ba;
    _u.m_w -= bw;
    _u.t() = u.t;
  }
  Camera C1, C2;
  IMU::Delta D;
  AlignedVector<float> work;
  ConvertCamera(X1, &C1);
  IMU::PreIntegrate(_us, t1, t2, C1, &D, &work, false);
  IMU::Propagate(pu, D, C1, C2);
  ConvertCamera(C2, X2);
}

bool Solver::SaveFeatures(const std::string fileName, const std::vector<MapPointMeasurement> &zs,
                          const std::vector<MapPoint> *Xs) {
  FILE *fp = fopen(fileName.c_str(), "w");
  if (!fp) {
    return false;
  }
  fprintf(fp, "key_points: [ ");
  const int Nz = static_cast<int>(zs.size());
  for (int iz = 0; iz < Nz; ++iz) {
    const MapPointMeasurement &z = zs[iz];
    //const int id = m_internal->m_iX2d[z.iX];
    const int iX = m_internal->m_idx2iX[z.idx];
    const int id = m_internal->m_iX2d[iX];
    if (id != -1) {
      fprintf(fp, "%f, %f, 0, 0, 0, 0, %d,\n", z.x.x[0], z.x.x[1], m_internal->m_id2idx[id]);
    }
  }
  fprintf(fp, "]\n");
  if (Xs) {
    fprintf(fp, "map_points: [\n");
    const int Nx = static_cast<int>(Xs->size());
    fprintf(fp, "%d\n", Nx);
    for (int ix = 0; ix < Nx; ++ix) {
      const MapPoint &X = Xs->at(ix);
      const int Nz = static_cast<int>(X.zs.size());
      fprintf(fp, "%d %d\n", X.X.idx, Nz);
      for (int i = 0; i < Nz; ++i) {
        const MapPointMeasurement &z = X.zs[i];
        fprintf(fp, "%d %f %f\n", z.iFrm, z.x.x[0], z.x.x[1]);
      }
    }
    fprintf(fp, "]\n");
  }
  fclose(fp);
  return true;
}

bool Solver::LoadFeatures(const std::string fileName, const int iFrm,
                          std::vector<MapPointMeasurement> *zs,
                          std::vector<MapPoint> *Xs,
                          const std::vector<int> *iFrms
#ifdef CFG_STEREO
                        , const ubyte right
#endif
                        ) {
#ifdef CFG_STEREO
  if (!right)
#endif
  {
    zs->resize(0);
    if (Xs) {
      Xs->resize(0);
    }
  }
  FILE *fp = fopen(fileName.c_str(), "r");
  if (!fp) {
    return false;
  }
  char line[UT_STRING_WIDTH_MAX];
  MapPointMeasurement z;
#ifdef CFG_STEREO
  z.right = right;
  std::vector<int> &idx2iXTmp = m_internal->m_idxsTmp;
  if (!right) {
    idx2iXTmp.resize(0);
  }
#endif
  int i = 0/*, idx*/;
  char *num;
  float v;
  const bool cov = FTR_VARIANCE == 0.0f;
  if (!cov) {
    z.x.S[0][0] = z.x.S[1][1] = FTR_VARIANCE;
    z.x.S[0][1] = z.x.S[1][0] = 0.0f;
  }
  const std::vector<int> &idx2iX = m_internal->m_idx2iX;
  const int idxMax = static_cast<int>(idx2iX.size()) - 1;
  while (fgets(line, UT_STRING_WIDTH_MAX, fp)) {
    const int len = int(std::remove(line, std::remove(line, line + strlen(line), 10), ' ') - line);
    if (len == 0 || line[0] == '%' || line[len - 1] == ':') {
      continue;
    }
    line[len] = 0;
    num = strtok(line, " ,");
    while (num && (sscanf(num, "%f", &v) == 1 || sscanf(num, "key_points:[%f", &v) == 1)) {
      switch (i) {
      case 0:
        z.x.x[0] = v;
        break;
      case 1:
        z.x.x[1] = v;
        break;
      case 2:
        if (cov) {
          z.x.S[0][0] = v;
        }
        break;
      case 3:
        if (cov) {
          z.x.S[0][1] = z.x.S[0][1] = v;
        }
        break;
      case 4:
        if (cov) {
          z.x.S[1][1] = v;
        }
        break;
      case 5:
        break;
      case 6:
        //idx = static_cast<int>(v);
        //z.iX = idx > idxMax ? -1 : idx2iX[idx];
        z.idx = static_cast<int>(v);
        break;
      }
      ++i;
      if (i == 7) {
        i = 0;
        if (z.idx > idxMax || idx2iX[z.idx] == -1) {
        //if (z.iX == -1) {
          if (Xs) {
#ifdef CFG_STEREO
            if (right) {
              const int iX = z.idx < static_cast<int>(idx2iXTmp.size()) ? idx2iXTmp[z.idx] : -1;
              if (iX != -1) {
                MapPoint &X = Xs->at(iX);
#ifdef CFG_DEBUG
                UT_ASSERT(X.X.idx == z.idx);
#endif
                z.iFrm = iFrm;
                X.zs.insert(std::lower_bound(X.zs.begin(), X.zs.end(), z), z);
              }
            } else
#endif
            {
              const int iX = static_cast<int>(Xs->size());
              Xs->resize(iX + 1);
              MapPoint &X = Xs->back();
              X.X.idx = z.idx;
              X.X.X[0] = FLT_MAX;
#ifdef CFG_STEREO
              if (z.idx >= static_cast<int>(idx2iXTmp.size())) {
                idx2iXTmp.resize(z.idx + 1, -1);
              }
              idx2iXTmp[z.idx] = iX;
#endif
              z.iFrm = iFrm;
              X.zs.assign(1, z);
            }
          }
        } else {
          zs->push_back(z);
        }
      }
      num = strtok(NULL, " ,");
    }
    if (line[len - 1] == ']') {
      break;
    }
  }
  if (Xs) {
    int Nx, Nz, iX, idx;
    float tmp[2];
    while (fgets(line, UT_STRING_WIDTH_MAX, fp)) {
      const int len = int(std::remove(line, std::remove(line, line + strlen(line), 10), ' ') - line);
      if (len == 0 || line[0] == '%' || line[len - 1] == ':') {
        continue;
      }
      line[len] = 0;
      if (strcmp(line, "map_points:[") != 0) {
        continue;
      }
      fscanf(fp, "%d", &Nx);
#ifdef CFG_STEREO
      if (!right)
#endif
      {
        Xs->reserve(Xs->size() + Nx);
      }
      for (int ix = 0; ix < Nx; ++ix) {
        fscanf(fp, "%d", &idx);
#ifdef CFG_STEREO
        if (right) {
          iX = idx < static_cast<int>(idx2iXTmp.size()) ? idx2iXTmp[idx] : -1;
          if (iX == -1) {
            fscanf(fp, "%d", &Nz);
            for (int i = 0; i < Nz; ++i) {
              fscanf(fp, "%d %f %f", &z.iFrm, &tmp[0], &tmp[1]);
            }
            continue;
          }
        } else
#endif
        {
          iX = static_cast<int>(Xs->size());
          Xs->resize(iX + 1);
          Xs->back().zs.resize(0);
#ifdef CFG_STEREO
          if (idx >= static_cast<int>(idx2iXTmp.size())) {
            idx2iXTmp.resize(idx + 1, -1);
          }
          idx2iXTmp[idx] = iX;
#endif
        }
        MapPoint &X = Xs->at(iX);
        X.X.idx = idx;
        fscanf(fp, "%d", &Nz);
        X.X.X[0] = FLT_MAX;
        for (int i = 0; i < Nz; ++i) {
          fscanf(fp, "%d %f %f", &z.iFrm, &z.x.x[0], &z.x.x[1]);
          z.x.S[0][0] = z.x.S[1][1] = FTR_VARIANCE;
          z.x.S[0][1] = z.x.S[1][0] = 0.0f;
          if (iFrms) {
            z.iFrm = iFrms->at(z.iFrm);
            if (z.iFrm == -1) {
              continue;
            }
          }
          X.zs.insert(std::lower_bound(X.zs.begin(), X.zs.end(), z), z);
        }
      }
      break;
    }
  }
  fclose(fp);
  return true;
}

void Solver::SaveB(FILE *fp) {
  m_internal->m_LM.SaveB(fp);
  m_internal->m_GM.SaveB(fp);
  m_internal->m_LBA.SaveB(fp);
  m_internal->m_GBA.SaveB(fp);
  UT::SaveB(m_internal->m_nFrms, fp);
  FRM::VectorSaveB(m_internal->m_Ts, fp);
  UT::VectorSaveB(m_internal->m_iFrmsKF, fp);
  UT::VectorSaveB(m_internal->m_iKF2d, fp);
  UT::VectorSaveB(m_internal->m_id2X, fp);
  UT::VectorSaveB(m_internal->m_iX2d, fp);
  UT::VectorSaveB(m_internal->m_id2idx, fp);
  UT::VectorSaveB(m_internal->m_idx2iX, fp);
  m_internal->m_CsKF.SaveB(fp);
  UT::VectorSaveB(m_internal->m_xs, fp);
  UT::VectorSaveB(m_internal->m_ds, fp);
#ifdef CFG_GROUND_TRUTH
  UT::VectorSaveB(m_internal->m_dsGT, fp);
  m_internal->m_tsGT.SaveB(fp);
  UT::VectorsSaveB(m_internal->m_zsGT, fp);
  UT::SaveB(m_internal->m_tsGT.Data(), fp);
#endif
#ifdef CFG_DEBUG_MT
  MT::SaveB(fp);
#endif
}

void Solver::LoadB(FILE *fp) {
  m_internal->m_LM.LoadB(fp);
  m_internal->m_GM.LoadB(fp);
  m_internal->m_LBA.LoadB(fp);
  m_internal->m_GBA.LoadB(fp);
  UT::LoadB(m_internal->m_nFrms, fp);
  FRM::VectorLoadB(m_internal->m_Ts, fp);
  UT::VectorLoadB(m_internal->m_iFrmsKF, fp);
  UT::VectorLoadB(m_internal->m_iKF2d, fp);
  UT::VectorLoadB(m_internal->m_id2X, fp);
  UT::VectorLoadB(m_internal->m_iX2d, fp);
  UT::VectorLoadB(m_internal->m_id2idx, fp);
  UT::VectorLoadB(m_internal->m_idx2iX, fp);
  m_internal->m_CsKF.LoadB(fp);
  UT::VectorLoadB(m_internal->m_xs, fp);
  UT::VectorLoadB(m_internal->m_ds, fp);
#ifdef CFG_GROUND_TRUTH
  UT::VectorLoadB(m_internal->m_dsGT, fp);
  m_internal->m_tsGT.LoadB(fp);
  UT::VectorsLoadB(m_internal->m_zsGT, fp);
  const LA::AlignedVector3f *t0 = UT::LoadB<LA::AlignedVector3f *>(fp);
  OffsetPointer(t0, m_internal->m_tsGT, &m_internal->m_zsGT);
#endif
#ifdef CFG_DEBUG_MT
  MT::LoadB(fp);
#endif
}

void Solver::ComputeErrorLBA(Error *e) {
  m_internal->m_LBA.ComputeErrorFeature(&e->ex);
  m_internal->m_LBA.ComputeErrorIMU(&e->eur, &e->eup, &e->euv, &e->euba, &e->eubw);
  m_internal->m_LBA.ComputeErrorDrift(&e->edr, &e->edp);
}

void Solver::ComputeErrorGBA(Error *e) {
  m_internal->m_GBA.ComputeErrorFeature(&e->ex);
  m_internal->m_GBA.ComputeErrorIMU(&e->eur, &e->eup, &e->euv, &e->euba, &e->eubw);
  m_internal->m_GBA.ComputeErrorDrift(&e->edr, &e->edp);
}

void Solver::PrintRMSE() {
  const float e = m_internal->m_GBA.ComputeRMSE();
  const float Sdist = m_internal->m_LBA.GetTotalDistance();
  UT::PrintSeparator();
  UT::Print("RMSE: %f / %f = %f%%\n", e, Sdist, UT::Percentage(e, Sdist));
}

static inline std::string ConvertFileName(const std::string fileName, const std::string dir, const bool append = true) {
  if (fileName == "") {
    return "";
  }
  std::string _fileName;
  if (UT::FileNameRemoveDirectoryExtension(fileName) == "*") {
    _fileName = dir.substr(0, dir.length() - 1) + "." + UT::FileNameExtractExtension(fileName);
    _fileName = UT::FileNameReplaceDirectory(_fileName, UT::FileNameExtractDirectory(_fileName),
                                             UT::FileNameExtractDirectory(fileName));
  } else {
    _fileName = fileName;
  }
  _fileName = UT::FileNameReplaceDirectory(_fileName, ".", dir);
  if (append) {
    _fileName = UT::FileNameAppendSuffix(_fileName);
  }
  const std::string _dir = UT::FileNameExtractDirectory(_fileName);
  if (!UT::FileExists(_dir) && _dir != "") {
#ifdef WIN32
    CreateDirectory(_dir.c_str(), 0);
#else
    boost::filesystem::create_directories(_dir);
#endif
  }
  return _fileName;
}

bool Solver::SaveCamerasLBA(const std::string fileName, const bool append, const bool poseOnly) {
  const std::string _fileName = ConvertFileName(fileName, m_internal->m_dir, append);
  return m_internal->m_LBA.SaveCameras(_fileName, poseOnly);
}

bool Solver::SaveCamerasGBA(const std::string fileName, const bool append, const bool poseOnly) {
  const std::string _fileName = ConvertFileName(fileName, m_internal->m_dir, append);
  return m_internal->m_GBA.SaveCameras(_fileName, poseOnly);
}

bool Solver::SaveCostsLBA(const std::string fileName, const bool append, const int type) {
  const std::string _fileName = ConvertFileName(fileName, m_internal->m_dir, append);
  return m_internal->m_LBA.SaveCosts(_fileName, type);
}

bool Solver::SaveCostsGBA(const std::string fileName, const bool append, const int type) {
  const std::string _fileName = ConvertFileName(fileName, m_internal->m_dir, append);
  return m_internal->m_GBA.SaveCosts(_fileName, type);
}

bool Solver::SaveResidualsLBA(const std::string fileName, const bool append, const int type) {
  const std::string _fileName = ConvertFileName(fileName, m_internal->m_dir, append);
  return m_internal->m_LBA.SaveResiduals(_fileName, type);
}

bool Solver::SaveResidualsGBA(const std::string fileName, const bool append, const int type) {
  const std::string _fileName = ConvertFileName(fileName, m_internal->m_dir, append);
  return m_internal->m_GBA.SaveResiduals(_fileName, type);
}

bool Solver::SavePriors(const std::string fileName, const bool append, const int type) {
  const std::string _fileName = ConvertFileName(fileName, m_internal->m_dir, append);
  return m_internal->m_LBA.SavePriors(_fileName, type);
}

bool Solver::SavePoints(const std::string fileName, const bool append) {
  const std::string _fileName = ConvertFileName(fileName, m_internal->m_dir, append);
  FILE *fp = fopen(_fileName.c_str(), "w");
  if (!fp) {
    return false;
  }
  ::Point3D X;
  const std::vector<::Depth::InverseGaussian> &ds = m_internal->m_GBA.m_ds;
  const int nKFs = static_cast<int>(GetKeyFrameIndexes().size());
  for (int iKF = 0; iKF < nKFs; ++iKF) {
    const Rigid3D &C = m_internal->m_GBA.m_Cs[iKF];
    const int id1 = m_internal->m_iKF2d[iKF], id2 = m_internal->m_iKF2d[iKF + 1];
    const ::Point2D *xs = m_internal->m_xs.data() + id1;
    const ::Depth::InverseGaussian *ds = m_internal->m_GBA.m_ds.data() + id1;
    const int *idxs = m_internal->m_id2idx.data() + id1;
    const int Nx = id2 - id1;
    for (int ix = 0; ix < Nx; ++ix) {
      C.ApplyInversely(xs[ix], 1.0f / ds[ix].u(), X);
      fprintf(fp, "%d %f %f %f\n", idxs[ix], X.x(), X.y(), X.z());
    }
  }
  fclose(fp);
  UT::PrintSaved(fileName);
  return true;
}

void Solver::GetTimeLBA(Time *t) {
  t->t = m_internal->m_LBA.GetTotalTime(&t->n);
  //t->nd = m_internal->m_LBA.GetTotalPoints();
}

void Solver::GetTimeGBA(Time *t) {
  t->t = m_internal->m_GBA.GetTotalTime(&t->n);
  //t->nd = m_internal->m_GBA.GetTotalPoints();
}

bool Solver::SaveTimesLBA(const std::string fileName, const bool append) {
  const std::string _fileName = ConvertFileName(fileName, m_internal->m_dir, append);
  return m_internal->m_LBA.SaveTimes(_fileName);
}

bool Solver::SaveTimesGBA(const std::string fileName, const bool append) {
  const std::string _fileName = ConvertFileName(fileName, m_internal->m_dir, append);
  return m_internal->m_GBA.SaveTimes(_fileName);
}

static inline void Rectify(const ::Intrinsic &K, const Point2D &x,
                           ::Point2D *xn, LA::SymmetricMatrix2x2f *Wn,
                           ::Intrinsic::UndistortionMap *UM = NULL,
                           const float eps = 0.0f
#ifdef CFG_STEREO
                         , Rotation3D *Rr = NULL
#endif
                         ) {
  ::Point2D xd;
  Point2DCovariance Sd, Sn;
  LA::AlignedMatrix2x2f J, JS;
  LA::AlignedMatrix2x2f Wd;
  K.k().ImageToNormalized(x.x, xd);
  K.Undistort(xd, xn, &J, UM);
//#ifdef CFG_DEBUG
#if 0
  UT::Print("fx = %f %f\n", K.k().m_fx, K.fx());
  UT::Print("cx = %f %f\n", K.k().m_cx, K.cx());
  UT::Print("x = %f %f\n", x.x[0], x.x[1]);
  UT::Print("xn = "); xn->Print();
  UT::Print("J = ");  J.Print();
#endif
  K.k().ImageToNormalized(x.S, Sd);
  Sd.GetInverse(Wd);
  LA::AlignedMatrix2x2f::ABT(J, Wd, JS);
  LA::SymmetricMatrix2x2f::ABT(JS, J, *Wn);
  Wn->GetInverse(Sn);
#ifdef CFG_STEREO
  if (Rr) {
    *xn = Rr->GetApplied(*xn, J);
    LA::SymmetricMatrix2x2f::AB(J, Sn, JS);
    LA::SymmetricMatrix2x2f::ABT(JS, J, Sn);
  }
#endif
  Sn.IncreaseDiagonal(eps);
  Sn.GetInverse(*Wn);
  //////////////////////////////////////////////////////////////////////////
  //Wn->Set(Wd.m00(), Wd.m01(), Wd.m11());
  //////////////////////////////////////////////////////////////////////////
}

const LocalBundleAdjustor::InputLocalFrame& Internal::PushCurrentFrame(const CurrentFrame &CF) {
  m_ILF.m_T.m_iFrm = CF.iFrm;
  m_ILF.m_T.m_t = CF.t;
  m_ILF.m_T.m_fileName = CF.fileName;
#ifdef CFG_STEREO
  m_ILF.m_T.m_fileNameRight = CF.fileNameRight;
#endif
#ifdef CFG_DEBUG
  UT_ASSERT(m_Ts.empty() || m_Ts.back() < m_ILF.m_T);
#endif
  m_Ts.push_back(m_ILF.m_T);
  ConvertCamera(CF.C, &m_ILF.m_C);
  ConvertDepth(CF.d, &m_ILF.m_d);
  ConvertFeatureMeasurements(CF.zs, &m_ILF);
  const int Nu = static_cast<int>(CF.us.size());
  m_ILF.m_us.Resize(Nu);
  for (int i = 0; i < Nu; ++i) {
    const IMUMeasurement &u = CF.us[i];
    IMU::Measurement &_u = m_ILF.m_us[i];
    _u.m_a.Set(u.a);
    _u.m_w.Set(u.w);
    if (m_K.m_Ru.Valid()) {
      _u.m_a = m_K.m_Ru.GetApplied(_u.m_a);
      _u.m_w = m_K.m_Ru.GetApplied(_u.m_w);
    }
    _u.m_a -= m_K.m_ba;
    _u.m_w -= m_K.m_bw;
    _u.t() = u.t;
  }
  ++m_nFrms;
  return m_ILF;
}

const GlobalMap::InputKeyFrame& Internal::PushKeyFrame(const KeyFrame &KF, const Camera *C) {
  const std::vector<FRM::Tag>::iterator T = std::lower_bound(m_Ts.begin(), m_Ts.end(), KF.iFrm);
#ifdef CFG_DEBUG
  UT_ASSERT(T != m_Ts.end() && T->m_iFrm == KF.iFrm);
#endif
  m_IKF.m_T = *T;
  m_Ts.erase(m_Ts.begin(), T);
  ConvertCamera(KF.C, &m_IKF.m_C.m_T);
  m_IKF.m_C.m_p.Set(KF.C.p);
  if (C) {
    m_IKF.m_C.m_v = C->m_v;
    m_IKF.m_C.m_ba = C->m_ba;
    m_IKF.m_C.m_bw = C->m_bw;
  } else {
    m_IKF.m_C.m_v.Invalidate();
    m_IKF.m_C.m_ba.Invalidate();
    m_IKF.m_C.m_bw.Invalidate();
  }
  ConvertDepth(KF.d, &m_IKF.m_d);
  ConvertFeatureMeasurements(KF.zs, &m_IKF);
  m_CsKF.Push(m_IKF.m_C.m_T);

  const int Nx1 = static_cast<int>(KF.Xs.size());
#ifdef CFG_DEBUG
  for (int ix = 0; ix < Nx1; ++ix) {
    const MapPoint &X = KF.Xs[ix];
    const int Nz = static_cast<int>(X.zs.size());
    for (int i = 1; i < Nz; ++i) {
      const MapPointMeasurement &z1 = X.zs[i - 1], &z2 = X.zs[i];
#ifdef CFG_STEREO
      UT_ASSERT(z1.iFrm < z2.iFrm || z1.iFrm == z2.iFrm && !z1.right && z2.right);
#else
      UT_ASSERT(z1.iFrm < z2.iFrm);
#endif
    }
  }
#endif
  m_iFrmsKF.push_back(KF.iFrm);
  int idxMax = static_cast<int>(m_idx2iX.size()) - 1;
  std::vector<MapPointIndex> &ixsSort = m_idxsSortTmp;
  ixsSort.resize(Nx1);
  for (int ix = 0; ix < Nx1; ++ix) {
    const MapPoint &X = KF.Xs[ix];
    idxMax = std::max(idxMax, X.X.idx);
    ixsSort[ix].Set(X.zs.front().iFrm, X.X.idx, ix);
  }
  std::sort(ixsSort.begin(), ixsSort.end());
  int iKF0, iKF1, iKF2;
  if (ixsSort.empty()) {
    iKF0 = iKF2 = -1;
  } else {
    iKF0 = static_cast<int>(std::upper_bound(m_iFrmsKF.begin(), m_iFrmsKF.end(),
                   ixsSort.front().m_iFrm) - m_iFrmsKF.begin()) - 1;
    iKF2 = static_cast<int>(std::upper_bound(m_iFrmsKF.begin() + iKF0, m_iFrmsKF.end(),
                    ixsSort.back().m_iFrm) - m_iFrmsKF.begin());
  }
  int i1, i2;
  FTR::Measurement z;
  ::Point3D X;
  m_IKF.m_Xs.resize(Nx1);
  std::vector<int> &ixs = m_idxsTmp;
  ixs.resize(Nx1);
  const float eps = FTR_VARIANCE_EPSILON * m_K.m_K.fxyI();
  const int nKFs = static_cast<int>(m_iFrmsKF.size());
  for (i1 = i2 = 0, iKF1 = iKF0; i1 < Nx1; ++i1) {
    const MapPointIndex &ix = ixsSort[i1];
//#ifdef CFG_DEBUG
#if 0
    if (ix.m_idx == 2001) {
      UT::DebugStart();
      UT::DebugStop();
    }
#endif
    if (ix.m_iFrm > m_iFrmsKF[iKF1]) {
      iKF1 = static_cast<int>(std::lower_bound(m_iFrmsKF.begin() + iKF1,
                                               m_iFrmsKF.begin() + iKF2, ix.m_iFrm) -
                                               m_iFrmsKF.begin());
      if (iKF1 == nKFs || m_iFrmsKF[iKF1] != ix.m_iFrm) {
        --iKF1;
        continue;
      }
    }
    z.m_iKF = iKF1;
    const MapPoint &X1 = KF.Xs[ix.m_ix];
    GlobalMap::Point &X2 = m_IKF.m_Xs[i2];
    X2.m_zs.resize(0);
    const int Nz1 = static_cast<int>(X1.zs.size());
    for (int i = 0; i < Nz1; ++i) {
      const MapPointMeasurement &_z = X1.zs[i];
#ifdef CFG_STEREO
      if (_z.right) {
        if (i == 0) {
          continue;
        }
        Rectify(m_K.m_Kr, _z.x, &z.m_zr, &z.m_Wr, &m_UMr, eps, &m_K.m_Rr);
        if (X1.zs[i - 1].iFrm != _z.iFrm) {
          z.m_z.Invalidate();
        }
        z.m_iKF = static_cast<int>(std::lower_bound(m_iFrmsKF.begin() + z.m_iKF,
                                                    m_iFrmsKF.begin() + iKF2, _z.iFrm) -
                                                    m_iFrmsKF.begin());
        if (z.m_iKF != nKFs && m_iFrmsKF[z.m_iKF] == _z.iFrm) {
          X2.m_zs.push_back(z);
        } else {
          --z.m_iKF;
        }
      } else
#endif
      {
        Rectify(m_K.m_K, _z.x, &z.m_z, &z.m_W, &m_UM, eps);
#ifdef CFG_STEREO
        const int j = i + 1;
        if (j == Nz1 || X1.zs[j].iFrm != _z.iFrm)
#endif
        {
#ifdef CFG_STEREO
          z.m_zr.Invalidate();
#endif
          z.m_iKF = static_cast<int>(std::lower_bound(m_iFrmsKF.begin() + z.m_iKF,
                                                      m_iFrmsKF.begin() + iKF2, _z.iFrm) -
                                                      m_iFrmsKF.begin());
          if (z.m_iKF != nKFs && m_iFrmsKF[z.m_iKF] == _z.iFrm) {
            X2.m_zs.push_back(z);
          } else {
            --z.m_iKF;
          }
        }
      }
    }
    const int Nz2 = static_cast<int>(X2.m_zs.size());
    if (Nz2 == 0) {
      continue;
    }
    const FTR::Measurement &_z = X2.m_zs.front();
    X2.m_iKF = _z.m_iKF;
    X2.m_x.m_x = _z.m_z;
    X2.m_W = _z.m_W;
#ifdef CFG_STEREO
    X2.m_x.m_xr = _z.m_zr;
    X2.m_x.m_Wr = _z.m_Wr;
#endif
    X2.m_zs.erase(X2.m_zs.begin());
    X.Set(X1.X.X);
    if (X.Valid()) {
      X2.m_d.Initialize(1.0f / m_CsKF[X2.m_iKF].GetAppliedZ(X));
    } else {
      X2.m_d.Invalidate();
    }
    ixs[i2] = ix.m_ix;
    ++i2;
  }
  const int Nx2 = i2;
  m_IKF.m_Xs.resize(Nx2);
  ixs.resize(Nx2);

  m_iKF2d.push_back(m_iKF2d.back());
  const int NX1 = static_cast<int>(m_iX2d.size()), NX2 = NX1 + Nx1;
  m_iX2d.resize(NX2, -1);
  m_idx2iX.resize(idxMax + 1, -1);
  for (int i1 = 0, i2 = 0; i1 < Nx2; i1 = i2) {
    const int iKF = m_IKF.m_Xs[i1].m_iKF;
    for (i2 = i1 + 1; i2 < Nx2 && m_IKF.m_Xs[i2].m_iKF == iKF; ++i2) {}
    const int id = m_iKF2d[iKF + 1], Nx = i2 - i1;
    for (int jKF = iKF; jKF < nKFs; ++jKF) {
      m_iKF2d[jKF + 1] += Nx;
    }
    m_id2X.insert(m_id2X.begin() + id, Nx, -1);
    const int Nd = static_cast<int>(m_id2X.size());
    for (int jd = id + Nx; jd < Nd; ++jd) {
      const int iX = m_id2X[jd];
      m_iX2d[iX] += Nx;
    }
    m_id2idx.insert(m_id2idx.begin() + id, Nx, -1);
    m_xs.insert(m_xs.begin() + id, Nx, ::Point2D());
    m_ds.insert(m_ds.begin() + id, Nx, ::Depth::InverseGaussian());
    const int *_ixs = ixs.data() + i1;
    const GlobalMap::Point *Xs = m_IKF.m_Xs.data() + i1;
    ::Point2D *xs = m_xs.data() + id;
    ::Depth::InverseGaussian *ds = m_ds.data() + id;
#ifdef CFG_GROUND_TRUTH
    ::Depth::InverseGaussian *dsGT = NULL;
    if (static_cast<int>(m_DsGT.size()) > idxMax) {
      m_dsGT.insert(m_dsGT.begin() + id, Nx, ::Depth::InverseGaussian());
      dsGT = m_dsGT.data() + id;
    }
#endif
    for (int i = 0, jd = id; i < Nx; ++i, ++jd) {
      const int ix = _ixs[i], iX = NX1 + ix, idx = KF.Xs[ix].X.idx;
      m_id2X[jd] = iX;
      m_iX2d[iX] = jd;
      m_id2idx[jd] = idx;
      const int _iX = m_idx2iX[idx];
      if (_iX != -1) {
        const int _id = m_iX2d[_iX];
        if (_id != -1) {
#ifdef CFG_DEBUG
          UT_ASSERT(m_id2X[_id] == _iX);
#endif
          //m_id2X[_id] = -1;
          //m_iX2d[_iX] = -1;
          m_id2idx[_id] = -1;
        }
      }
      m_idx2iX[idx] = iX;
      const GlobalMap::Point &X = Xs[i];
      xs[i] = X.m_x.m_x;
      ds[i] = X.m_d;
#ifdef CFG_GROUND_TRUTH
      if (dsGT) {
        dsGT[i] = m_DsGT[idx];
      }
#endif
    }
  }
  return m_IKF;
}

void Internal::ConvertFeatureMeasurements(const std::vector<MapPointMeasurement> &zs,
                                          FRM::Frame *F) {
  F->ClearMeasurements();

  int i, j;
  const float eps = FTR_VARIANCE_EPSILON * m_K.m_K.fxyI();
  const int Nz1 = static_cast<int>(zs.size());
  m_zsSortTmp.resize(Nz1);
  for (i = j = 0; i < Nz1; ++i) {
    const MapPointMeasurement &z1 = zs[i];
    FeatureMeasurement &z2 = m_zsSortTmp[j];
    const int iX = m_idx2iX[z1.idx];
    //if (iX == -1) {
    //  continue;
    //}
#ifdef CFG_DEBUG
    UT_ASSERT(iX != -1);
#endif
    if ((z2.m_id = m_iX2d[iX]) == -1) {
      continue;
    }
#ifdef CFG_STEREO
    z2.m_right = z1.right;
    if (z1.right) {
      Rectify(m_K.m_Kr, z1.x, &z2.m_z, &z2.m_W, &m_UMr, eps, &m_K.m_Rr);
    } else
#endif
    {
      Rectify(m_K.m_K, z1.x, &z2.m_z, &z2.m_W, &m_UM, eps);
    }
    ++j;
  }
  const int Nz2 = j;
  m_zsSortTmp.resize(Nz2);
  std::sort(m_zsSortTmp.begin(), m_zsSortTmp.end());
#ifdef CFG_DEBUG
  UT_ASSERT(m_zsSortTmp.empty() || m_zsSortTmp.front().m_id != -1);
#endif
  FTR::Measurement z;
  for (int i1 = 0, i2 = 0, iKF = 0; i1 < Nz2; i1 = i2) {
    const int id1 = m_zsSortTmp[i1].m_id;
    iKF = static_cast<int>(std::upper_bound(m_iKF2d.begin() + iKF, m_iKF2d.end(), id1) -
                                            m_iKF2d.begin()) - 1;
    const int id0 = m_iKF2d[iKF], id2 = m_iKF2d[iKF + 1];
#ifdef CFG_DEBUG
    UT_ASSERT(id1 >= id0 && id1 < id2);
#endif
    i2 = static_cast<int>(std::lower_bound(m_zsSortTmp.begin() + i1, m_zsSortTmp.end(), id2) -
                                           m_zsSortTmp.begin());
#ifdef CFG_DEBUG
    UT_ASSERT(i2 == Nz2 || m_zsSortTmp[i2].m_id >= id2);
    for (i = i1; i < i2; ++i) {
      const int id = m_zsSortTmp[i].m_id;
      UT_ASSERT(id >= id0 && id < id2);
    }
#endif
    m_zsTmp.resize(0);
    for (i = i1; i < i2; ++i) {
//#ifdef CFG_DEBUG
#if 0
      if (m_zsTmp.size() == 58) {
        UT::DebugStart();
        UT::DebugStop();
      }
#endif
      const FeatureMeasurement &_z = m_zsSortTmp[i];
      z.m_ix = _z.m_id - id0;
#ifdef CFG_STEREO
      if (_z.m_right) {
        z.m_zr = _z.m_z;
        z.m_Wr = _z.m_W;
        if (i == i1 || m_zsSortTmp[i - 1].m_id != _z.m_id) {
          z.m_z.Invalidate();
        }
        m_zsTmp.push_back(z);
      } else
#endif
      {
        z.m_z = _z.m_z;
        z.m_W = _z.m_W;
#ifdef CFG_STEREO
        j = i + 1;
        if (j == i2 || m_zsSortTmp[j].m_id != _z.m_id)
#endif
        {
          z.m_ix = _z.m_id - id0;
#ifdef CFG_STEREO
          z.m_zr.Invalidate();
#endif
          m_zsTmp.push_back(z);
        }
      }
    }
    F->PushFrameMeasurement(iKF, m_zsTmp);
  }
}

void Internal::AssertConsistency() {
  const int N = static_cast<int>(m_Ts.size()), nKFs = static_cast<int>(m_iFrmsKF.size());
  if (N > 0) {
    for (int i = 1; i < N; ++i) {
      UT_ASSERT(m_Ts[i - 1] < m_Ts[i]);
    }
    if (nKFs > 0) {
      UT_ASSERT(m_iFrmsKF.back() <= m_Ts.front().m_iFrm);
    }
  }
  UT_ASSERT(m_CsKF.Size() == nKFs);
  UT_ASSERT(static_cast<int>(m_iKF2d.size()) == nKFs + 1);
  for (int iKF = 0; iKF < nKFs; ++iKF) {
    UT_ASSERT(m_iKF2d[iKF] <= m_iKF2d[iKF + 1]);
  }
  const int Nd = m_iKF2d.back();
  int SNd = 0;
  const int NX = static_cast<int>(m_iX2d.size());
  for (int iX = 0; iX < NX; ++iX) {
    const int id = m_iX2d[iX];
    if (id == -1) {
      continue;
    }
    UT_ASSERT(m_id2X[id] == iX);
    ++SNd;
  }
  UT_ASSERT(SNd == Nd);
  UT_ASSERT(static_cast<int>(m_id2X.size()) == Nd && static_cast<int>(m_id2idx.size()) == Nd);
  for (int id = 0; id < Nd; ++id) {
    const int iX = m_id2X[id], idx = m_id2idx[id];
    UT_ASSERT(m_iX2d[iX] == id);
    if (idx != -1) {
      UT_ASSERT(m_idx2iX[idx] == iX);
    }
  }
  const int idxMax = static_cast<int>(m_idx2iX.size() - 1);
  for (int idx = 0; idx <= idxMax; ++idx) {
    const int iX = m_idx2iX[idx];
    if (iX == -1) {
      continue;
    }
    const int id = m_iX2d[iX];
    if (id != -1) {
      UT_ASSERT(m_id2idx[id] == idx);
    }
  }
  UT_ASSERT(static_cast<int>(m_xs.size()) == Nd);
  UT_ASSERT(static_cast<int>(m_ds.size()) == Nd);
}

void LoadParameters(const std::string fileName) {
  if (!UT::FileExists(fileName)) {
    return;
  }
  const Configurator cfgor(fileName.c_str());
  //cfgor.Print();
  ::LoadParameters(cfgor);
}

bool SaveCalibration(const std::string fileName, const Calibration &K) {
  FILE *fp = fopen(fileName.c_str(), "wb");
  if (!fp) {
    return false;
  }
  UT::SaveB(K, fp);
  fclose(fp);
  UT::PrintSaved(fileName);
  return true;
}

bool LoadCalibration(const std::string fileName, Calibration *K) {
  FILE *fp = fopen(fileName.c_str(), "rb");
  if (!fp) {
    return false;
  }
  UT::LoadB(*K, fp);
  fclose(fp);
  UT::PrintLoaded(fileName);
//#ifdef CFG_DEBUG
#if 0
  K->ba[0] = 1.0f;
  K->ba[1] = -1.0f;
  K->ba[2] = 0.5f;
  UT::Check("Noise\n");
#endif
  return true;
}

bool LoadCalibration(const std::string fileName, float T[3][4], Intrinsic *K,
                     float *ba, float *bw/*, float *sa*/) {
  FILE *fp = fopen(fileName.c_str(), "r");
  if (!fp) {
    return false;
  }
  if (ba) {
    ba[0] = ba[1] = ba[2] = 0.0f;
  }
  if (bw) {
    bw[0] = bw[1] = bw[2] = 0.0f;
  }
  //if (sa) {
  //  sa[0] = sa[1] = sa[2] = 1.0f;
  //}
/*  const std::string ext = UT::FileNameExtractExtension(fileName);
  if (ext == "txt") {
    memset(T, 0, sizeof(float) * 12);
    T[0][0] = T[1][1] = T[2][2] = 1.0f;
    if (fscanf(fp, "%f %f %f %f", &K->fx, &K->fy, &K->cx, &K->cy) != 4)
      return false;
    memset(K->ds, 0, sizeof(K->ds));
    for (int i = 0; i < 8 && UT::Load<float>(K->ds[i], fp); ++i) {}
  }
  else if (ext == "yaml") */{
    int i = 0, j = 0;
    float k[3];
    char line[UT_STRING_WIDTH_MAX];
    memset(K->ds, 0, sizeof(K->ds));
    while (fgets(line, UT_STRING_WIDTH_MAX, fp)) {
      const int len = int(std::remove(line, std::remove(line, line + strlen(line), 10), ' ') - line);
      if (len == 0 || line[len - 1] == ':')
        continue;
      line[len] = 0;
      switch (i) {
      case 0: {
        if (j == 3 ||
            sscanf(line, "- [%f, %f, %f, %f]", &T[j][0], &T[j][1], &T[j][2], &T[j][3]) == 4) {
          ++j;
        }
        break; }
      case 1: {
        if (sscanf(line, "- [%f, %f, %f]", &k[0], &k[1], &k[2]) == 3) {
          if (j == 0) {
            K->fx = k[0];
            K->cx = k[2];
          } else if (j == 1) {
            K->fy = k[1];
            K->cy = k[2];
          }
          ++j;
        }
        break; }
      case 2: {
        if (sscanf(line, "- [%f]", &K->ds[j]) == 1)
          ++j;
        break; }
      case 3: {
        if (j == 0 && ba) {
          sscanf(line, "accel_bias: [%f, %f, %f]", &ba[0], &ba[1], &ba[2]);
        }
        if (j == 1 && bw) {
          sscanf(line, "gyro_bias: [%f, %f, %f]", &bw[0], &bw[1], &bw[2]);
        }
        //if (j == 2 && sa) {
        //  sscanf(line, "accel_scale: [%f, %f, %f]", &sa[0], &sa[1], &sa[2]);
        //}
        ++j;
        break; }
      }
      if ((i == 0 && j == 4) || (i == 1 && j == 3) || (i == 2 && j == 8) || (i == 3 && j == 2)) {
        j = 0;
        ++i;
      }
    }
    if (i < 3) {
      return false;
    }
    Rotation3D R;
    R.Set(T[0], T[1], T[2]);
    R.MakeOrthogonal();
    R.Get(T[0], T[1], T[2]);
  }
  UT::PrintLoaded(fileName);
  return true;
}

void PrintCalibration(const Calibration &K) {
  Rigid3D T;
  T.Set(&K.Tu[0][0]);
  T.Print("Tcu = ", false);
#ifdef CFG_STEREO
  T.Set(&K.Tr[0][0]);
  T.Print("Tlr = ", false);
#endif
  UT::Print("K   = %f %f %f %f\n", K.K.fx, K.K.fy, K.K.cx, K.K.cy);
#ifdef CFG_STEREO
  UT::Print("Kr  = %f %f %f %f\n", K.Kr.fx, K.Kr.fy, K.Kr.cx, K.Kr.cy);
#endif
}

bool SaveGroundTruth(const std::string fileName, const std::vector<CameraIMUState> &XsGT) {
  return UT::VectorSaveB(XsGT, fileName.c_str());
}

bool LoadGroundTruth(const std::string fileName, std::vector<CameraIMUState> *XsGT) {
  return UT::VectorLoadB(*XsGT, fileName.c_str());
}

bool SaveGroundTruth(const std::string fileName, const std::vector<Depth> &dsGT) {
  return UT::VectorSaveB(dsGT, fileName.c_str());
}

bool LoadGroundTruth(const std::string fileName, std::vector<Depth> *dsGT) {
  if (!UT::VectorLoadB(*dsGT, fileName.c_str())) {
    return false;
  }
  const int N = static_cast<int>(dsGT->size());
  for (int i = 0; i < N; ++i) {
    Depth &d = dsGT->at(i);
    if (d.d != 0.0f) {
      continue;
    }
    d.d = DEPTH_INITIAL;
    d.s2 = DEPTH_VARIANCE_INITIAL;
  }
  return true;
}

bool LoadKeyFrames(const std::string fileName, const std::vector<float> &ts,
                   std::vector<ubyte> *kfs, const float dtMax) {
  FILE *fp = fopen(fileName.c_str(), "r");
  if (!fp) {
    kfs->resize(0);
    return false;
  }
  char line[UT_STRING_WIDTH_MAX];
  int kf, iFrm;
  float t, dt1, dt2;
  std::vector<ubyte> &_kfs = *kfs;
  _kfs.assign(ts.size(), 0);
  std::vector<float>::const_iterator it = ts.begin();
  while (fgets(line, UT_STRING_WIDTH_MAX, fp)) {
    if (line[0] == '#') {
      continue;
    }
    sscanf(line, "%f %d", &t, &kf);
    it = std::lower_bound(it, ts.end(), t);
    dt1 = it == ts.begin() ? FLT_MAX : (t - *(it - 1));
    dt2 = it == ts.end() ? FLT_MAX : (*it - t);
    if (dt1 > dtMax && dt2 > dtMax) {
      continue;
    } else if (dt1 < dt2) {
      iFrm = static_cast<int>(it - ts.begin()) - 1;
    } else {
      iFrm = static_cast<int>(it - ts.begin());
    }
    _kfs[iFrm] = static_cast<ubyte>(kf);
  }
  if (!_kfs.empty()) {
    _kfs.front() = 1;
  }
  fclose(fp);
  UT::PrintLoaded(fileName);
  return true;
}

bool LoadKeyFrames(const std::string fileName, std::vector<int> *iFrms) {
  iFrms->resize(0);
  FILE *fp = fopen(fileName.c_str(), "r");
  if (!fp) {
    return false;
  }
  int iFrm;
  while (fscanf(fp, "%d", &iFrm) == 1) {
    iFrms->push_back(iFrm);
  }
  fclose(fp);
  return true;
}

bool SaveCurrentFrame(const std::string fileName, const CurrentFrame &CF, const KeyFrame &KF) {
  FILE *fp = fopen(fileName.c_str(), "wb");
  if (!fp) {
    return false;
  }
  UT::SaveB(CF.iFrm, fp);
  UT::SaveB(CF.C, fp);
  UT::VectorSaveB(CF.zs, fp);
  UT::VectorSaveB(CF.us, fp);
  UT::SaveB(CF.t, fp);
  UT::SaveB(CF.d, fp);
  UT::StringSaveB(CF.fileName, fp);
#ifdef CFG_STEREO
  UT::StringSaveB(CF.fileNameRight, fp);
#endif
  UT::SaveB(KF.iFrm, fp);
  UT::SaveB(KF.C, fp);
  UT::VectorSaveB(KF.zs, fp);
  const int NX = static_cast<int>(KF.Xs.size());
  UT::SaveB(NX, fp);
  for (int iX = 0; iX < NX; ++iX) {
    const MapPoint &X = KF.Xs[iX];
    UT::VectorSaveB(X.zs, fp);
    UT::SaveB(X.X, fp);
  }
  UT::SaveB(KF.d, fp);
  fclose(fp);
  return true;
}

bool LoadCurrentFrame(const std::string fileName, CurrentFrame *CF, KeyFrame *KF) {
  FILE *fp = fopen(fileName.c_str(), "rb");
  if (!fp) {
    return false;
  }
  UT::LoadB(CF->iFrm, fp);
  UT::LoadB(CF->C, fp);
  UT::VectorLoadB(CF->zs, fp);
  UT::VectorLoadB(CF->us, fp);
  UT::LoadB(CF->t, fp);
  UT::LoadB(CF->d, fp);
  UT::StringLoadB(CF->fileName, fp);
#ifdef CFG_STEREO
  UT::StringLoadB(CF->fileNameRight, fp);
#endif
  UT::LoadB(KF->iFrm, fp);
  UT::LoadB(KF->C, fp);
  UT::VectorLoadB(KF->zs, fp);
  const int NX = UT::LoadB<int>(fp);
  KF->Xs.resize(NX);
  for (int iX = 0; iX < NX; ++iX) {
    MapPoint &X = KF->Xs[iX];
    UT::VectorLoadB(X.zs, fp);
    UT::LoadB(X.X, fp);
  }
  UT::LoadB(KF->d, fp);
  fclose(fp);
#ifdef CFG_DEBUG
//#if 1
  for (int iX = 0; iX < NX; ++iX) {
    const MapPoint &X = KF->Xs[iX];
    UT_ASSERT(X.zs.back().iFrm == KF->iFrm);
  }
#endif
//#ifdef CFG_DEBUG
#if 0
  const int Nz = static_cast<int>(CF->zs.size());
  for (int iz = 0; iz < Nz; ++iz) {
    const int idx = CF->zs[iz].idx;
    if (idx == 845) {
      UT::DebugStart();
      UT::DebugStop();
      UT::Print("[%d] %s iz = %d idx = %d\n", CF->iFrm, fileName.c_str(), iz, idx);
      exit(0);
    }
  }
#endif
//#ifdef CFG_DEBUG
#if 0
  if (KF->iFrm != -1) {
    for (int iX = 0; iX < NX; ++iX) {
      const int idx = KF->Xs[iX].X.idx;
      if (idx == 845) {
        UT::DebugStart();
        UT::DebugStop();
        UT::Print("[%d] %s iX = %d idx = %d\n", CF->iFrm, fileName.c_str(), iX, idx);
        exit(0);
      }
    }
  }
#endif
  return true;
}

bool LoadCurrentFrameTime(const std::string fileName, double *t) {
  FILE *fp = fopen(fileName.c_str(), "rb");
  if (!fp) {
    return false;
  }
  fseek(fp, sizeof(int) + sizeof(CameraIMUState), SEEK_CUR);
  const int Nz = UT::LoadB<int>(fp);
  fseek(fp, sizeof(MapPointMeasurement) * Nz, SEEK_CUR);
  const int Nu = UT::LoadB<int>(fp);
  fseek(fp, sizeof(IMUMeasurement) * Nu, SEEK_CUR);
  *t = static_cast<double>(UT::LoadB<float>(fp));
  fclose(fp);
  return true;
}

bool LoadRelativeConstraints(const std::string fileName, const std::vector<float> &ts,
                             std::vector<RelativeConstraint> *Zs, const float dtMax) {
  Zs->resize(0);
  FILE *fp = fopen(fileName.c_str(), "r");
  if (!fp) {
    return false;
  }
  char line[UT_STRING_WIDTH_MAX];
  float t1, t2;
  RelativeConstraint Z;
  //const float sr = 1.0f * UT_FACTOR_DEG_TO_RAD;
  //const float sr2 = sr * sr;
  std::string format = "%f";
  for (int i = 0; i < 40; ++i) {
    format = format + " %f";
  }
  memset(&Z.S[0][0], 0, sizeof(Z.S));
  while (fgets(line, UT_STRING_WIDTH_MAX, fp)) {
    if (line[0] == '#') {
      continue;
    }
    sscanf(line, format.c_str(),
           &t1, &t2,
           &Z.T.p[0], &Z.T.p[1], &Z.T.p[2],
           &Z.T.R[0][0], &Z.T.R[0][1], &Z.T.R[0][2],
           &Z.T.R[1][0], &Z.T.R[1][1], &Z.T.R[1][2],
           &Z.T.R[2][0], &Z.T.R[2][1], &Z.T.R[2][2],
           &Z.S[0][0], &Z.S[0][1], &Z.S[0][2],
           &Z.S[1][0], &Z.S[1][1], &Z.S[1][2],
           &Z.S[2][0], &Z.S[2][1], &Z.S[2][2],
           &Z.S[0][3], &Z.S[0][4], &Z.S[0][5],
           &Z.S[1][3], &Z.S[1][4], &Z.S[1][5],
           &Z.S[2][3], &Z.S[2][4], &Z.S[2][5],
           &Z.S[3][3], &Z.S[3][4], &Z.S[3][5],
           &Z.S[4][3], &Z.S[4][4], &Z.S[4][5],
           &Z.S[5][3], &Z.S[5][4], &Z.S[5][5]);
    //Z.S[3][3] = Z.S[4][4] = Z.S[5][5] = sr2;
    for (int i = 0; i < 3; ++i) {
      for (int j = 3; j < 6; ++j) {
        Z.S[j][i] = Z.S[i][j];
      }
    }
    const std::vector<float>::const_iterator it1 = std::lower_bound(ts.begin(), ts.end(), t1);
    const std::vector<float>::const_iterator it2 = std::lower_bound(ts.begin(), ts.end(), t2);
    if (it1 != ts.end() && fabs(t1 - *it1) <= dtMax && it2 != ts.end() && fabs(t2 - *it2) <= dtMax) {
      Z.iFrm1 = static_cast<int>(it1 - ts.begin());
      Z.iFrm2 = static_cast<int>(it2 - ts.begin());
      Zs->push_back(Z);
    }
  }
  fclose(fp);
  UT::PrintLoaded(fileName);
  return true;
}

void PrintCameraPose(const int iFrm, const CameraPose &C, const bool n) {
  const ::Point3D p = C.p;
  const Quaternion q = Rotation3D(C.R).GetQuaternion();
  if (n) {
    UT::Print("[%d]   %e %e %e   %e %e %e %e\n", iFrm, p.x(), p.y(), p.z(),
              q.x(), q.y(), q.z(), q.w());
  } else {
    UT::Print("\r[%d]   %f %f %f   %f %f %f %f", iFrm, p.x(), p.y(), p.z(),
              q.x(), q.y(), q.z(), q.w());
  }
}

void SaveCameraPose(const int iFrm, const CameraPose &C, FILE *fp) {
  const ::Point3D p = C.p;
  const Quaternion q = Rotation3D(C.R).GetQuaternion();
  fprintf(fp, "[%d]   %e %e %e   %e %e %e %e\n", iFrm, p.x(), p.y(), p.z(),
          q.x(), q.y(), q.z(), q.w());
}

bool LoadCameraPoses(const std::string fileName, std::vector<int> *iFrms,
                     std::vector<CameraPose> *Cs) {
  iFrms->resize(0);
  FILE *fp = fopen(fileName.c_str(), "r");
  if (!fp) {
    return false;
  }
  int iFrm;
  CameraPose C;
  while (fscanf(fp, "%d", &iFrm) == 1 &&
         fscanf(fp, "%e %e %e", &C.R[0][0], &C.R[0][1], &C.R[0][2]) == 3 &&
         fscanf(fp, "%e %e %e", &C.R[1][0], &C.R[1][1], &C.R[1][2]) == 3 &&
         fscanf(fp, "%e %e %e", &C.R[2][0], &C.R[2][1], &C.R[2][2]) == 3 &&
         fscanf(fp, "%e %e %e", &C.p[0], &C.p[1], &C.p[2]) == 3) {
    iFrms->push_back(iFrm);
    Cs->push_back(C);
  }
  fclose(fp);
  return true;
}

static inline bool AssertError(const int iFrm, const float e, const float eMax, const std::string str) {
  if (e <= eMax) {
    return true;
  }
  UT::Print("[%d] %s = %f > %f\n", iFrm, str.c_str(), e, eMax);
  return false;
}

bool AssertError(const int iFrm, const Error &e) {
  const bool ex = AssertError(iFrm, e.ex, MAX_ERROR_FEATURE, "ex");
  const bool eur = AssertError(iFrm, e.eur, MAX_ERROR_IMU_ROTATION, "eur");
  const bool eup = AssertError(iFrm, e.eup, MAX_ERROR_IMU_POSITION, "eup");
  const bool euv = AssertError(iFrm, e.euv, MAX_ERROR_IMU_VELOCITY, "euv");
  const bool euba = AssertError(iFrm, e.euba, MAX_ERROR_IMU_BIAS_ACCELERATION, "euba");
  const bool eubw = AssertError(iFrm, e.eubw, MAX_ERROR_IMU_BIAS_GYROSCOPE, "eubw");
  const bool edr = AssertError(iFrm, e.edr, MAX_ERROR_DRIFT_ROTATION, "edr");
  const bool edp = AssertError(iFrm, e.edp, MAX_ERROR_DRIFT_POSITION, "edp");
  return ex && eur && eup && euv && euba && eubw && edr && edp;
}

void PrintError(const std::string str, const Error &e) {
  const std::string _str(str.size(), ' ');
  UT::PrintSeparator();
  UT::Print("%sex = %f\n", str.c_str(), e.ex);
  UT::Print("%seu = %f %f %f %f %f\n", _str.c_str(), e.eur, e.eup, e.euv, e.euba, e.eubw);
  UT::Print("%sed = %f %f\n", _str.c_str(), e.edr, e.edp);
}

}  // namespace IBA