sSlam_vins.cpp

Go to the documentation of this file.

/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see http://www.openspace3d.com
 
Copyright (c) 2012 I-maginer
 
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
 
This program 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 Lesser General Public License for more details.
 
You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt
 
-----------------------------------------------------------------------------
*/
 
//#include "sService.h"
#include "ArManager.h"
#include "ArCameraParam.h"
#include "sSlam.h"
 
#include <vector>
 
uint32 firstPO2From(uint32 n)
{
  uint32 n1 = n;
  --n1;
  n1 |= n1 >> 16;
  n1 |= n1 >> 8;
  n1 |= n1 >> 4;
  n1 |= n1 >> 2;
  n1 |= n1 >> 1;
  ++n1;
  n1 = n1 >> 1;
  
  return n1;
}
 
void Sophus2Cv(const dso::SE3 &smat, cv::Mat &mat)
{
  if (smat.unit_quaternion().norm() < dso::SE3::Scalar(1e-10))
    return;
 
  dso::Vec3f pos = smat.translation().cast<float>();
  dso::Mat33f rotation_matrix = smat.rotationMatrix().cast<float>();
 
  cv::Mat nmat(4, 4, CV_32F);
  nmat.at<float>(0, 0) = rotation_matrix(0, 0);
  nmat.at<float>(1, 0) = rotation_matrix(1, 0);
  nmat.at<float>(2, 0) = rotation_matrix(2, 0);
  nmat.at<float>(3, 0) = 0.0f;
  nmat.at<float>(0, 1) = rotation_matrix(0, 1);
  nmat.at<float>(1, 1) = rotation_matrix(1, 1);
  nmat.at<float>(2, 1) = rotation_matrix(2, 1);
  nmat.at<float>(3, 1) = 0.0f;
  nmat.at<float>(0, 2) = rotation_matrix(0, 2);
  nmat.at<float>(1, 2) = rotation_matrix(1, 2);
  nmat.at<float>(2, 2) = rotation_matrix(2, 2);
  nmat.at<float>(3, 2) = 0.0f;
  nmat.at<float>(0, 3) = pos.x();
  nmat.at<float>(1, 3) = pos.y();
  nmat.at<float>(2, 3) = pos.z();
  nmat.at<float>(3, 3) = 1.0f;
  nmat.copyTo(mat);
}
 
void cvMatToQuatAndPos(const cv::Mat mat, BtQuaternion &quat, Vector3 &vec, bool invertx = false, bool flip = false)
{
  double modelview_matrix[16];
  float invx = 1.0f;
 
  if (invertx)
    invx = -1.0f;
 
  modelview_matrix[0] = mat.at<float>(0, 0) * invx;
  modelview_matrix[1] = mat.at<float>(1, 0) * invx;
  modelview_matrix[2] = mat.at<float>(2, 0) * invx;
  modelview_matrix[3] = 0.0;
 
  modelview_matrix[4] = mat.at<float>(0, 1);
  modelview_matrix[5] = mat.at<float>(1, 1);
  modelview_matrix[6] = mat.at<float>(2, 1);
  modelview_matrix[7] = 0.0;
 
  modelview_matrix[8] = mat.at<float>(0, 2);
  modelview_matrix[9] = mat.at<float>(1, 2);
  modelview_matrix[10] = mat.at<float>(2, 2);
  modelview_matrix[11] = 0.0;
 
  modelview_matrix[12] = mat.at<float>(0, 3);
  modelview_matrix[13] = mat.at<float>(1, 3);
  modelview_matrix[14] = mat.at<float>(2, 3);
  modelview_matrix[15] = 1.0;
 
  quat = BtQuaternion(0.0f, 0.0f, 1.0f, 0.0f) * BtQuaternion::FromRotationMatrix(modelview_matrix, flip);
  vec = Vector3(static_cast<float>(flip ? -modelview_matrix[12] : modelview_matrix[12]), static_cast<float>(modelview_matrix[13]), static_cast<float>(modelview_matrix[14]));
}
 
