sSlam_ucoslam.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>
 
#ifdef WIN32
#include <direct.h>
#define GetCurrentDir _getcwd
#else
#include <unistd.h>
#define GetCurrentDir getcwd
#endif
 
std::string GetCurrentWorkingDir(void)
{
  char buff[FILENAME_MAX];
  GetCurrentDir(buff, FILENAME_MAX);
  std::string current_working_dir(buff);
 
  for (unsigned int i = 0; i < current_working_dir.length(); i++)
  {
    if (current_working_dir.substr(i, 1) == "\\")
      current_working_dir.replace(i, 1, "/");
  }
  return current_working_dir;
}
 
Sslam::Sslam() : btopenutils::Thread()
{
  mIsDirty = false;
  needUpdate = false;
  mScale = 1.0f;
  mSlam = 0;
  mFrameIdx = 0;
  
  //mParams.runSequential = true;//avoid parallel processing
  //mParams.global_optimizer = "g2o";//which global optimizer to use
  //mParams.detectMarkers = false;//(dis/e)nables marker detection
  //mParams.detectKeyPoints = true;//(dis/e)nables kp detection
  //mParams.kpDescriptorType = ucoslam::DescriptorTypes::Type::DESC_ORB;//keypoint descriptor employed
  //mParams.KPNonMaximaSuppresion = false;//activating it will create maps with less points (faster).
  //mParams.KFMinConfidence = 0.65;//value that regulates when a keyframe is added. Range (0,inf). low values will include less keyframes. high value will include more keyframes
  //mParams.maxFeatures = 1000;//number of features to be detected
  //mParams.nOctaveLevels = 8;//number of octaves for keypoint detection
  //mParams.scaleFactor = 1.2;//scale factor
  //mParams.KFCulling = 0.8;// Value indicating how many redundant keypoints must be in a keyframe to remove it. Range [0,1]. If low, few keyframes will survive. If 1, no keyframe will be removed once added.
  //mParams.aruco_markerSize = 0.08;//Size of markers in meters
  //mParams.maxNewPoints = 350;//maximum number of new points created when a new keyframe is added
  //mParams.forceInitializationFromMarkers = false;//If true, the system will not initialize until a good initialization from markers is obtained
  //mParams.nthreads_feature_detector = 2;//number of threads employed in keypoint detection
  //mParams.autoAdjustKpSensitivity = true;//enables/disables automatic keypoint detector sensitivity to adapt environement with low texture
  //mParams.kptImageScaleFactor = 1.0;//[0,1] indicates the desired scale factor employed for keypoint detection.  If 1 the original input image is used. Otherwise, the
    //input image is resized with the specified scale factor. PLease notice that markers will be detected in the  original input images anyway
 
  //aruco::MarkerDetector::Params aruco_DetectorParams;//the internal parameters of Aruco Library for marker detection
  //parameters of the aruco marker detector
  mParams.aruco_Dictionary = "ARUCO";
  mParams.aruco_DetectionMode = "DM_NORMAL";
  mParams.aruco_CornerRefimentMethod = "CORNER_LINES";
  mParams.aruco_minMarkerSize = 0;
 
#ifdef ANDROID
  mVocFile = CopyFileFromAssetToSD("btdata/orb_vocab.fbow", "orb_vocab.fbow", "btdata");
#else
  mVocFile = GetCurrentWorkingDir() + "/plugins/btdata/orb_vocab.fbow";
#endif
 
  mSlam = new ucoslam::UcoSlam();
  mMap = std::make_shared<ucoslam::Map>();
 
  try
  {
    mSlam->setParams(mMap, mParams, mVocFile);
 
    // Run thread
    StartThreading(boost::bind(&Sslam::GoThread, this));
  }
  catch (std::runtime_error)
  {
    //error
  }
}
 
Sslam::~Sslam()
{
  StopThreading();
 
  if (mSlam)
    mSlam->waitForFinished();
  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
  mCamParams.CamSize = camparam.CamSize;
  mCamParams.CameraMatrix = camparam.CameraMatrix;
  mCamParams.Distorsion = camparam.Distorsion;
  //mCamParams.resize(cv::Size((int)((float)camparam.CamSize.width * mScale), (int)((float)camparam.CamSize.height * mScale)));
}
 
