Skip to content

Commit

Permalink
Merge pull request #4 from lacie-life/3d-cuboid-dev
Browse files Browse the repository at this point in the history 
3d cuboid dev
  • Loading branch information
@lacie-life
lacie-life authored Feb 12, 2023
2 parents b452648 + 6b503a4 commit 1cb24b2
Show file tree
Hide file tree
Showing 52 changed files with 17,003 additions and 250 deletions.
5 changes: 4 additions & 1 deletion orb_slam3_ros/.vscode/settings.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,10 @@
"cinttypes": "cpp",
"typeindex": "cpp",
"typeinfo": "cpp",
"variant": "cpp"
"variant": "cpp",
"core": "cpp",
"numericaldiff": "cpp",
"specialfunctions": "cpp"
},
"python.autoComplete.extraPaths": [
"/home/lacie/slam_ws/devel/lib/python3/dist-packages",
Expand Down
8 changes: 8 additions & 0 deletions orb_slam3_ros/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,13 +133,21 @@ file(GLOB HEADERS
${CMAKE_CURRENT_SOURCE_DIR}/orb_slam3/include/*.hpp
${CMAKE_CURRENT_SOURCE_DIR}/orb_slam3/include/CameraModels/*.h
${CMAKE_CURRENT_SOURCE_DIR}/orb_slam3/include/cuda/*.hpp
${CMAKE_CURRENT_SOURCE_DIR}/orb_slam3/include/line_lbd/*.hpp
${CMAKE_CURRENT_SOURCE_DIR}/orb_slam3/include/line_lbd/*.h
${CMAKE_CURRENT_SOURCE_DIR}/orb_slam3/include/line_lbd/line_descriptor/*.hpp
${CMAKE_CURRENT_SOURCE_DIR}/orb_slam3/include/detect_3d_cuboid/*.h
)

file(GLOB SOURCES
${CMAKE_CURRENT_SOURCE_DIR}/orb_slam3/src/*.cc
${CMAKE_CURRENT_SOURCE_DIR}/orb_slam3/src/*.cpp
${CMAKE_CURRENT_SOURCE_DIR}/orb_slam3/src/CameraModels/*.cpp
${CMAKE_CURRENT_SOURCE_DIR}/orb_slam3/src/cuda/*.cu
${CMAKE_CURRENT_SOURCE_DIR}/orb_slam3/src/line_detect/libs/*.cpp
${CMAKE_CURRENT_SOURCE_DIR}/orb_slam3/src/line_detect/libs/*.hpp
${CMAKE_CURRENT_SOURCE_DIR}/orb_slam3/src/line_detect/*.cpp
${CMAKE_CURRENT_SOURCE_DIR}/orb_slam3/src/detect_3d_cuboid/*.cpp
)

source_group("Header Files" FILES ${HEADERS})
Expand Down
28 changes: 28 additions & 0 deletions orb_slam3_ros/config/RGB-D/TUM2.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,11 @@ Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500

Viewer.pangolin: 1
Viewer.rviz: 1
Viewer.readlocalobject: 0
Viewer.show_object3d_frame: 1

#--------------------------------------------------------------------------------------------
# PointCloud Mapping
#--------------------------------------------------------------------------------------------
Expand All @@ -90,3 +95,26 @@ octoMap.res: 0.05
# Dynamic point
#--------------------------------------------------------------------------------------------
Dynamic.flow: 10.0

#--------------------------------------------------------------------------------------------
# 3D cuboid
#--------------------------------------------------------------------------------------------
IE.rows: 18
IE.cols: 18
IE.P_occ: 0.6
IE.P_free: 0.4
IE.P_prior: 0.5
IE.Threshold: 5

#--------------------------------------------------------------------------------------------
# MapPublisher
#--------------------------------------------------------------------------------------------
# 1: robot_camera tf; 2: use imu
ConstraintType: 1
Trobot_camera.qx: 0.0003
Trobot_camera.qy: 0.8617
Trobot_camera.qz: -0.5072
Trobot_camera.qw: -0.0145
Trobot_camera.tx: 0.0 #-0.6832
Trobot_camera.ty: 0.0 #2.6909
Trobot_camera.tz: 1.7373
19 changes: 19 additions & 0 deletions orb_slam3_ros/orb_slam3/include/Converter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,8 @@ class Converter
static cv::Mat toCvMat(const Eigen::MatrixXf &m);
static cv::Mat toCvMat(const Eigen::MatrixXd &m);

static cv::Mat toCvMat(const Sophus::SE3f &T);

static cv::Mat toCvSE3(const Eigen::Matrix<double,3,3> &R, const Eigen::Matrix<double,3,1> &t);
static cv::Mat tocvSkewMatrix(const cv::Mat &v);

Expand All @@ -75,6 +77,23 @@ class Converter
//TODO: Sophus migration, to be deleted in the future
static Sophus::SE3<float> toSophus(const cv::Mat& T);
static Sophus::Sim3f toSophus(const g2o::Sim3& S);

// For 3D cuboid testing
static float bboxOverlapratio(const cv::Rect& rect1, const cv::Rect& rect2);
static float bboxOverlapratioLatter(const cv::Rect& rect1, const cv::Rect& rect2);
static float bboxOverlapratioFormer(const cv::Rect& rect1, const cv::Rect& rect2);

static cv::Mat Quation2CvMat(const double qx, const double qy, const double qz, const double qw, const double tx, const double ty, const double tz );
static Eigen::Matrix4d Quation2Eigen(const double qx, const double qy, const double qz, const double qw, const double tx, const double ty, const double tz );

static Eigen::Quaterniond ExtractQuaterniond(const Eigen::Isometry3d &Iso );
static Eigen::Quaterniond ExtractQuaterniond(const Eigen::Matrix4d &matrix );
static Eigen::Quaterniond ExtractQuaterniond(const cv::Mat &mat );

static Eigen::Isometry3d Matrix4dtoIsometry3d(const Eigen::Matrix4d &matrix );
static Eigen::Matrix4d Isometry3dtoMatrix4d(const Eigen::Isometry3d &Iso );
static Eigen::Matrix4d cvMattoMatrix4d(const cv::Mat &cvMat4);
static Eigen::Isometry3d cvMattoIsometry3d(const cv::Mat &cvMat4);
};

}// namespace ORB_SLAM
Expand Down
34 changes: 34 additions & 0 deletions orb_slam3_ros/orb_slam3/include/Frame.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,13 +32,22 @@

#include "Converter.h"
#include "Settings.h"
#include "YoloDetection.h"

#include <mutex>
#include <opencv2/opencv.hpp>

#include "Eigen/Core"
#include "sophus/se3.hpp"

// line
#include <line_lbd/line_descriptor.hpp>
#include <line_lbd/line_lbd_allclass.h>

// cube slam.
#include "detect_3d_cuboid/matrix_utils.h"
#include "detect_3d_cuboid/detect_3d_cuboid.h"

namespace ORB_SLAM3
{
#define FRAME_GRID_ROWS 48
Expand All @@ -49,6 +58,7 @@ class KeyFrame;
class ConstraintPoseImu;
class GeometricCamera;
class ORBextractor;
class Object_2D;

class Frame
{
Expand All @@ -68,6 +78,11 @@ class Frame
// Constructor for Monocular cameras.
Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, GeometricCamera* pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());

// For 3D cuboid
Frame(const cv::Mat &imColor, const cv::Mat &imGray, const cv::Mat &imDepth, const cv::Mat &imMask, const double &timeStamp,
ORBextractor* extractor, line_lbd_detect* line_lbd_ptr_frame, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf,
const float &thDepth, const std::vector<BoxSE>& bbox, GeometricCamera* pCamera,Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());

// Destructor
// ~Frame();

Expand Down Expand Up @@ -377,6 +392,25 @@ class Frame
}

Sophus::SE3<double> T_test;

// For 3D cuboid
public:
std::vector<BoxSE> boxes; // object box, vector<BoxSE> format.
//Eigen::MatrixXd boxes_eigen; // object box, Eigen::MatrixXd format.
cv::Mat mGroundtruthPose_mat; // camera groundtruth.
std::vector<Object_2D*> mvObject_2ds; // 2d object in current frame.
std::vector<Object_2D*> mvLastObject_2ds; // last frame.
std::vector<Object_2D*> mvLastLastObject_2ds; // last last frame.
bool AppearNewObject = false; // Whether new objects appear in the current frame.
cv::Mat mColorImage;
cv::Mat mQuadricImage; // On the basis of mColorImage, the ellipsoid is drawn

// line.
std::vector< KeyLine> keylines_raw, keylines_out;
cv::Mat all_lines_mat;
Eigen::MatrixXd all_lines_eigen;
std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> > vObjsLines;
line_lbd_detect* mpline_lbd_ptr_frame;
};

}// namespace ORB_SLAM
Expand Down
106 changes: 106 additions & 0 deletions orb_slam3_ros/orb_slam3/include/FrameDrawer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,112 @@ class FrameDrawer
map<long unsigned int, cv::Point2f> mmProjectPoints;
map<long unsigned int, cv::Point2f> mmMatchedInImage;

private:
std::vector<cv::Scalar> colors = { cv::Scalar(135,0,248),
cv::Scalar(255,0,253),
cv::Scalar(4,254,119),
cv::Scalar(255,126,1),
cv::Scalar(0,112,255),
cv::Scalar(0,250,250), };
std::vector<std::string> class_names = {
"person",
"bicycle",
"car",
"motorbike",
"aeroplane",
"bus",
"train",
"truck",
"boat",
"traffic light",
"fire hydrant",
"stop sign",
"parking meter",
"bench",
"bird",
"cat",
"dog",
"horse",
"sheep",
"cow",
"elephant",
"bear",
"zebra",
"giraffe",
"backpack",
"umbrella",
"handbag",
"tie",
"suitcase",
"frisbee",
"skis",
"snowboard",
"sports ball",
"kite",
"baseball bat",
"baseball glove",
"skateboard",
"surfboard",
"tennis racket",
"bottle",
"wine glass",
"cup",
"fork",
"knife",
"spoon",
"bowl",
"banana",
"apple",
"sandwich",
"orange",
"broccoli",
"carrot",
"hot dog",
"pizza",
"donut",
"cake",
"chair",
"sofa",
"pottedplant",
"bed",
"diningtable",
"toilet",
"tvmonitor",
"laptop",
"mouse",
"remote",
"keyboard",
"cell phone",
"microwave",
"oven",
"toaster",
"sink",
"refrigerator",
"book",
"clock",
"vase",
"scissors",
"teddy bear",
"hair drier",
"toothbrush"
};

public:
// color image
cv::Mat mQuadricIm;
bool mbshow_yolo_result;
cv::Mat GetQuadricImage();
cv::Mat DrawQuadricImage();

// bounding box.
std::vector<BoxSE> Dboxes;
cv::Mat DrawYoloInfo(cv::Mat &im, bool bText);
int CurFrameId = -1;

// lines.
std::vector< KeyLine> Dkeylines_raw_nouse, Dkeylines_out_nouse;
double DTimeStamp_nouse;
std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> > DObjsLines; // object lines. std::vector<Eigen::MatrixXd>
};

} //namespace ORB_SLAM
Expand Down
Loading

0 comments on commit 1cb24b2

Please sign in to comment.