const float eps = 1e-4;
cv::Mat ExpSO3(const float &x, const float &y, const float &z)
{
  cv::Mat I = cv::Mat::eye(3, 3, CV_32F);
  const float d2 = x * x + y * y + z * z;
  const float d = sqrt(d2);
  cv::Mat W;
  W = (cv::Mat_<float>(3, 3) << 0, -z, y, z, 0, -x, -y, x, 0);
 
  if (d < eps)
    return (I + W + 0.5f*W*W);
  else
    return (I + W * sin(d) / d + W * W*(1.0f - cos(d)) / d2);
}
 
cv::Mat ExpSO3(const cv::Mat &v)
{
  return ExpSO3(v.at<float>(0), v.at<float>(1), v.at<float>(2));
}
 
cv::Point2f Camera2Pixel(cv::Mat poseCamera, cv::Mat mk)
{
  return cv::Point2f(
    poseCamera.at<float>(0, 0) / poseCamera.at<float>(2, 0) * mk.at<float>(0, 0) + mk.at<float>(0, 2),
    poseCamera.at<float>(1, 0) / poseCamera.at<float>(2, 0) * mk.at<float>(1, 1) + mk.at<float>(1, 2)
  );
}
 
cv::Mat Plane::DetectPlane(cv::Mat campos, std::vector<slamPoint> &vMPs, const int iterations)
{
  // Retrieve 3D points
  unsigned int maxPoints = std::min(800, (int)vMPs.size());
  
  std::vector<cv::Mat> vPoints;
  vPoints.reserve(maxPoints);
  std::vector<slamPoint> vPointMP;
  vPointMP.reserve(maxPoints);
 
  for (size_t i = 0; i < maxPoints; i++)
  {
    slamPoint pMP = vMPs[i];
    if (pMP.ptr)
    {
      vPoints.push_back(pMP.point3d);
      vPointMP.push_back(pMP);
    }
  }
 
  const int N = vPoints.size();
 
  if (N < 50)
    return mTpw;
 
  // Indices for minimum set selection
  std::vector<size_t> vAllIndices;
  vAllIndices.reserve(N);
  std::vector<size_t> vAvailableIndices;
 
  for (int i = 0; i < N; i++)
  {
    vAllIndices.push_back(i);
  }
 
  float bestDist = 1e10;
  std::vector<float> bestvDist;
 
  //RANSAC
  for (int n = 0; n < iterations; n++)
  {
    vAvailableIndices = vAllIndices;
 
    cv::Mat A(3, 4, CV_32F);
    A.col(3) = cv::Mat::ones(3, 1, CV_32F);
 
    // Get min set of points
    for (short i = 0; i < 3; ++i)
    {
      int randi = Random(0, vAvailableIndices.size() - 1);
 
      int idx = vAvailableIndices[randi];
 
      A.row(i).colRange(0, 3) = vPoints[idx].t();
 
      vAvailableIndices[randi] = vAvailableIndices.back();
      vAvailableIndices.pop_back();
    }
 
    cv::Mat u, w, vt;
    cv::SVDecomp(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
 
    const float a = vt.at<float>(3, 0);
    const float b = vt.at<float>(3, 1);
    const float c = vt.at<float>(3, 2);
    const float d = vt.at<float>(3, 3);
 
    std::vector<float> vDistances(N, 0);
 
    const float f = 1.0f / sqrt(a*a + b * b + c * c + d * d);
 
    for (int i = 0; i < N; i++)
    {
      vDistances[i] = fabs(vPoints[i].at<float>(0)*a + vPoints[i].at<float>(1)*b + vPoints[i].at<float>(2)*c + d)*f;
    }
 
    std::vector<float> vSorted = vDistances;
    std::sort(vSorted.begin(), vSorted.end());
 
    int nth = std::max((int)(0.2*N), 20);
    const float medianDist = vSorted[nth];
 
    if (medianDist < bestDist)
    {
      bestDist = medianDist;
      bestvDist = vDistances;
    }
  }
 
  // Compute threshold inlier/outlier
  const float th = 1.4 * bestDist;
  std::vector<bool> vbInliers(N, false);
  int nInliers = 0;
  for (int i = 0; i < N; i++)
  {
    if (bestvDist[i] < th)
    {
      nInliers++;
      vbInliers[i] = true;
    }
  }
 
  std::vector<slamPoint> vInlierMPs(nInliers);
  int nin = 0;
  for (int i = 0; i < N; i++)
  {
    if (vbInliers[i])
    {
      vInlierMPs[nin] = vPointMP[i];
      nin++;
    }
  }
 
  campos.copyTo(mMTcw);
  mMvMPs = vInlierMPs;
  mRang = -3.14f / 2 + ((float)rand() / RAND_MAX)*3.14f;
  Recompute();
  return mTpw;
}
 
void Plane::Recompute()
{
  const int N = mMvMPs.size();
 
  // Recompute plane with all points
  cv::Mat A = cv::Mat(N, 4, CV_32F);
  A.col(3) = cv::Mat::ones(N, 1, CV_32F);
 
  mOrigin = cv::Mat::zeros(3, 1, CV_32F);
 
  int nPoints = 0;
  for (int i = 0; i < N; i++)
  {
    slamPoint pMP = mMvMPs[i];
    if (pMP.ptr)
    {
      cv::Mat Xw = pMP.point3d;
      mOrigin += Xw;
      A.row(nPoints).colRange(0, 3) = Xw.t();
      nPoints++;
    }
  }
 
  if (!nPoints)
    return;
 
  A.resize(nPoints);
 
  cv::Mat u, w, vt;
  cv::SVDecomp(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
 
  float a = vt.at<float>(3, 0);
  float b = vt.at<float>(3, 1);
  float c = vt.at<float>(3, 2);
 
  mOrigin = mOrigin * (1.0f / nPoints);
  const float f = 1.0f / sqrt(a * a + b * b + c * c);
 
  // Compute XC just the first time
  if (mXC.empty())
  {
    cv::Mat Oc = -mMTcw.colRange(0, 3).rowRange(0, 3).t() * mMTcw.rowRange(0, 3).col(3);
    mXC = Oc - mOrigin;
  }
 
  if ((mXC.at<float>(0) * a + mXC.at<float>(1) * b + mXC.at<float>(2) * c) < 0)
  {
    a = -a;
    b = -b;
    c = -c;
  }
 
  const float nx = a * f;
  const float ny = b * f;
  const float nz = c * f;
 
  mNormal = (cv::Mat_<float>(3, 1) << nx, ny, nz);
 
  cv::Mat up;
  up = (cv::Mat_<float>(3, 1) << 0.0f, 1.0f, 0.0f);
 
  cv::Mat v = up.cross(mNormal);
  const float sa = cv::norm(v);
  const float ca = up.dot(mNormal);
  const float ang = atan2(sa, ca);
 
  mTpw = cv::Mat::eye(4, 4, CV_32F);
 
  //mTpw.rowRange(0, 3).colRange(0, 3) = ExpSO3(v * ang / sa) * ExpSO3(up * mRang);
  ExpSO3(v*ang / sa).copyTo(mTpw.rowRange(0, 3).colRange(0, 3));
  mOrigin.copyTo(mTpw.col(3).rowRange(0, 3));
}
 
Sslam::Sslam()
{
  mIsDirty = false;
  needUpdate = false;
  mScaleX = 1.0f;
  mScaleY = 1.0f;
  mSlam = 0;
  mFrameIdx = 0;
  mFound = false;
  mInitialized = false;
  mUseSensor = false;
  mReady = 0;
  mLastUpdate = 0.0;
 
  dso::setting_solverMode = SOLVER_FIX_LAMBDA | SOLVER_ORTHOGONALIZE_X_LATER;
  dso::setting_solverModeDelta = 0.00001;
  dso::setting_forceAceptStep = true;
  dso::goStepByStep = false;
  dso::setting_useExposure = true;
  dso::setting_realTimeMaxKF = true;
  dso::setting_use_stereo = false;
  dso::stereo_weight = 0;
 
  dso::setting_use_imu_track = false;
  dso::setting_use_imu = false;
  dso::imu_weight = 1.0;
  dso::imu_weight_tracker = 0.1;
  dso::AccCov = dso::Mat33::Identity();
  dso::AccRandomWalkNoise = dso::Mat33::Identity();
  dso::GyrCov = dso::Mat33::Identity();
  dso::GyrRandomWalkNoise = dso::Mat33::Identity();
 
  dso::setting_use_optimize = true;
  dso::setting_use_Dmargin = true;
 
  dso::disableAllDisplay = true;
  dso::setting_render_plotTrackingFull = false;
  dso::setting_render_renderWindowFrames = false;
  dso::setting_logStuff = false;
  dso::setting_debugout_runquiet = true;
  dso::setting_desiredImmatureDensity = 500;
  dso::setting_desiredPointDensity = 650;
  dso::setting_minFrames = 2;
  dso::setting_maxFrames = 4;
  dso::setting_maxOptIterations = 3;
  dso::setting_minOptIterations = 1;
  dso::setting_minPointsRemaining = 0.05f;
  dso::setting_maxLogAffFacInWindow = 0.7f;
  dso::setting_kfGlobalWeight = 4.0;
  dso::setting_maxAffineWeight = 2.0;
  dso::setting_maxIMUSamples = 10000;
 
  dso::setting_maxPixSearch = 0.027f; // max length of the ep. line segment searched during immature point tracking. relative to image resolution.
  dso::setting_minTraceQuality = 4.0f;
  dso::setting_minTraceTestRadius = 3;
  dso::setting_GNItsOnPointActivation = 3;
  dso::setting_trace_stepsize = 1.0f;                           // stepsize for initial discrete search.
  dso::setting_trace_GNIterations = 3;                          // max # GN iterations
  dso::setting_trace_GNThreshold = 0.1f;                                // GN stop after this stepsize.
  dso::setting_trace_extraSlackOnTH = 1.2f;                     // for energy-based outlier check, be slightly more relaxed by this factor.
  dso::setting_trace_slackInterval = 1.5f;                      // if pixel-interval is smaller than this, leave it be.
  dso::setting_trace_minImprovementFactor = 2.0f;               // if pixel-interval is smaller than this, leave it be.
 
  dso::setting_photometricCalibration = 0;
  //dso::setting_affineOptModeA = 0;
  //dso::setting_affineOptModeB = 0;
  //dso::setting_initialRotPrior = 0;
  //dso::setting_initialTransPrior = 0;
  //dso::setting_initialAffBPrior = 0;
  //dso::setting_initialAffAPrior = 0;
  //dso::setting_initialCalibHessian = 0;
  //dso::setting_gammaWeightsPixelSelect = 1;
  dso::multiThreading = true;
  dso::sparsityFactor = 4;
 
  mCamParams.inWidth = 0;
  mCamParams.inHeight = 0;
 
  mUndistorter = 0;
 
  // Run thread
  StartThreading(boost::bind(&Sslam::GoThread, this));
}
 
Sslam::~Sslam()
{
  StopThreading();
 
  if (mSlam)
  {
    for (dso::IOWrap::Output3DWrapper* ow : mSlam->outputWrapper)
    {
      ow->join();
      if (ow != this)
        delete ow;
    }
  }
  SAFE_DELETE(mSlam);
  SAFE_DELETE(mUndistorter);
}
 
void Sslam::join()
{
}
 
void Sslam::reset()
{
  mReady = 0;
  mInitialized = false;
}
 
void Sslam::publishGraph(const std::map<uint64_t, Eigen::Vector2i, std::less<uint64_t>, Eigen::aligned_allocator<std::pair<const uint64_t, Eigen::Vector2i>>> &connectivity)
{
}
 
void Sslam::publishKeyframes(std::vector<dso::FrameHessian*> &frames, bool final, dso::CalibHessian* HCalib)
{
  float fx = HCalib->fxl();
  float fy = HCalib->fyl();
  float cx = HCalib->cxl();
  float cy = HCalib->cyl();
  float fxi = 1.0f / fx;
  float fyi = 1.0f / fy;
  float cxi = -cx / fx;
  float cyi = -cy / fy;
 
  cv::Mat camtoworld(4, 4, CV_32F);
  for (dso::FrameHessian* fh : frames)
  {
    Sophus2Cv((dso::T_WR_align.inverse() * fh->shell->camToWorld).inverse(), camtoworld);
 
    if (camtoworld.empty())
      continue;
 
    for (dso::PointHessian* ph : fh->pointHessians)
    {
      /*
      for (dso::PointFrameResidual* r : ph->residuals)
      {
        cv::Scalar cT;
 
        if (r->state_state == dso::ResState::INLIER) cT = cv::Scalar(255, 0, 0);
        else if (r->state_state == dso::ResState::OOB) cT = cv::Scalar(255, 255, 0);
        else if (r->state_state == dso::ResState::OUTLIER) cT = cv::Scalar(0, 0, 255);
        else cT = cv::Scalar(255, 255, 255);
 
        slamPoint spt;
        spt.ptr = ph;
        spt.color = cT;
        spt.point2d = cv::Point2f(ph->u + 0.5f, ph->v + 0.5f);
        spt.depth = 0.3f;
        {
          boost::mutex::scoped_lock lock(mDebugMutex);
          mCloudPoints.push_back(spt);
        }
      }
      */
 
      if (ph->idepth_scaled < 0)
        continue;
 
      if ((ph->status == dso::PointHessian::MARGINALIZED) || (ph->status == dso::PointHessian::ACTIVE))
      {
        float depth = 1.0f / ph->idepth;
        float x = (ph->u * fxi + cxi) * depth;
        float y = (ph->v * fyi + cyi) * depth;
        float z = depth * (1 + 2 * fxi);
        
        cv::Mat pm4 = camtoworld.inv() * cv::Mat(cv::Vec4f(x, y, z, 1.0));
        cv::Mat pm(cv::Point3f(pm4.at<float>(0), pm4.at<float>(1), pm4.at<float>(2)));
 
        slamPoint dspt;
        dspt.ptr = ph;
        dspt.point3d = pm;
        dspt.color = cv::Scalar(0, 0, 255);
        dspt.point2d = cv::Point2i(ph->u, ph->v);
        dspt.depth = ph->idepth_scaled;
 
        boost::mutex::scoped_lock lock(mDebugMutex);
        if (mCloudPoints.size() > dso::setting_desiredPointDensity * 3)
          mCloudPoints.erase(mCloudPoints.begin());
 
        mCloudPoints.push_back(dspt);
      }
    }
  }
}
 
void Sslam::publishCamPose(dso::FrameShell* frame, dso::CalibHessian* HCalib)
{
  cv::Mat camtoworld(4, 4, CV_32F);
  Sophus2Cv((dso::T_WR_align.inverse() * frame->camToWorld).inverse(), camtoworld);
 
  if (camtoworld.empty())
    return;
  
  camtoworld.copyTo(mCameraWorld);
 
  if ((mCloudPoints.size() > dso::setting_desiredPointDensity) && !mInitialized && !mCameraWorld.empty() && !mSlam->isLost && mSlam->initialized && !mSlam->initFailed)
  {
    boost::mutex::scoped_lock lock(mDebugMutex);
    Plane p;
    cv::Mat plane2w = p.DetectPlane(mCameraWorld, mCloudPoints, 50);
    if (!plane2w.empty())
    {
      mCentroid = p.mOrigin;
      mPlane2World = plane2w;
      mInitialized = true;
    }
  }
 
  if (mInitialized && frame->poseValid && !mPlane2World.empty())
  {
    mPlane2Camera = camtoworld * mPlane2World;
 
    mPrevCamPos = mLastCamPos;
    mPrevCamQuat = mLastCamQuat;
 
    if (mUseSensor)
    {
      camtoworld.at<float>(0, 3) = mPlane2Camera.at<float>(0, 3);
      camtoworld.at<float>(1, 3) = mPlane2Camera.at<float>(1, 3);
      camtoworld.at<float>(2, 3) = mPlane2Camera.at<float>(2, 3);
 
      cvMatToQuatAndPos(camtoworld.inv(), mLastCamQuat, mLastCamPos, true, mLastData.reversedBitmap);
    }
    else
    {
      cvMatToQuatAndPos(mPlane2Camera.inv(), mLastCamQuat, mLastCamPos, true, mLastData.reversedBitmap);
    }
 
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    mLastUpdate = std::chrono::duration<double, std::milli>(t1 - mLastTimeFrame).count();
    mLastTimeFrame = t1;
    mFound = true;
  }
  else
  {
    mFound = false;
  }
}
 
void Sslam::pushLiveFrame(dso::FrameHessian* image)
{
}
 
bool Sslam::needPushDepthImage()
{
  return false;
}
 
void Sslam::pushDepthImage(dso::MinimalImageB3* image)
{
}
 
void Sslam::SetDirty()
{
  mIsDirty = true;
}
 
void Sslam::BuildCameraParam(const aruco::CameraParameters &camparam)
{
  mScaleX = 1.0f;
  mScaleY = 1.0f;
  float scale = 1.0f;
 
  float maxsize = static_cast<float>(std::max(camparam.CamSize.width, camparam.CamSize.height));
  if (maxsize > 320)
    scale = 320.0f / maxsize;
 
  mScaleX = static_cast<float>(firstPO2From((float)camparam.CamSize.width * scale)) / static_cast<float>(camparam.CamSize.width);
  mScaleY = static_cast<float>(firstPO2From((float)camparam.CamSize.height * scale)) / static_cast<float>(camparam.CamSize.height);
 
  mCamParams.type = dso::Model_FOV;
  mCamParams.rectify = dso::Rectify_Crop;
 
  mCamParams.inWidth = (int)(static_cast<float>(camparam.CamSize.width) * mScaleX);
  mCamParams.inHeight = (int)(static_cast<float>(camparam.CamSize.height) * mScaleY);
 
  mCamParams.outWidth = mCamParams.inWidth;
  mCamParams.outHeight = mCamParams.inHeight;
 
  mCamParams.cx = static_cast<float>(mCamParams.inWidth) * 0.5f;  //(335.503f / 640.0f) * mCamParams.inWidth;//
  mCamParams.cy = static_cast<float>(mCamParams.inHeight) * 0.5f; //(213.655 / 360.0f) * mCamParams.inHeight;//
                                                                 
  float f = std::max(mCamParams.cx, mCamParams.cy) * 2.0f;
  mCamParams.fx = f; //(555.807f / 640.0f) * mCamParams.inWidth;
  mCamParams.fy = f; //(557.683 / 360.0f) * mCamParams.inHeight;
 
  mCamParams.omega = 0.0f;
}
 
void Sslam::InitDetector()
{
  try
  {
    if (mSlam)
    {
      mSlam->blockUntilMappingIsFinished();
      std::vector<dso::IOWrap::Output3DWrapper*> wraps = mSlam->outputWrapper;
 
      boost::mutex::scoped_lock lock(mResetMutex);
      SAFE_DELETE(mSlam);
 
      for (dso::IOWrap::Output3DWrapper* ow : wraps)
        ow->reset();
 
      SAFE_DELETE(mUndistorter);
      mUndistorter = dso::Undistort::getUndistorter(mCamParams, "", "");
 
      dso::setGlobalCalib((int)mUndistorter->getSize()[0], (int)mUndistorter->getSize()[1], mUndistorter->getK().cast<float>());
 
      float fsize = (float)mUndistorter->getSize()[0] + (float)mUndistorter->getSize()[1];
      dso::setting_maxShiftWeightT = 0.04f * fsize;
      dso::setting_maxShiftWeightR = 0.0f * fsize;
      dso::setting_maxShiftWeightRT = 0.02f * fsize;
 
      mSlam = new dso::FullSystem();
      mSlam->outputWrapper = wraps;
    }
    else
    {
      boost::mutex::scoped_lock lock(mResetMutex);
      SAFE_DELETE(mUndistorter);
      mUndistorter = dso::Undistort::getUndistorter(mCamParams, "", "");
 
      dso::setGlobalCalib((int)mUndistorter->getSize()[0], (int)mUndistorter->getSize()[1], mUndistorter->getK().cast<float>());
 
      float fsize = (float)mUndistorter->getSize()[0] + (float)mUndistorter->getSize()[1];
      dso::setting_maxShiftWeightT = 0.04f * fsize;
      dso::setting_maxShiftWeightR = 0.0f * fsize;
      dso::setting_maxShiftWeightRT = 0.02f * fsize;
 
      mSlam = new dso::FullSystem();
      mSlam->outputWrapper.push_back(this);
    }
 
    mSlam->linearizeOperation = true;
 
    if (mUndistorter && mUndistorter->photometricUndist != 0)
      mSlam->setGammaFunction(mUndistorter->photometricUndist->getG());
  }
  catch (std::exception &)
  {
    SAFE_DELETE(mSlam);
  }
 
  mLastTimeFrame = std::chrono::steady_clock::now();
  mFrameIdx = 0;
  mInitialized = false;
  mReady = 0;
  mFound = false;
 
  {
    boost::mutex::scoped_lock lock(mDebugMutex);
    mCloudPoints.clear();
  }
}
 
Vector3 Sslam::GetCameraPosition()
{
  if (mLastUpdate == 0.0)
    return mLastCamPos;
 
  double frame = 1.0 / mLastUpdate;
  double lerpcoef = frame * std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - mLastTimeFrame).count();
  lerpcoef = std::min(lerpcoef, 1.0);
 
  Vector3 lerpVec = mPrevCamPos + ((mLastCamPos - mPrevCamPos) * lerpcoef);
  return lerpVec;
}
 
BtQuaternion Sslam::GetCameraOrientation()
{
  if (mLastUpdate == 0.0)
    return mLastCamQuat;
 
  double frame = 1.0 / mLastUpdate;
  double lerpcoef = frame * std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - mLastTimeFrame).count();
  lerpcoef = std::min(lerpcoef, 1.0);
 
  return BtQuaternion::Slerp(lerpcoef, mPrevCamQuat, mLastCamQuat, true);
}
 
