sSlam_vslam.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 "openvslam/publish/map_publisher.h"
#include "openvslam/publish/frame_publisher.h"
#include "openvslam/data/landmark.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;
  mTimestamp = 0.0;
  mCfgNode = YAML::Load(
"#==============#\n\
# Camera Model #\n\
#==============#\n\
\n\
Camera.name: \"Default monocular\"\n\
Camera.setup : \"monocular\"\n\
Camera.model : \"perspective\"\n\
\n\
Camera.fx : 640.0\n\
Camera.fy : 640.0\n\
Camera.cx : 480.0\n\
Camera.cy : 480.0\n\
\n\
Camera.k1 : 0.0\n\
Camera.k2 : 0.0\n\
Camera.p1 : 0.0\n\
Camera.p2 : 0.0\n\
Camera.k3 : 0.0\n\
\n\
Camera.fps : 20.0\n\
Camera.cols : 640\n\
Camera.rows : 480\n\
\n\
Camera.color_order : \"Gray\"\n\
\n\
#================#\n\
# ORB Parameters #\n\
#================#\n\
\n\
Feature.max_num_keypoints: 1200\n\
Feature.scale_factor : 1.2\n\
Feature.num_levels : 8\n\
Feature.ini_fast_threshold : 20\n\
Feature.min_fast_threshold : 7\n\
\n\
#=====================#\n\
# Tracking Parameters #\n\
#=====================#\n\
\n\
depth_threshold: 35\n");
 
  mCfg = std::make_shared<openvslam::config>(mCfgNode);
 
  // Run thread
  StartThreading(boost::bind(&Sslam::GoThread, this));
}
 
