sSlam.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 <chrono>
#include "DSO/dso.h"
#include "DSO/FullSystem/CoarseInitializer.h"
#include "DSO/IOWrapper/Output3DWrapper.h"
#include "DSO/util/Undistort.h"
#include "DSO/util/NumType.h"
#include "DSO/util/FrameShell.h"
#include "DSO/util/settings.h"
 
struct slamPoint
{
  dso::PointHessian* ptr;
  cv::Point2f point2d;
  cv::Mat point3d;
  cv::Scalar color;
  float depth;
};
 
class Plane
{
public:
  Plane() {}
  cv::Mat DetectPlane(cv::Mat campos, std::vector<slamPoint> &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<slamPoint> mMvMPs;
  //camera pose when the plane was first observed (to compute normal direction)
  cv::Mat mMTcw;
  cv::Mat mXC;
};
 
class Sslam : public dso::IOWrap::Output3DWrapper, btopenutils::Thread
{
        public:
    Sslam();
    ~Sslam();
  protected:
 
        private:
 
    LASTDATA mLastData;
    boost::mutex mImuMutex;
    boost::mutex mResetMutex;
    boost::mutex mDebugMutex;
    dso::FullSystem* mSlam;
    dso::Undistort* mUndistorter;
    dso::ModelParams mCamParams;
    Vector3 mLastCamPos;
    BtQuaternion mLastCamQuat;
    Vector3 mPrevCamPos;
    BtQuaternion mPrevCamQuat;
    std::vector<dso::ImuData> mImuFrame;
          uint32_t mFrameIdx;
    cv::Mat mPlane2World;
    cv::Mat mPlane2Camera;
    cv::Mat mCameraWorld;
    cv::Mat mCentroid;
    float mScaleX;
    float mScaleY;
    bool mIsDirty;
    bool mFound;
    bool mInitialized;
    bool mUseSensor;
    int mReady;
    std::string mVocFile;
    std::vector<slamPoint> mCloudPoints;
    std::chrono::steady_clock::time_point mLastTimeFrame;
    double mLastUpdate;
 
    void BuildCameraParam(const aruco::CameraParameters &camparam);
    void InitDetector();
    void Detect();
 
        public:
    // ==================== Output3DWrapper Functionality ======================
    virtual void publishGraph(const std::map<uint64_t, Eigen::Vector2i, std::less<uint64_t>, Eigen::aligned_allocator<std::pair<const uint64_t, Eigen::Vector2i>>> &connectivity) override;
    virtual void publishKeyframes(std::vector<dso::FrameHessian*> &frames, bool final, dso::CalibHessian* HCalib) override;
    virtual void publishCamPose(dso::FrameShell* frame, dso::CalibHessian* HCalib) override;
 
    virtual void pushLiveFrame(dso::FrameHessian* image) override;
    virtual void pushDepthImage(dso::MinimalImageB3* image) override;
    virtual bool needPushDepthImage() override;
 
    virtual void join() override;
    virtual void reset() override;
 
    bool needUpdate;
    void GoThread();
    void SetDirty();
    Vector3 GetCameraPosition();
    BtQuaternion GetCameraOrientation();
    bool IsFound();
    void Reset();
    void DrawLandmarks(cv::Mat image);
  protected:
 
        private:
};
 
#endif