bool Sslam::IsFound()
{
  return mFound;
}
 
void Sslam::Reset()
{
  mCloudPoints.clear();
  mFound = false;
  mInitialized = false;
  mReady = 0;
 
  if (mSlam)
  {
    for (dso::IOWrap::Output3DWrapper* ow : mSlam->outputWrapper)
    {
      ow->join();
      if (ow != this)
        delete ow;
    }
  }
  mLastTimeFrame = std::chrono::steady_clock::now();
 
  SAFE_DELETE(mSlam);
  SAFE_DELETE(mUndistorter);
}
 
void Sslam::DrawLandmarks(cv::Mat image)
{
  if (mSlam && !image.empty())
  {
    //cv::Mat small;
    //cv::cvtColor(image, small, cv::COLOR_BGR2GRAY);
    //cv::resize(small, small, cv::Size(mCamParams.inWidth, mCamParams.inHeight), 0, 0, cv::INTER_LINEAR);
    //cv::equalizeHist(small, small);
    //cv::cvtColor(small, small, cv::COLOR_GRAY2BGR);
    //cv::resize(small, image, cv::Size(image.cols, image.rows), 0, 0, cv::INTER_LINEAR);
 
    if (!mCameraWorld.empty())
    {
      boost::mutex::scoped_lock lock(mDebugMutex);
      for (unsigned int i = 0; i < mCloudPoints.size(); i++)
      {
        slamPoint spt = mCloudPoints[i];
        if (!spt.point3d.empty())
        {
          cv::Mat pm4 = mCameraWorld * cv::Mat(cv::Vec4f(spt.point3d.at<float>(0), spt.point3d.at<float>(1), spt.point3d.at<float>(2), 1.0));
          cv::circle(image, Camera2Pixel(pm4, mLastData.arCamParam.camParam.CameraMatrix), 2, cv::Scalar(250, 0, 0));
        }
      }
    }
 
    if (!mCentroid.empty() && mInitialized)
    {
      std::vector<cv::Mat> axisPoints(4);
      axisPoints[0] = (cv::Mat_ <float>(4, 1) << 0.0, 0, 0, 1);
      axisPoints[1] = (cv::Mat_ <float>(4, 1) << 0.3, 0, 0, 1);
      axisPoints[2] = (cv::Mat_ <float>(4, 1) << 0.0, 0.3, 0, 1);
      axisPoints[3] = (cv::Mat_ <float>(4, 1) << 0, 0, 0.3, 1);
      std::vector<cv::Point2f> drawPoints(4);
      for (int i = 0; i < 4; i++)
      {
        axisPoints[i] = mPlane2Camera * axisPoints[i];
        drawPoints[i] = Camera2Pixel(axisPoints[i], mLastData.arCamParam.camParam.CameraMatrix);
      }
 
      cv::line(image, drawPoints[0], drawPoints[1], cv::Scalar(250, 0, 0), 5);
      cv::line(image, drawPoints[0], drawPoints[2], cv::Scalar(0, 250, 0), 5);
      cv::line(image, drawPoints[0], drawPoints[3], cv::Scalar(0, 0, 250), 5);
    }
  }
}
 