void Sslam::InitDetector()
{
  boost::unique_lock<boost::recursive_mutex> ld(mUpdateMutex);
 
  try
  {
    // build a SLAM system
    mSlam->waitForFinished();
    mSlam->clear();
    mSlam->setParams(mMap, mParams, mVocFile);
  }
  catch (std::exception &)
  {
    SAFE_DELETE(mSlam);
  }
  mFrameIdx = 0;
  ld.unlock();
}
 
Vector3 Sslam::GetCameraPosition()
{
  return mLastCamPos;
}
 
Quaternion Sslam::GetCameraOrientation()
{
  return mLastCamQuat;
}
 
void Sslam::DrawLandmarks(cv::Mat image)
{
  if (mSlam && !image.empty())
  {
    //std::shared_ptr<openvslam::publish::frame_publisher> frame = mSlam->get_frame_publisher();
    //frame->draw_frame(true).copyTo(image);
  }
}
 
void Sslam::GoThread()
{
  while (IsRunning())
  {
    if (needUpdate)
    {
      ArManager* manager = ArManager::GetInstance();
      manager->GetLastData(mLastData);
 
      //cv::equalizeHist(mLastData.gray, mLastData.gray);
 
      needUpdate = false;
 
      //cv::blur(mLastData.gray, mLastData.gray, cv::Size(2, 2));
      cv::Size nsize;
      nsize.width = (int)((float)mLastData.gray.cols * mScale);
      nsize.height = (int)((float)mLastData.gray.rows * mScale);
      // Detect all markers seen on gray
 
      // camera size changed
      if (!mSlam || (((int)((float)mLastData.camParam.CamSize.width * mScale)) != mCamParams.CamSize.width || ((int)((float)mLastData.camParam.CamSize.height * mScale)) != mCamParams.CamSize.height))
        mIsDirty = true;
 
      // init detector
      if (mIsDirty)
      {
        mIsDirty = false;
        BuildCameraParam(mLastData.camParam);
        InitDetector();
      }
      
      if (mScale != 1.0f)
        cv::resize(mLastData.gray, mLastData.gray, mCamParams.CamSize, 0, 0, cv::INTER_LINEAR);
 
      // thread to detect do it after mutex lock
      if (!mLastData.image.empty() && mSlam)
      {
        cv::Mat mat = mSlam->process(mLastData.gray, mCamParams, mFrameIdx);
        mFrameIdx += 1;
 
        if (!mat.empty())
        {
          cv::Mat Rwc(3, 3, CV_32F);
          cv::Mat twc(3, 1, CV_32F);
          {
            Rwc = mat.rowRange(0, 3).colRange(0, 3).t();
            twc = -Rwc * mat.rowRange(0, 3).col(3);
          }
 
          double modelview_matrix[16];
          modelview_matrix[0 + 0 * 4] = Rwc.at<float>(0, 0);
          modelview_matrix[0 + 1 * 4] = Rwc.at<float>(1, 0);
          modelview_matrix[0 + 2 * 4] = Rwc.at<float>(2, 0);
          modelview_matrix[0 + 3 * 4] = 0.0;
 
          modelview_matrix[1 + 0 * 4] = Rwc.at<float>(0, 1);
          modelview_matrix[1 + 1 * 4] = Rwc.at<float>(1, 1);
          modelview_matrix[1 + 2 * 4] = Rwc.at<float>(2, 1);
          modelview_matrix[1 + 3 * 4] = 0.0;
 
          modelview_matrix[2 + 0 * 4] = Rwc.at<float>(0, 2);
          modelview_matrix[2 + 1 * 4] = Rwc.at<float>(1, 2);
          modelview_matrix[2 + 2 * 4] = Rwc.at<float>(2, 2);
          modelview_matrix[2 + 3 * 4] = 0.0;
 
          modelview_matrix[3 + 0 * 4] = twc.at<float>(0);
          modelview_matrix[3 + 1 * 4] = twc.at<float>(1);
          modelview_matrix[3 + 2 * 4] = twc.at<float>(2);
          modelview_matrix[3 + 3 * 4] = 1.0;
 
          mLastCamPos = Vector3(twc.at<float>(0), twc.at<float>(1), twc.at<float>(2));
 
          mLastCamQuat = Quaternion::FromRotationMatrix(modelview_matrix, mLastData.reversedBitmap);
        }
      }
    }
    else
    {
      //prevent cpu burn
      boost::this_thread::sleep_for(boost::chrono::milliseconds(1)); //DO not burn too much CPU
    }
  }
}