sSlam_sd.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_sd.h"
#include "SD_SLAM/extra/utils.h"
#include <vector>
#include <opencv2/core/eigen.hpp>
 
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(const cv::Mat Tcw, const std::vector<SD_SLAM::MapPoint*> &vMPs, const int iterations)
{
  // Retrieve 3D points
  std::vector<cv::Mat> vPoints;
  vPoints.reserve(vMPs.size());
  std::vector<SD_SLAM::MapPoint*> vPointMP;
  vPointMP.reserve(vMPs.size());
 
  for (size_t i = 0; i < vMPs.size(); i++)
  {
    SD_SLAM::MapPoint* pMP = vMPs[i];
    if (pMP && !pMP->isBad())
    {
      if (pMP->Observations() > 5)
      {
        Eigen::Vector3d Pos = pMP->GetWorldPos();
        cv::Mat pos(cv::Point3f(Pos(0), Pos(1), Pos(2)));
        vPoints.push_back(pos);
        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 = SD_SLAM::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<SD_SLAM::MapPoint*> vInlierMPs(nInliers, NULL);
  int nin = 0;
  for (int i = 0; i < N; i++)
  {
    if (vbInliers[i])
    {
      vInlierMPs[nin] = vPointMP[i];
      nin++;
    }
  }
 
  mMvMPs = vInlierMPs;
  mMTcw = Tcw.clone();
  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++)
  {
    SD_SLAM::MapPoint* pMP = mMvMPs[i];
    if (!pMP->isBad())
    {
      Eigen::Vector3d Pos = pMP->GetWorldPos();
      cv::Mat Xw(cv::Point3f(Pos(0), Pos(1), Pos(2)));
      mOrigin += Xw;
      A.row(nPoints).colRange(0, 3) = Xw.t();
      nPoints++;
    }
  }
  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() : btopenutils::Thread()
{
  mIsDirty = false;
  needUpdate = false;
  mScale = 1.0f;
  mSlam = 0;
  mFound = false;
  mInitialized = false;
  mPlane2World = cv::Mat::eye(4, 4, CV_32F);
 
  SD_SLAM::Config &params = SD_SLAM::Config::GetInstance();
  params.SetCameraIntrinsics(640, 480, 640, 640, 320, 240);
  params.SetUsePattern(false);
  params.SetOrbParameters(600);
 
  // Run thread
  StartThreading(boost::bind(&Sslam::GoThread, this));
}
 
Sslam::~Sslam()
{
  StopThreading();
  if (mSlam)
    mSlam->Shutdown();
 
  SAFE_DELETE(mSlam);
}
 
void Sslam::SetDirty()
{
  mIsDirty = true;
}
 
void Sslam::BuildCameraParam(const aruco::CameraParameters &camparam)
{
  mScale = 1.0f;
  int maxsize = std::max(camparam.CamSize.width, camparam.CamSize.height);
  if (maxsize > 640)
    mScale = 640.0f / (float)maxsize;  
 
  //construct camParam
  SD_SLAM::Config &params = SD_SLAM::Config::GetInstance();
  double f = std::max((int)(static_cast<float>(camparam.CamSize.width) * mScale), (int)(static_cast<float>(camparam.CamSize.height) * mScale));
  params.SetCameraIntrinsics((int)(static_cast<float>(camparam.CamSize.width) * mScale), (int)(static_cast<float>(camparam.CamSize.height) * mScale), f, f, (int)(static_cast<float>(camparam.CamSize.width) * mScale * 0.5f), (int)(static_cast<float>(camparam.CamSize.height) * mScale * 0.5f));
 
  int wlvl = SD_SLAM::Config::Width();
  int hlvl = SD_SLAM::Config::Height();
  int pyrLevels = 1;
  while (wlvl % 2 == 0 && hlvl % 2 == 0 && wlvl*hlvl > 5000 && pyrLevels < 12)
  {
    wlvl /= 2;
    hlvl /= 2;
    pyrLevels++;
  }
 
  params.SetPyrLevels(pyrLevels);
}
 
void Sslam::InitDetector()
{
  boost::unique_lock<boost::recursive_mutex> ld(mUpdateMutex);
 
  if (mSlam)
    mSlam->Shutdown();
 
  SAFE_DELETE(mSlam);
 
  try
  {
    // build a SLAM system
    mSlam = new SD_SLAM::System(SD_SLAM::System::MONOCULAR, true);
    mSlam->DeactivateLocalizationMode();
    //mSlam->ActivateLocalizationMode();
  }
  catch (std::exception &)
  {
    SAFE_DELETE(mSlam);
  }
  mFound = false;
  mInitialized = false;
  ld.unlock();
}
 
Vector3 Sslam::GetCameraPosition()
{
  return mLastCamPos;
}
 
BtQuaternion Sslam::GetCameraOrientation()
{
  return mLastCamQuat;
}
 
bool Sslam::IsFound()
{
  return mFound;
}
 
void Sslam::Reset()
{
  InitDetector();
}
 
void Sslam::DrawLandmarks(cv::Mat image)
{
  if (mSlam && !image.empty() && !mCameraWorld.empty())
  {
    //boost::unique_lock<boost::recursive_mutex> ld(mUpdateMutex);
    const std::vector<SD_SLAM::MapPoint*> mapPoints = mSlam->GetMap()->GetAllMapPoints();
    cv::Scalar color(0, 255, 0);
 
    for (unsigned int i = 0; i < mapPoints.size(); i++)
    {
      SD_SLAM::MapPoint* pMP = mapPoints[i];
      if (pMP && (pMP->Observations() >= 3))
      {
        Eigen::Vector3d Pos = pMP->GetWorldPos();
        cv::Mat pm4 = mCameraWorld * cv::Mat(cv::Vec4f(Pos(0), Pos(1), Pos(2), 1.0));
        cv::circle(image, Camera2Pixel(pm4, mLastData.camParam.CameraMatrix), 2, color);
      }
    }
 
    if (!mCentroid.empty() && (mSlam->GetTrackingState() == 2))
    {
      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.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);
 
      /*
      std::vector<cv::Point3f> drawRectPoints(8);
      drawRectPoints[0] = cv::Point3f(-0.15, 0, -0.15);
      drawRectPoints[1] = cv::Point3f(0.15, 0, -0.15);
      drawRectPoints[2] = cv::Point3f(-0.15, 0, 0.15);
      drawRectPoints[3] = cv::Point3f(-0.15, 0.3, -0.15);
      drawRectPoints[4] = cv::Point3f(-0.15, 0.3, 0.15);
      drawRectPoints[5] = cv::Point3f(0.15, 0.3, 0.15);
      drawRectPoints[6] = cv::Point3f(0.15, 0, 0.15);
      drawRectPoints[7] = cv::Point3f(0.15, 0.3, -0.15);
 
      cv::Mat Rcp, Tcp;
      std::vector<cv::Point2f> projectedPoints;
      cv::Rodrigues(mPlane2Camera.rowRange(0, 3).colRange(0, 3), Rcp);
      Tcp = mPlane2Camera.col(3).rowRange(0, 3);
      cv::projectPoints(drawRectPoints, Rcp, Tcp, tracker->GetCalibrationMatrix(), tracker->GetDistortionMatrix(), projectedPoints);
 
      cv::line(image, projectedPoints[0] / mScale, projectedPoints[1] / mScale, cv::Scalar(250, 0, 0), 5);
      cv::line(image, projectedPoints[0] / mScale, projectedPoints[2] / mScale, cv::Scalar(0, 250, 0), 5);
      cv::line(image, projectedPoints[0] / mScale, projectedPoints[3] / mScale, cv::Scalar(0, 0, 250), 5);
      cv::line(image, projectedPoints[1] / mScale, projectedPoints[7] / mScale, cv::Scalar(10, 0, 50), 2);
      cv::line(image, projectedPoints[3] / mScale, projectedPoints[7] / mScale, cv::Scalar(20, 0, 50), 2);
      cv::line(image, projectedPoints[3] / mScale, projectedPoints[4] / mScale, cv::Scalar(30, 0, 50), 2);
      cv::line(image, projectedPoints[2] / mScale, projectedPoints[4] / mScale, cv::Scalar(40, 0, 50), 2);
      cv::line(image, projectedPoints[1] / mScale, projectedPoints[6] / mScale, cv::Scalar(50, 0, 50), 2);
      cv::line(image, projectedPoints[2] / mScale, projectedPoints[6] / mScale, cv::Scalar(60, 0, 50), 2);
      cv::line(image, projectedPoints[4] / mScale, projectedPoints[5] / mScale, cv::Scalar(70, 0, 50), 2);
      cv::line(image, projectedPoints[5] / mScale, projectedPoints[6] / mScale, cv::Scalar(80, 0, 50), 2);
      cv::line(image, projectedPoints[5] / mScale, projectedPoints[7] / mScale, cv::Scalar(90, 0, 50), 2);
 
      cv::circle(image, projectedPoints[0] / mScale, 4, cv::Scalar(0, 0, 250), 1, 8);
      */
    }
 
    switch (mSlam->GetTrackingState())
    {
      case -1: {cv::putText(image, "SYSTEM NOT READY", cv::Point(100, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255), 1); }break;
      case 0: {cv::putText(image, "NO IMAGES YET", cv::Point(100, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255), 1); }break;
      case 1: {cv::putText(image, "SLAM NOT INITIALIZED", cv::Point(100, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255), 1); }break;
      case 2: {cv::putText(image, "SLAM ON", cv::Point(100, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 0), 1); break; }
      case 3: {cv::putText(image, "SLAM LOST", cv::Point(100, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255), 1); break; }
      default:break;
    }
    //ld.unlock();
  }
}
 
void Sslam::Detect()
{
  int prevState = mSlam->GetTrackingState();
  cv::Mat pose;
  {
    boost::unique_lock<boost::recursive_mutex> ld(mUpdateMutex);
 
    Eigen::Matrix4d epose = mSlam->TrackMonocular(mLastData.gray);
    cv::Mat cpose;
    eigen2cv(epose, cpose);
    cpose.convertTo(pose, CV_32F);
 
    if (mSlam->MapChanged() || (mSlam->GetTrackingState() < 2))
    {
      mInitialized = false;
      mFound = false;
    }
 
    //mFound = (mSlam->GetTrackingState() == 2) ? true : false;
    ld.unlock();
  }
  pose.copyTo(mCameraWorld);
 
  if (pose.empty() || (mSlam->GetTrackingState() != 2))
  {
    mFound = false;
 
    if ((prevState == 2) && (mSlam->GetTrackingState() == 3))
      mTimestamp = std::chrono::steady_clock::now();
 
    double passed = std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::steady_clock::now() - mTimestamp).count();
    if ((mSlam->GetTrackingState() == 3) && (passed > 4))
      Reset();
  }
  else
  {
    if (!mInitialized)
    {
      Plane mplane;
      const std::vector<SD_SLAM::MapPoint*> vpMPs = mSlam->GetMap()->GetAllMapPoints();
      //const std::vector<SD_SLAM::MapPoint*> vpTMPs = mSlam->GetTrackedMapPoints();
      if (vpMPs.size() > 200)
      {
        mPlane2World = mplane.DetectPlane(pose, vpMPs, 50);
 
        if (!mPlane2World.empty())
        {
          mCentroid = mplane.mOrigin;
          mInitialized = true;
          mPlane2Camera = pose * mPlane2World;
        }
      }
    }
    else
    {
      mPlane2Camera = pose * mPlane2World;
    }
 
    if (!mInitialized || pose.empty())
    {
      mFound = false;
    }
    else
    {
      mFound = true;
      cv::Mat camtoplane = mPlane2Camera.inv();
 
      double modelview_matrix[16];
      modelview_matrix[0] = -camtoplane.at<float>(0, 0);
      modelview_matrix[1] = -camtoplane.at<float>(1, 0);
      modelview_matrix[2] = -camtoplane.at<float>(2, 0);
      modelview_matrix[3] = 0.0;
 
      modelview_matrix[4] = camtoplane.at<float>(0, 1);
      modelview_matrix[5] = camtoplane.at<float>(1, 1);
      modelview_matrix[6] = camtoplane.at<float>(2, 1);
      modelview_matrix[7] = 0.0;
 
      modelview_matrix[8]  = camtoplane.at<float>(0, 2);
      modelview_matrix[9]  = camtoplane.at<float>(1, 2);
      modelview_matrix[10] = camtoplane.at<float>(2, 2);
      modelview_matrix[11] = 0.0;
 
      modelview_matrix[12] = camtoplane.at<float>(0, 3);
      modelview_matrix[13] = camtoplane.at<float>(1, 3);
      modelview_matrix[14] = camtoplane.at<float>(2, 3);
      modelview_matrix[15] = 1.0;
 
      mLastCamQuat = BtQuaternion(0.0f, 0.0f, 1.0f, 0.0f) * BtQuaternion::FromRotationMatrix(modelview_matrix, mLastData.reversedBitmap);
      mLastCamPos = Vector3(static_cast<float>(mLastData.reversedBitmap ? -modelview_matrix[12] : modelview_matrix[12]), static_cast<float>(modelview_matrix[13]), static_cast<float>(modelview_matrix[14]));
    }
  }
}
 
void Sslam::GoThread()
{
  while (IsRunning())
  {
    if (needUpdate)
    {
      ArManager* manager = ArManager::GetInstance();
      manager->GetLastData(mLastData);
      needUpdate = false;
 
      //cv::blur(mLastData.gray, mLastData.gray, cv::Size(2, 2));
 
      // Detect all markers seen on gray
 
      // camera size changed
      if (!mSlam || (((int)((float)mLastData.camParam.CamSize.width * mScale)) != SD_SLAM::Config::Width() || ((int)((float)mLastData.camParam.CamSize.height * mScale)) != SD_SLAM::Config::Height()))
        mIsDirty = true;
 
      // init detector
      if (mIsDirty)
      {
        mIsDirty = false;
        BuildCameraParam(mLastData.camParam);
        InitDetector();
      }
 
      if (mScale != 1.0f)
        cv::resize(mLastData.gray, mLastData.gray, cv::Size((int)((float)mLastData.gray.cols * mScale), (int)((float)mLastData.gray.rows * mScale)), 0, 0, cv::INTER_LINEAR);
 
      //cv::equalizeHist(mLastData.gray, mLastData.gray);
 
      // thread to detect do it after mutex lock
      if (!mLastData.image.empty() && mSlam)
      {
        Detect();
      }
    }
    else
    {
      //prevent cpu burn
      boost::this_thread::sleep_for(boost::chrono::milliseconds(1)); //DO not burn too much CPU
    }
  }
}