void Sslam::Detect()
{
  cv::equalizeHist(mLastData.gray, mLastData.gray);
  dso::MinimalImageB minImg((int)mLastData.gray.cols, (int)mLastData.gray.rows, (unsigned char*)mLastData.gray.data);
  dso::ImageAndExposure* undistImg = mUndistorter->undistort<unsigned char>(&minImg, 1, mLastData.timeStamp, 1.0f);
  dso::SE3 groundTruth;
 
  if (mLastData.arCamParam.GetCameraQuat() != BtQuaternion::IDENTITY)
  {
    mUseSensor = true;
    BtQuaternion quat = BtQuaternion(sqrt(0.5f), 0.0f, sqrt(0.5f), 0.0f) * (mLastData.arCamParam.GetCameraQuat() * BtQuaternion(0.0f, 1.0f, 0.0f, 0.0f));
    groundTruth.setQuaternion(dso::Quat_d(quat.w, quat.x, quat.y, quat.z));
  }
 
  std::vector<ArImuData> frameImu = mLastData.arCamParam.GetCameraImu();
  std::vector<dso::ImuData> imuCpy;
 
  // copy imu list
  if (!frameImu.empty())
  {
    dso::setting_use_imu_track = true;
    dso::setting_use_imu = true;
 
    for (unsigned int i = 0; i < frameImu.size(); i++)
      imuCpy.push_back(dso::ImuData(dso::Vec3(frameImu[i].m_gry.x, frameImu[i].m_gry.y, frameImu[i].m_gry.z), dso::Vec3(frameImu[i].m_acc.x, frameImu[i].m_acc.y, frameImu[i].m_acc.z), frameImu[i].m_delta));
  };
  mSlam->addActiveFrame(undistImg, 0, mFrameIdx, groundTruth, imuCpy);
 
  mFrameIdx += 1;
  SAFE_DELETE(undistImg);
 
  if (!mInitialized || mSlam->initFailed || !mSlam->initialized || mSlam->isLost)
    mFound = false;
}
 
