sSlam_sd.h

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
 
-----------------------------------------------------------------------------
*/
 
#ifndef __BTK_SSLAM_H__
#define __BTK_SSLAM_H__
 
#include "Prerequisites.h"
#include "ArCameraParam.h"
#include "ArManager.h"
#include "BitmapToolkitThread.h"
 
#include "SD_SLAM/System.h"
#include "SD_SLAM/Config.h"
 
class Plane
{
public:
  Plane() {}
  cv::Mat DetectPlane(const cv::Mat Tcw, const std::vector<SD_SLAM::MapPoint*> &vMPs, const int iterations);
  void Recompute();
 
  //normal
  cv::Mat mNormal;
  //origin
  cv::Mat mOrigin;
  //arbitrary orientation along normal
  float mRang;
  //transformation from world to the plane
  cv::Mat mTpw;
  //MapPoints that define the plane
  std::vector<SD_SLAM::MapPoint*> mMvMPs;
  //camera pose when the plane was first observed (to compute normal direction)
  cv::Mat mMTcw, mXC;
};
 
class Sslam : btopenutils::Thread
{
public:
  Sslam();
  ~Sslam();
protected:
 
private:
  LASTDATA mLastData;
  boost::recursive_mutex mUpdateMutex;
  SD_SLAM::System* mSlam;
  Vector3 mLastCamPos;
  BtQuaternion mLastCamQuat;
  cv::Mat mPlane2World;
  cv::Mat mPlane2Camera;
  cv::Mat mCameraWorld;
  cv::Mat mCentroid;
  std::chrono::steady_clock::time_point mTimestamp;
  float mScale;
  bool mIsDirty;
  bool mFound;
  bool mInitialized;
  int mFullInit;
  void BuildCameraParam(const aruco::CameraParameters &camparam);
  void InitDetector();
  void Detect();
 
public:
  bool needUpdate;
  void GoThread();
  void SetDirty();
  Vector3 GetCameraPosition();
  BtQuaternion GetCameraOrientation();
  bool IsFound();
  void Reset();
  void DrawLandmarks(cv::Mat image);
protected:
 
private:
};
 
#endif