Sslam::~Sslam()
{
  StopThreading();
 
  if (mSlam)
  {
    mSlam->abort_loop_BA();
          while (mSlam->loop_BA_is_running())
                  boost::this_thread::sleep_for(boost::chrono::milliseconds(100));
 
          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 / (float)maxsize;
 
  /*if (maxsize > 1600)
    mScale = 0.4f;
  else if (maxsize > 1200)
    mScale = 0.53333f;
  else if (maxsize > 750)
    mScale = 0.85333f;
  else if (maxsize > 640)
    mScale = 0.9f;
  else
    mScale = 1.0f;
 
#if defined(ANDROID) || defined(APPLE_IOS) || defined(RPI)
  if (maxsize > 640)
    mScale *= 0.75f;
#endif
  */
 
  //construct camParam
  mCfgNode["Camera.cols"] = (int)((float)camparam.CamSize.width * mScale);
  mCfgNode["Camera.rows"] = (int)((float)camparam.CamSize.height * mScale);
 
  float f = std::max(camparam.CamSize.width, camparam.CamSize.height);
  mCfgNode["Camera.fx"] = f * mScale;
  mCfgNode["Camera.fy"] = f * mScale;
  mCfgNode["Camera.cx"] = static_cast<float>(camparam.CamSize.width) * mScale * 0.5f;
  mCfgNode["Camera.cy"] = static_cast<float>(camparam.CamSize.height) * mScale * 0.5f;
 
  mCfg = std::make_shared<openvslam::config>(mCfgNode);
}
 
void Sslam::InitDetector()
{
  boost::unique_lock<boost::recursive_mutex> ld(mUpdateMutex);
 
  if (mSlam)
  {
    mSlam->abort_loop_BA();
          while (mSlam->loop_BA_is_running())
                  boost::this_thread::sleep_for(boost::chrono::milliseconds(100));
 
    mSlam->shutdown();
          SAFE_DELETE(mSlam);
  }
 
  try
  {
    std::string sdata;
#ifdef ANDROID
    sdata = CopyFileFromAssetToSD("btdata/orb_vocab.dbow2", "orb_vocab.dbow2", "btdata");
#else
    sdata = GetCurrentWorkingDir() + "/plugins/btdata/orb_vocab.dbow2";
#endif
 
    // build a SLAM system
    mSlam = new openvslam::system(mCfg, sdata);
    // startup the SLAM process
    mSlam->startup();
    mSlam->enable_mapping_module();
    mSlam->disable_loop_detector();
  }
  catch (std::exception &)
  {
    SAFE_DELETE(mSlam);
  }
  mTimestamp = 0.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()
{
  std::chrono::steady_clock::time_point lastTimeStamp = std::chrono::steady_clock::now();
  while (IsRunning())
  {
    if (needUpdate)
    {
      ArManager* manager = ArManager::GetInstance();
      manager->GetLastData(mLastData);
 
      cv::Size nsize;
      nsize.width = (int)((float)mLastData.gray.cols * mScale);
      nsize.height = (int)((float)mLastData.gray.rows * mScale);
 
      if (mScale != 1.0f)
        cv::resize(mLastData.gray, mLastData.gray, nsize, 0, 0, cv::INTER_LINEAR);
 
      //cv::equalizeHist(mLastData.gray, mLastData.gray);
 
      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)) != mCfg->camera_->cols_ || ((int)((float)mLastData.camParam.CamSize.height * mScale)) != mCfg->camera_->rows_))
        mIsDirty = true;
 
      // init detector
      if (mIsDirty)
      {
        mIsDirty = false;
        BuildCameraParam(mLastData.camParam);
        InitDetector();
      }
 
      // thread to detect do it after mutex lock
      if (!mLastData.image.empty() && mSlam)
      {
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
        double ttrack = std::chrono::duration_cast<std::chrono::duration<double>>(t1 - lastTimeStamp).count();
        mTimestamp += ttrack;
        lastTimeStamp = std::chrono::steady_clock::now();
 
        mSlam->feed_monocular_frame(mLastData.gray, mTimestamp);
 
        std::shared_ptr<openvslam::publish::map_publisher> map = mSlam->get_map_publisher();
        openvslam::Mat44_t mat = map->get_current_cam_pose().inverse().eval();
 
        //std::vector<openvslam::data::landmark*> all_landmarks;
        //std::set<openvslam::data::landmark*> local_landmarks;
        //map->get_landmarks(all_landmarks, local_landmarks);
 
        if (!isnan(mat(0, 3)))
        {
          openvslam::Mat33_t rotation_matrix = mat.block<3, 3>(0, 0);
          openvslam::Vec3_t translation_vector = mat.block<3, 1>(0, 3);
          
          openvslam::Quat_t quat(rotation_matrix);
          quat.normalize();
 
          mLastCamPos = Vector3(translation_vector(0), -translation_vector(1), -translation_vector(2));
          /*double modelview_matrix[16];
 
          modelview_matrix[0 + 0 * 4] = rotation_matrix(0, 0);
          modelview_matrix[0 + 1 * 4] = rotation_matrix(0, 1);
          modelview_matrix[0 + 2 * 4] = rotation_matrix(0, 2);
          modelview_matrix[0 + 3 * 4] = 0.0;
          modelview_matrix[1 + 0 * 4] = -rotation_matrix(1, 0);
          modelview_matrix[1 + 1 * 4] = -rotation_matrix(1, 1);
          modelview_matrix[1 + 2 * 4] = -rotation_matrix(1, 2);
          modelview_matrix[0 + 3 * 4] = 0.0;
          modelview_matrix[2 + 0 * 4] = rotation_matrix(2, 0);
          modelview_matrix[2 + 1 * 4] = rotation_matrix(2, 1);
          modelview_matrix[2 + 2 * 4] = rotation_matrix(2, 2);
          modelview_matrix[0 + 3 * 4] = 0.0;
          modelview_matrix[3 + 0 * 4] = 0.0;
          modelview_matrix[3 + 0 * 4] = 0.0;
          modelview_matrix[3 + 0 * 4] = 0.0;
          modelview_matrix[3 + 0 * 4] = 1.0;
          
          mLastCamQuat = Quaternion::FromRotationMatrix(modelview_matrix, mLastData.reversedBitmap);// *Quaternion(sqrt(0.5f), 0.0, sqrt(0.5f), 0.0f);
          */
          mLastCamQuat = Quaternion(-quat.w(), (mLastData.reversedBitmap) ? quat.x() : -quat.x(), quat.y(), quat.z());
        }
      }
    }
    else
    {
      //prevent cpu burn
      boost::this_thread::sleep_for(boost::chrono::milliseconds(1)); //DO not burn too much CPU
    }
  }
}