void Sslam::GoThread()
{
  while (IsRunning())
  {
    ArManager* manager = ArManager::GetInstance();
 
    if (needUpdate)
    {
      manager->GetLastData(mLastData);
      needUpdate = false;
 
      cv::Size nsize;
      nsize.width = (int)((float)mLastData.gray.cols * mScaleX);
      nsize.height = (int)((float)mLastData.gray.rows * mScaleY);
 
      // camera size changed
      // or too much bad frames
      if (!mSlam || (nsize.width != mCamParams.inWidth) || (nsize.height != mCamParams.inHeight) ||
        (mSlam && mSlam->initialized && mSlam->initFailed) || (mSlam && (mFrameIdx > 50) && (!mSlam->initialized || mSlam->isLost)))
        mIsDirty = true;
 
      // init detector
      if (mIsDirty)
      {
        mIsDirty = false;
        BuildCameraParam(mLastData.arCamParam.GetCameraParameter());
        InitDetector();
      }
 
      if ((mScaleX != 1.0f) || (mScaleY != 1.0f))
        cv::resize(mLastData.gray, mLastData.gray, cv::Size(mCamParams.inWidth, mCamParams.inHeight), 0, 0, cv::INTER_LINEAR);
      //cv::blur(mLastData.gray, mLastData.gray, cv::Size(2, 2));
 
      if (!mLastData.image.empty() && mSlam && mUndistorter && (dso::pyrLevelsUsed >= 3))
      {
        Detect();
      }
    }
    else
    {
      //prevent cpu burn
      boost::this_thread::sleep_for(boost::chrono::milliseconds(1)); //DO not burn too much CPU
    }
  }
}