diff --git a/CMakeLists.txt b/CMakeLists.txt index fadce0f29c..556932de3a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,7 @@ endif() LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) -find_package(OpenCV 2.4.3 REQUIRED) +find_package(OpenCV 3 REQUIRED) find_package(Eigen3 3.1.0 REQUIRED) find_package(Pangolin REQUIRED) @@ -77,7 +77,7 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D) add_executable(rgbd_tum Examples/RGB-D/rgbd_tum.cc) -target_link_libraries(rgbd_tum ${PROJECT_NAME}) +target_link_libraries(rgbd_tum ${PROJECT_NAME} ${OpenCV_LIBS} opencv_core opencv_videoio opencv_highgui opencv_imgcodecs ) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo) @@ -85,11 +85,6 @@ add_executable(stereo_kitti Examples/Stereo/stereo_kitti.cc) target_link_libraries(stereo_kitti ${PROJECT_NAME}) -add_executable(stereo_euroc -Examples/Stereo/stereo_euroc.cc) -target_link_libraries(stereo_euroc ${PROJECT_NAME}) - - set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular) add_executable(mono_tum @@ -100,7 +95,3 @@ add_executable(mono_kitti Examples/Monocular/mono_kitti.cc) target_link_libraries(mono_kitti ${PROJECT_NAME}) -add_executable(mono_euroc -Examples/Monocular/mono_euroc.cc) -target_link_libraries(mono_euroc ${PROJECT_NAME}) - diff --git a/Examples/ROS/ORB_SLAM2/CMakeLists.txt b/Examples/ROS/ORB_SLAM2/CMakeLists.txt index 3c032fe8ab..61cd495665 100644 --- a/Examples/ROS/ORB_SLAM2/CMakeLists.txt +++ b/Examples/ROS/ORB_SLAM2/CMakeLists.txt @@ -30,7 +30,7 @@ endif() LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../../cmake_modules) -find_package(OpenCV 2.4.3 REQUIRED) +find_package(OpenCV 3 REQUIRED) find_package(Eigen3 3.1.0 REQUIRED) find_package(Pangolin REQUIRED) diff --git a/Examples/ROS/ORB_SLAM2/DenseInput.h b/Examples/ROS/ORB_SLAM2/DenseInput.h new file mode 100644 index 0000000000..c0b770a8d5 --- /dev/null +++ b/Examples/ROS/ORB_SLAM2/DenseInput.h @@ -0,0 +1,306 @@ +// Generated by gencpp from file svo_msgs/DenseInput.msg +// DO NOT EDIT! + + +#ifndef SVO_MSGS_MESSAGE_DENSEINPUT_H +#define SVO_MSGS_MESSAGE_DENSEINPUT_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace svo_msgs +{ +template +struct DenseInput_ +{ + typedef DenseInput_ Type; + + DenseInput_() + : header() + , frame_id(0) + , pose() + , image() + , min_depth(0.0) + , max_depth(0.0) { + } + DenseInput_(const ContainerAllocator& _alloc) + : header(_alloc) + , frame_id(0) + , pose(_alloc) + , image(_alloc) + , min_depth(0.0) + , max_depth(0.0) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef uint32_t _frame_id_type; + _frame_id_type frame_id; + + typedef ::geometry_msgs::Pose_ _pose_type; + _pose_type pose; + + typedef ::sensor_msgs::Image_ _image_type; + _image_type image; + + typedef float _min_depth_type; + _min_depth_type min_depth; + + typedef float _max_depth_type; + _max_depth_type max_depth; + + + + + typedef boost::shared_ptr< ::svo_msgs::DenseInput_ > Ptr; + typedef boost::shared_ptr< ::svo_msgs::DenseInput_ const> ConstPtr; + +}; // struct DenseInput_ + +typedef ::svo_msgs::DenseInput_ > DenseInput; + +typedef boost::shared_ptr< ::svo_msgs::DenseInput > DenseInputPtr; +typedef boost::shared_ptr< ::svo_msgs::DenseInput const> DenseInputConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::svo_msgs::DenseInput_ & v) +{ +ros::message_operations::Printer< ::svo_msgs::DenseInput_ >::stream(s, "", v); +return s; +} + +} // namespace svo_msgs + +namespace ros +{ +namespace message_traits +{ + + + +// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'sensor_msgs': ['/opt/ros/kinetic/share/sensor_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/kinetic/share/geometry_msgs/cmake/../msg'], 'svo_msgs': ['/home/ayush/catkin_ws/src/svo_msgs/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + + + + +template +struct IsFixedSize< ::svo_msgs::DenseInput_ > + : FalseType + { }; + +template +struct IsFixedSize< ::svo_msgs::DenseInput_ const> + : FalseType + { }; + +template +struct IsMessage< ::svo_msgs::DenseInput_ > + : TrueType + { }; + +template +struct IsMessage< ::svo_msgs::DenseInput_ const> + : TrueType + { }; + +template +struct HasHeader< ::svo_msgs::DenseInput_ > + : TrueType + { }; + +template +struct HasHeader< ::svo_msgs::DenseInput_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::svo_msgs::DenseInput_ > +{ + static const char* value() + { + return "cea677f47dcf08581cc9f5efece2f7e7"; + } + + static const char* value(const ::svo_msgs::DenseInput_&) { return value(); } + static const uint64_t static_value1 = 0xcea677f47dcf0858ULL; + static const uint64_t static_value2 = 0x1cc9f5efece2f7e7ULL; +}; + +template +struct DataType< ::svo_msgs::DenseInput_ > +{ + static const char* value() + { + return "svo_msgs/DenseInput"; + } + + static const char* value(const ::svo_msgs::DenseInput_&) { return value(); } +}; + +template +struct Definition< ::svo_msgs::DenseInput_ > +{ + static const char* value() + { + return "Header header\n\ +uint32 frame_id\n\ +geometry_msgs/Pose pose\n\ +sensor_msgs/Image image\n\ +float32 min_depth\n\ +float32 max_depth\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +time stamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Pose\n\ +# A representation of pose in free space, composed of position and orientation. \n\ +Point position\n\ +Quaternion orientation\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Point\n\ +# This contains the position of a point in free space\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +\n\ +================================================================================\n\ +MSG: geometry_msgs/Quaternion\n\ +# This represents an orientation in free space in quaternion form.\n\ +\n\ +float64 x\n\ +float64 y\n\ +float64 z\n\ +float64 w\n\ +\n\ +================================================================================\n\ +MSG: sensor_msgs/Image\n\ +# This message contains an uncompressed image\n\ +# (0, 0) is at top-left corner of image\n\ +#\n\ +\n\ +Header header # Header timestamp should be acquisition time of image\n\ + # Header frame_id should be optical frame of camera\n\ + # origin of frame should be optical center of cameara\n\ + # +x should point to the right in the image\n\ + # +y should point down in the image\n\ + # +z should point into to plane of the image\n\ + # If the frame_id here and the frame_id of the CameraInfo\n\ + # message associated with the image conflict\n\ + # the behavior is undefined\n\ +\n\ +uint32 height # image height, that is, number of rows\n\ +uint32 width # image width, that is, number of columns\n\ +\n\ +# The legal values for encoding are in file src/image_encodings.cpp\n\ +# If you want to standardize a new string format, join\n\ +# ros-users@lists.sourceforge.net and send an email proposing a new encoding.\n\ +\n\ +string encoding # Encoding of pixels -- channel meaning, ordering, size\n\ + # taken from the list of strings in include/sensor_msgs/image_encodings.h\n\ +\n\ +uint8 is_bigendian # is this data bigendian?\n\ +uint32 step # Full row length in bytes\n\ +uint8[] data # actual matrix data, size is (step * rows)\n\ +"; + } + + static const char* value(const ::svo_msgs::DenseInput_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::svo_msgs::DenseInput_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.frame_id); + stream.next(m.pose); + stream.next(m.image); + stream.next(m.min_depth); + stream.next(m.max_depth); + } + + ROS_DECLARE_ALLINONE_SERIALIZER; + }; // struct DenseInput_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::svo_msgs::DenseInput_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::svo_msgs::DenseInput_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "frame_id: "; + Printer::stream(s, indent + " ", v.frame_id); + s << indent << "pose: "; + s << std::endl; + Printer< ::geometry_msgs::Pose_ >::stream(s, indent + " ", v.pose); + s << indent << "image: "; + s << std::endl; + Printer< ::sensor_msgs::Image_ >::stream(s, indent + " ", v.image); + s << indent << "min_depth: "; + Printer::stream(s, indent + " ", v.min_depth); + s << indent << "max_depth: "; + Printer::stream(s, indent + " ", v.max_depth); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SVO_MSGS_MESSAGE_DENSEINPUT_H diff --git a/Examples/ROS/ORB_SLAM2/src/ros_mono.cc b/Examples/ROS/ORB_SLAM2/src/ros_mono.cc index 2913525bfc..170bfc4931 100644 --- a/Examples/ROS/ORB_SLAM2/src/ros_mono.cc +++ b/Examples/ROS/ORB_SLAM2/src/ros_mono.cc @@ -31,66 +31,150 @@ #include"../../../include/System.h" +#include +#include "tf/transform_datatypes.h" +#include +#include using namespace std; +ros::Publisher pose_pub; +ros::Publisher pub_dense; +double old_max; +double old_min; +bool init = false; + class ImageGrabber { public: - ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){} + ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){} - void GrabImage(const sensor_msgs::ImageConstPtr& msg); + void GrabImage(const sensor_msgs::ImageConstPtr& msg); - ORB_SLAM2::System* mpSLAM; + ORB_SLAM2::System* mpSLAM; }; int main(int argc, char **argv) { - ros::init(argc, argv, "Mono"); - ros::start(); + ros::init(argc, argv, "Mono"); + ros::start(); - if(argc != 3) - { - cerr << endl << "Usage: rosrun ORB_SLAM2 Mono path_to_vocabulary path_to_settings" << endl; - ros::shutdown(); - return 1; - } + if(argc != 3) + { + cerr << endl << "Usage: rosrun ORB_SLAM2 Mono path_to_vocabulary path_to_settings" << endl; + ros::shutdown(); + return 1; + } // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true); + ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true); - ImageGrabber igb(&SLAM); + ImageGrabber igb(&SLAM); - ros::NodeHandle nodeHandler; - ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb); - - ros::spin(); + ros::NodeHandle nodeHandler; + ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb); + pose_pub = nodeHandler.advertise("/camera_pose",1); + pub_dense = nodeHandler.advertise("/ORB/DenseInput",1); + ros::spin(); // Stop all threads - SLAM.Shutdown(); + SLAM.Shutdown(); // Save camera trajectory - SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); + SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); - ros::shutdown(); + ros::shutdown(); - return 0; + return 0; } void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg) { // Copy the ros image message to cv::Mat. - cv_bridge::CvImageConstPtr cv_ptr; - try - { - cv_ptr = cv_bridge::toCvShare(msg); - } - catch (cv_bridge::Exception& e) - { - ROS_ERROR("cv_bridge exception: %s", e.what()); - return; - } - - mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec()); + cv_bridge::CvImageConstPtr cv_ptr; + cv_bridge::CvImageConstPtr cv_ptr_rgb; + try + { + cv_ptr = cv_bridge::toCvShare(msg,sensor_msgs::image_encodings::MONO8); + cv_ptr_rgb = cv_bridge::toCvShare(msg,sensor_msgs::image_encodings::BGR8); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return; + } + + cv::Mat pose = mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec()); + if (pose.empty()) + return; + + cv::Mat TWC=mpSLAM->mpTracker->mCurrentFrame.mTcw.inv(); + cv::Mat RWC= TWC.rowRange(0,3).colRange(0,3); + cv::Mat tWC= TWC.rowRange(0,3).col(3); + + tf::Matrix3x3 M(RWC.at(0,0),RWC.at(0,1),RWC.at(0,2), + RWC.at(1,0),RWC.at(1,1),RWC.at(1,2), + RWC.at(2,0),RWC.at(2,1),RWC.at(2,2)); + tf::Vector3 V(tWC.at(0), tWC.at(1), tWC.at(2)); + tf::Quaternion q; + M.getRotation(q); + + static tf::TransformBroadcaster br; + tf::Transform transform = tf::Transform(M, V); + br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "init_link", "camera_pose")); + + geometry_msgs::PoseStamped _pose; + _pose.pose.position.x = transform.getOrigin().x(); + _pose.pose.position.y = transform.getOrigin().y(); + _pose.pose.position.z = transform.getOrigin().z(); + _pose.pose.orientation.x = transform.getRotation().x(); + _pose.pose.orientation.y = transform.getRotation().y(); + _pose.pose.orientation.z = transform.getRotation().z(); + _pose.pose.orientation.w = transform.getRotation().w(); + + _pose.header.stamp = ros::Time::now(); + _pose.header.frame_id = "init_link"; + pose_pub.publish(_pose); + + double min_z = std::numeric_limits::max(); + double max_z = std::numeric_limits::min(); + bool flag = mpSLAM->mpTracker->mCurrentFrame.getSceneDepth(mpSLAM->mpTracker->mCurrentFrame,max_z,min_z); + //ROS_INFO("REACHED 2"); + //ROS_INFO("Max: %f Min: %f",max_z,min_z); + if(flag) + { + old_min = min_z; + old_max = max_z; + init = true; + + } + if(init) + { + svo_msgs::DenseInput msg_dense; + msg_dense.header.stamp = ros::Time::now(); + msg_dense.header.frame_id = "world"; + + cv_bridge::CvImage img_msg; + img_msg.header.stamp=msg_dense.header.stamp; + img_msg.header.frame_id="camera"; + img_msg.image=cv_ptr_rgb->image; + + img_msg.encoding = sensor_msgs::image_encodings::BGR8; + msg_dense.image = *img_msg.toImageMsg(); + + msg_dense.min_depth = (float)old_min; + msg_dense.max_depth = (float)old_max; + + msg_dense.pose.position.x = tWC.at(0,0); + msg_dense.pose.position.y = tWC.at(1,0); + msg_dense.pose.position.z = tWC.at(2,0); + msg_dense.pose.orientation.x = q.x(); + msg_dense.pose.orientation.y = q.y(); + msg_dense.pose.orientation.z = q.z(); + msg_dense.pose.orientation.w = q.w(); + pub_dense.publish(msg_dense); + + } + } diff --git a/Examples/ROS/ORB_SLAM2/src/ros_stereo.cc b/Examples/ROS/ORB_SLAM2/src/ros_stereo.cc index 9a0f9acc35..81e747340c 100644 --- a/Examples/ROS/ORB_SLAM2/src/ros_stereo.cc +++ b/Examples/ROS/ORB_SLAM2/src/ros_stereo.cc @@ -1,4 +1,4 @@ -/** +/* * This file is part of ORB-SLAM2. * * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) @@ -25,6 +25,8 @@ #include #include +#include "tf/transform_datatypes.h" +#include #include #include #include @@ -33,8 +35,10 @@ #include #include"../../../include/System.h" - +#include using namespace std; +cv::Mat pose; +ros::Publisher pose_pub; class ImageGrabber { @@ -114,6 +118,7 @@ int main(int argc, char **argv) typedef message_filters::sync_policies::ApproximateTime sync_pol; message_filters::Synchronizer sync(sync_pol(10), left_sub,right_sub); sync.registerCallback(boost::bind(&ImageGrabber::GrabStereo,&igb,_1,_2)); + pose_pub = nh.advertise("/camera_pose",1); ros::spin(); @@ -121,15 +126,13 @@ int main(int argc, char **argv) SLAM.Shutdown(); // Save camera trajectory - SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_TUM_Format.txt"); - SLAM.SaveTrajectoryTUM("FrameTrajectory_TUM_Format.txt"); - SLAM.SaveTrajectoryKITTI("FrameTrajectory_KITTI_Format.txt"); + //SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_TUM_Format.txt"); + //SLAM.SaveTrajectoryTUM("FrameTrajectory_TUM_Format.txt"); + //SLAM.SaveTrajectoryKITTI("FrameTrajectory_KITTI_Format.txt"); ros::shutdown(); - return 0; } - void ImageGrabber::GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight) { // Copy the ros image message to cv::Mat. @@ -160,12 +163,43 @@ void ImageGrabber::GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const se cv::Mat imLeft, imRight; cv::remap(cv_ptrLeft->image,imLeft,M1l,M2l,cv::INTER_LINEAR); cv::remap(cv_ptrRight->image,imRight,M1r,M2r,cv::INTER_LINEAR); - mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec()); + pose = mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec()); } else { - mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec()); + pose = mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec()); } + + if (pose.empty()) + return; + + cv::Mat TWC=mpSLAM->mpTracker->mCurrentFrame.mTcw.inv(); + cv::Mat RWC= TWC.rowRange(0,3).colRange(0,3); + cv::Mat tWC= TWC.rowRange(0,3).col(3); + + tf::Matrix3x3 M(RWC.at(0,0),RWC.at(0,1),RWC.at(0,2), + RWC.at(1,0),RWC.at(1,1),RWC.at(1,2), + RWC.at(2,0),RWC.at(2,1),RWC.at(2,2)); + tf::Vector3 V(tWC.at(0), tWC.at(1), tWC.at(2)); + tf::Quaternion q; + M.getRotation(q); + + static tf::TransformBroadcaster br; + tf::Transform transform = tf::Transform(M, V); + br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "init_link", "camera_pose")); + + geometry_msgs::PoseStamped _pose; + _pose.pose.position.x = transform.getOrigin().x(); + _pose.pose.position.y = transform.getOrigin().y(); + _pose.pose.position.z = transform.getOrigin().z(); + _pose.pose.orientation.x = transform.getRotation().x(); + _pose.pose.orientation.y = transform.getRotation().y(); + _pose.pose.orientation.z = transform.getRotation().z(); + _pose.pose.orientation.w = transform.getRotation().w(); + + _pose.header.stamp = ros::Time::now(); + _pose.header.frame_id = "init_link"; + pose_pub.publish(_pose); } diff --git a/README.md b/README.md index 4710dad7b5..f0f9a9e052 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,20 @@ # ORB-SLAM2 -**Authors:** [Raul Mur-Artal](http://webdiis.unizar.es/~raulmur/), [Juan D. Tardos](http://webdiis.unizar.es/~jdtardos/), [J. M. M. Montiel](http://webdiis.unizar.es/~josemari/) and [Dorian Galvez-Lopez](http://doriangalvez.com/) ([DBoW2](https://github.com/dorian3d/DBoW2)) +Modified version of ORB-SLAM2 + +**Change Log** +[Author: Ayush Gaud] +Compatible with ROS Kinetic (OpenCV 3, Ubuntu 16.04) +Stereo node pose and tf publisher +Monocular pose publish +Compatible with REMODE (Topic: /ORB/DenseInput) +Added Support for coloured reconstruction (https://github.com/ayushgaud/rpg_open_remode) + +![coloured_reconstruction](https://cloud.githubusercontent.com/assets/4923897/26147541/7f7feec8-3b11-11e7-8b4c-5458aa922709.png) + +![orb remode](https://cloud.githubusercontent.com/assets/4923897/23068075/20bd81d4-f548-11e6-824d-ace54982f74b.png) + + +**Authors:** [Raul Mur-Artal (Original Author)](http://webdiis.unizar.es/~raulmur/), [Juan D. Tardos](http://webdiis.unizar.es/~jdtardos/), [J. M. M. Montiel](http://webdiis.unizar.es/~josemari/) and [Dorian Galvez-Lopez](http://doriangalvez.com/) ([DBoW2](https://github.com/dorian3d/DBoW2)) **Current version:** 1.0.0 diff --git a/Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h b/Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h index d61c633c71..f808e75ef9 100644 --- a/Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h +++ b/Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h @@ -53,7 +53,7 @@ class LinearSolverEigen: public LinearSolver public: typedef Eigen::SparseMatrix SparseMatrix; typedef Eigen::Triplet Triplet; - typedef Eigen::PermutationMatrix PermutationMatrix; + typedef Eigen::PermutationMatrix PermutationMatrix; /** * \brief Sub-classing Eigen's SimplicialLDLT to perform ordering with a given ordering */ diff --git a/include/Frame.h b/include/Frame.h index a6a8032f57..dbfadc1b6d 100644 --- a/include/Frame.h +++ b/include/Frame.h @@ -187,7 +187,7 @@ class Frame static bool mbInitialComputations; - + bool getSceneDepth(Frame& frame, double& depth_mean, double& depth_min); private: // Undistort keypoints given OpenCV distortion parameters. @@ -206,6 +206,8 @@ class Frame cv::Mat mtcw; cv::Mat mRwc; cv::Mat mOw; //==mtwc + + }; }// namespace ORB_SLAM diff --git a/include/System.h b/include/System.h index 59f4491e90..3486cb111b 100644 --- a/include/System.h +++ b/include/System.h @@ -112,6 +112,11 @@ class System // SaveMap(const string &filename); // LoadMap(const string &filename); + // Tracker. It receives a frame and computes the associated camera pose. + // It also decides when to insert a new keyframe, create some new MapPoints and + // performs relocalization if tracking fails. + Tracking* mpTracker; + private: // Input sensor @@ -126,10 +131,7 @@ class System // Map structure that stores the pointers to all KeyFrames and MapPoints. Map* mpMap; - // Tracker. It receives a frame and computes the associated camera pose. - // It also decides when to insert a new keyframe, create some new MapPoints and - // performs relocalization if tracking fails. - Tracking* mpTracker; + // Local Mapper. It manages the local map and performs local bundle adjustment. LocalMapping* mpLocalMapper; diff --git a/src/Frame.cc b/src/Frame.cc index 0e37d49335..b84e4e34ba 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -22,661 +22,702 @@ #include "Converter.h" #include "ORBmatcher.h" #include - namespace ORB_SLAM2 { -long unsigned int Frame::nNextId=0; -bool Frame::mbInitialComputations=true; -float Frame::cx, Frame::cy, Frame::fx, Frame::fy, Frame::invfx, Frame::invfy; -float Frame::mnMinX, Frame::mnMinY, Frame::mnMaxX, Frame::mnMaxY; -float Frame::mfGridElementWidthInv, Frame::mfGridElementHeightInv; + long unsigned int Frame::nNextId=0; + bool Frame::mbInitialComputations=true; + float Frame::cx, Frame::cy, Frame::fx, Frame::fy, Frame::invfx, Frame::invfy; + float Frame::mnMinX, Frame::mnMinY, Frame::mnMaxX, Frame::mnMaxY; + float Frame::mfGridElementWidthInv, Frame::mfGridElementHeightInv; -Frame::Frame() -{} + Frame::Frame() + {} //Copy Constructor -Frame::Frame(const Frame &frame) + Frame::Frame(const Frame &frame) :mpORBvocabulary(frame.mpORBvocabulary), mpORBextractorLeft(frame.mpORBextractorLeft), mpORBextractorRight(frame.mpORBextractorRight), - mTimeStamp(frame.mTimeStamp), mK(frame.mK.clone()), mDistCoef(frame.mDistCoef.clone()), - mbf(frame.mbf), mb(frame.mb), mThDepth(frame.mThDepth), N(frame.N), mvKeys(frame.mvKeys), - mvKeysRight(frame.mvKeysRight), mvKeysUn(frame.mvKeysUn), mvuRight(frame.mvuRight), - mvDepth(frame.mvDepth), mBowVec(frame.mBowVec), mFeatVec(frame.mFeatVec), - mDescriptors(frame.mDescriptors.clone()), mDescriptorsRight(frame.mDescriptorsRight.clone()), - mvpMapPoints(frame.mvpMapPoints), mvbOutlier(frame.mvbOutlier), mnId(frame.mnId), - mpReferenceKF(frame.mpReferenceKF), mnScaleLevels(frame.mnScaleLevels), - mfScaleFactor(frame.mfScaleFactor), mfLogScaleFactor(frame.mfLogScaleFactor), - mvScaleFactors(frame.mvScaleFactors), mvInvScaleFactors(frame.mvInvScaleFactors), - mvLevelSigma2(frame.mvLevelSigma2), mvInvLevelSigma2(frame.mvInvLevelSigma2) -{ - for(int i=0;i(NULL)) -{ + Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth) + :mpORBvocabulary(voc),mpORBextractorLeft(extractorLeft),mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), + mpReferenceKF(static_cast(NULL)) + { // Frame ID - mnId=nNextId++; + mnId=nNextId++; // Scale Level Info - mnScaleLevels = mpORBextractorLeft->GetLevels(); - mfScaleFactor = mpORBextractorLeft->GetScaleFactor(); - mfLogScaleFactor = log(mfScaleFactor); - mvScaleFactors = mpORBextractorLeft->GetScaleFactors(); - mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors(); - mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares(); - mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares(); + mnScaleLevels = mpORBextractorLeft->GetLevels(); + mfScaleFactor = mpORBextractorLeft->GetScaleFactor(); + mfLogScaleFactor = log(mfScaleFactor); + mvScaleFactors = mpORBextractorLeft->GetScaleFactors(); + mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors(); + mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares(); + mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares(); // ORB extraction - thread threadLeft(&Frame::ExtractORB,this,0,imLeft); - thread threadRight(&Frame::ExtractORB,this,1,imRight); - threadLeft.join(); - threadRight.join(); + thread threadLeft(&Frame::ExtractORB,this,0,imLeft); + thread threadRight(&Frame::ExtractORB,this,1,imRight); + threadLeft.join(); + threadRight.join(); - N = mvKeys.size(); + N = mvKeys.size(); - if(mvKeys.empty()) - return; + if(mvKeys.empty()) + return; - UndistortKeyPoints(); + UndistortKeyPoints(); - ComputeStereoMatches(); + ComputeStereoMatches(); - mvpMapPoints = vector(N,static_cast(NULL)); - mvbOutlier = vector(N,false); + mvpMapPoints = vector(N,static_cast(NULL)); + mvbOutlier = vector(N,false); // This is done only for the first Frame (or after a change in the calibration) - if(mbInitialComputations) - { - ComputeImageBounds(imLeft); + if(mbInitialComputations) + { + ComputeImageBounds(imLeft); - mfGridElementWidthInv=static_cast(FRAME_GRID_COLS)/(mnMaxX-mnMinX); - mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/(mnMaxY-mnMinY); + mfGridElementWidthInv=static_cast(FRAME_GRID_COLS)/(mnMaxX-mnMinX); + mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/(mnMaxY-mnMinY); - fx = K.at(0,0); - fy = K.at(1,1); - cx = K.at(0,2); - cy = K.at(1,2); - invfx = 1.0f/fx; - invfy = 1.0f/fy; + fx = K.at(0,0); + fy = K.at(1,1); + cx = K.at(0,2); + cy = K.at(1,2); + invfx = 1.0f/fx; + invfy = 1.0f/fy; - mbInitialComputations=false; - } + mbInitialComputations=false; + } - mb = mbf/fx; + mb = mbf/fx; - AssignFeaturesToGrid(); -} + AssignFeaturesToGrid(); + } -Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth) - :mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast(NULL)), - mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) -{ + Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth) + :mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast(NULL)), + mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) + { // Frame ID - mnId=nNextId++; + mnId=nNextId++; // Scale Level Info - mnScaleLevels = mpORBextractorLeft->GetLevels(); - mfScaleFactor = mpORBextractorLeft->GetScaleFactor(); - mfLogScaleFactor = log(mfScaleFactor); - mvScaleFactors = mpORBextractorLeft->GetScaleFactors(); - mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors(); - mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares(); - mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares(); + mnScaleLevels = mpORBextractorLeft->GetLevels(); + mfScaleFactor = mpORBextractorLeft->GetScaleFactor(); + mfLogScaleFactor = log(mfScaleFactor); + mvScaleFactors = mpORBextractorLeft->GetScaleFactors(); + mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors(); + mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares(); + mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares(); // ORB extraction - ExtractORB(0,imGray); + ExtractORB(0,imGray); - N = mvKeys.size(); + N = mvKeys.size(); - if(mvKeys.empty()) - return; + if(mvKeys.empty()) + return; - UndistortKeyPoints(); + UndistortKeyPoints(); - ComputeStereoFromRGBD(imDepth); + ComputeStereoFromRGBD(imDepth); - mvpMapPoints = vector(N,static_cast(NULL)); - mvbOutlier = vector(N,false); + mvpMapPoints = vector(N,static_cast(NULL)); + mvbOutlier = vector(N,false); // This is done only for the first Frame (or after a change in the calibration) - if(mbInitialComputations) - { - ComputeImageBounds(imGray); + if(mbInitialComputations) + { + ComputeImageBounds(imGray); - mfGridElementWidthInv=static_cast(FRAME_GRID_COLS)/static_cast(mnMaxX-mnMinX); - mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/static_cast(mnMaxY-mnMinY); + mfGridElementWidthInv=static_cast(FRAME_GRID_COLS)/static_cast(mnMaxX-mnMinX); + mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/static_cast(mnMaxY-mnMinY); - fx = K.at(0,0); - fy = K.at(1,1); - cx = K.at(0,2); - cy = K.at(1,2); - invfx = 1.0f/fx; - invfy = 1.0f/fy; + fx = K.at(0,0); + fy = K.at(1,1); + cx = K.at(0,2); + cy = K.at(1,2); + invfx = 1.0f/fx; + invfy = 1.0f/fy; - mbInitialComputations=false; - } + mbInitialComputations=false; + } - mb = mbf/fx; + mb = mbf/fx; - AssignFeaturesToGrid(); -} + AssignFeaturesToGrid(); + } -Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth) - :mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast(NULL)), - mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) -{ + Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth) + :mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast(NULL)), + mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) + { // Frame ID - mnId=nNextId++; + mnId=nNextId++; // Scale Level Info - mnScaleLevels = mpORBextractorLeft->GetLevels(); - mfScaleFactor = mpORBextractorLeft->GetScaleFactor(); - mfLogScaleFactor = log(mfScaleFactor); - mvScaleFactors = mpORBextractorLeft->GetScaleFactors(); - mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors(); - mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares(); - mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares(); + mnScaleLevels = mpORBextractorLeft->GetLevels(); + mfScaleFactor = mpORBextractorLeft->GetScaleFactor(); + mfLogScaleFactor = log(mfScaleFactor); + mvScaleFactors = mpORBextractorLeft->GetScaleFactors(); + mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors(); + mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares(); + mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares(); // ORB extraction - ExtractORB(0,imGray); + ExtractORB(0,imGray); - N = mvKeys.size(); + N = mvKeys.size(); - if(mvKeys.empty()) - return; + if(mvKeys.empty()) + return; - UndistortKeyPoints(); + UndistortKeyPoints(); // Set no stereo information - mvuRight = vector(N,-1); - mvDepth = vector(N,-1); + mvuRight = vector(N,-1); + mvDepth = vector(N,-1); - mvpMapPoints = vector(N,static_cast(NULL)); - mvbOutlier = vector(N,false); + mvpMapPoints = vector(N,static_cast(NULL)); + mvbOutlier = vector(N,false); // This is done only for the first Frame (or after a change in the calibration) - if(mbInitialComputations) - { - ComputeImageBounds(imGray); + if(mbInitialComputations) + { + ComputeImageBounds(imGray); - mfGridElementWidthInv=static_cast(FRAME_GRID_COLS)/static_cast(mnMaxX-mnMinX); - mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/static_cast(mnMaxY-mnMinY); + mfGridElementWidthInv=static_cast(FRAME_GRID_COLS)/static_cast(mnMaxX-mnMinX); + mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/static_cast(mnMaxY-mnMinY); - fx = K.at(0,0); - fy = K.at(1,1); - cx = K.at(0,2); - cy = K.at(1,2); - invfx = 1.0f/fx; - invfy = 1.0f/fy; + fx = K.at(0,0); + fy = K.at(1,1); + cx = K.at(0,2); + cy = K.at(1,2); + invfx = 1.0f/fx; + invfy = 1.0f/fy; - mbInitialComputations=false; - } + mbInitialComputations=false; + } - mb = mbf/fx; + mb = mbf/fx; - AssignFeaturesToGrid(); -} + AssignFeaturesToGrid(); + } -void Frame::AssignFeaturesToGrid() -{ - int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS); - for(unsigned int i=0; imbTrackInView = false; + void Frame::SetPose(cv::Mat Tcw) + { + mTcw = Tcw.clone(); + UpdatePoseMatrices(); + } + + void Frame::UpdatePoseMatrices() + { + mRcw = mTcw.rowRange(0,3).colRange(0,3); + mRwc = mRcw.t(); + mtcw = mTcw.rowRange(0,3).col(3); + mOw = -mRcw.t()*mtcw; + } + + bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit) + { + pMP->mbTrackInView = false; // 3D in absolute coordinates - cv::Mat P = pMP->GetWorldPos(); + cv::Mat P = pMP->GetWorldPos(); // 3D in camera coordinates - const cv::Mat Pc = mRcw*P+mtcw; - const float &PcX = Pc.at(0); - const float &PcY= Pc.at(1); - const float &PcZ = Pc.at(2); + const cv::Mat Pc = mRcw*P+mtcw; + const float &PcX = Pc.at(0); + const float &PcY= Pc.at(1); + const float &PcZ = Pc.at(2); // Check positive depth - if(PcZ<0.0f) - return false; + if(PcZ<0.0f) + return false; // Project in image and check it is not outside - const float invz = 1.0f/PcZ; - const float u=fx*PcX*invz+cx; - const float v=fy*PcY*invz+cy; + const float invz = 1.0f/PcZ; + const float u=fx*PcX*invz+cx; + const float v=fy*PcY*invz+cy; - if(umnMaxX) - return false; - if(vmnMaxY) - return false; + if(umnMaxX) + return false; + if(vmnMaxY) + return false; // Check distance is in the scale invariance region of the MapPoint - const float maxDistance = pMP->GetMaxDistanceInvariance(); - const float minDistance = pMP->GetMinDistanceInvariance(); - const cv::Mat PO = P-mOw; - const float dist = cv::norm(PO); + const float maxDistance = pMP->GetMaxDistanceInvariance(); + const float minDistance = pMP->GetMinDistanceInvariance(); + const cv::Mat PO = P-mOw; + const float dist = cv::norm(PO); - if(distmaxDistance) - return false; + if(distmaxDistance) + return false; // Check viewing angle - cv::Mat Pn = pMP->GetNormal(); + cv::Mat Pn = pMP->GetNormal(); - const float viewCos = PO.dot(Pn)/dist; + const float viewCos = PO.dot(Pn)/dist; - if(viewCosPredictScale(dist,this); + const int nPredictedLevel = pMP->PredictScale(dist,this); // Data used by the tracking - pMP->mbTrackInView = true; - pMP->mTrackProjX = u; - pMP->mTrackProjXR = u - mbf*invz; - pMP->mTrackProjY = v; - pMP->mnTrackScaleLevel= nPredictedLevel; - pMP->mTrackViewCos = viewCos; - - return true; -} + pMP->mbTrackInView = true; + pMP->mTrackProjX = u; + pMP->mTrackProjXR = u - mbf*invz; + pMP->mTrackProjY = v; + pMP->mnTrackScaleLevel= nPredictedLevel; + pMP->mTrackViewCos = viewCos; + + return true; + } -vector Frame::GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel, const int maxLevel) const -{ - vector vIndices; - vIndices.reserve(N); + vector Frame::GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel, const int maxLevel) const + { + vector vIndices; + vIndices.reserve(N); - const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv)); - if(nMinCellX>=FRAME_GRID_COLS) - return vIndices; + const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv)); + if(nMinCellX>=FRAME_GRID_COLS) + return vIndices; - const int nMaxCellX = min((int)FRAME_GRID_COLS-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv)); - if(nMaxCellX<0) - return vIndices; + const int nMaxCellX = min((int)FRAME_GRID_COLS-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv)); + if(nMaxCellX<0) + return vIndices; - const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv)); - if(nMinCellY>=FRAME_GRID_ROWS) - return vIndices; + const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv)); + if(nMinCellY>=FRAME_GRID_ROWS) + return vIndices; - const int nMaxCellY = min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv)); - if(nMaxCellY<0) - return vIndices; + const int nMaxCellY = min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv)); + if(nMaxCellY<0) + return vIndices; - const bool bCheckLevels = (minLevel>0) || (maxLevel>=0); + const bool bCheckLevels = (minLevel>0) || (maxLevel>=0); - for(int ix = nMinCellX; ix<=nMaxCellX; ix++) - { - for(int iy = nMinCellY; iy<=nMaxCellY; iy++) - { - const vector vCell = mGrid[ix][iy]; - if(vCell.empty()) - continue; - - for(size_t j=0, jend=vCell.size(); j=0) - if(kpUn.octave>maxLevel) + for(int iy = nMinCellY; iy<=nMaxCellY; iy++) + { + const vector vCell = mGrid[ix][iy]; + if(vCell.empty()) continue; - } - - const float distx = kpUn.pt.x-x; - const float disty = kpUn.pt.y-y; - - if(fabs(distx)=0) + if(kpUn.octave>maxLevel) + continue; + } + + const float distx = kpUn.pt.x-x; + const float disty = kpUn.pt.y-y; + + if(fabs(distx)=FRAME_GRID_COLS || posY<0 || posY>=FRAME_GRID_ROWS) - return false; + if(posX<0 || posX>=FRAME_GRID_COLS || posY<0 || posY>=FRAME_GRID_ROWS) + return false; - return true; -} + return true; + } -void Frame::ComputeBoW() -{ - if(mBowVec.empty()) - { - vector vCurrentDesc = Converter::toDescriptorVector(mDescriptors); - mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4); - } -} + void Frame::ComputeBoW() + { + if(mBowVec.empty()) + { + vector vCurrentDesc = Converter::toDescriptorVector(mDescriptors); + mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4); + } + } -void Frame::UndistortKeyPoints() -{ - if(mDistCoef.at(0)==0.0) - { - mvKeysUn=mvKeys; - return; - } + void Frame::UndistortKeyPoints() + { + if(mDistCoef.at(0)==0.0) + { + mvKeysUn=mvKeys; + return; + } // Fill matrix with points - cv::Mat mat(N,2,CV_32F); - for(int i=0; i(i,0)=mvKeys[i].pt.x; - mat.at(i,1)=mvKeys[i].pt.y; - } + cv::Mat mat(N,2,CV_32F); + for(int i=0; i(i,0)=mvKeys[i].pt.x; + mat.at(i,1)=mvKeys[i].pt.y; + } // Undistort points - mat=mat.reshape(2); - cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK); - mat=mat.reshape(1); + mat=mat.reshape(2); + cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK); + mat=mat.reshape(1); // Fill undistorted keypoint vector - mvKeysUn.resize(N); - for(int i=0; i(i,0); - kp.pt.y=mat.at(i,1); - mvKeysUn[i]=kp; - } -} - -void Frame::ComputeImageBounds(const cv::Mat &imLeft) -{ - if(mDistCoef.at(0)!=0.0) - { - cv::Mat mat(4,2,CV_32F); - mat.at(0,0)=0.0; mat.at(0,1)=0.0; - mat.at(1,0)=imLeft.cols; mat.at(1,1)=0.0; - mat.at(2,0)=0.0; mat.at(2,1)=imLeft.rows; - mat.at(3,0)=imLeft.cols; mat.at(3,1)=imLeft.rows; + mvKeysUn.resize(N); + for(int i=0; i(i,0); + kp.pt.y=mat.at(i,1); + mvKeysUn[i]=kp; + } + } - // Undistort corners - mat=mat.reshape(2); - cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK); - mat=mat.reshape(1); + void Frame::ComputeImageBounds(const cv::Mat &imLeft) + { + if(mDistCoef.at(0)!=0.0) + { + cv::Mat mat(4,2,CV_32F); + mat.at(0,0)=0.0; mat.at(0,1)=0.0; + mat.at(1,0)=imLeft.cols; mat.at(1,1)=0.0; + mat.at(2,0)=0.0; mat.at(2,1)=imLeft.rows; + mat.at(3,0)=imLeft.cols; mat.at(3,1)=imLeft.rows; - mnMinX = min(mat.at(0,0),mat.at(2,0)); - mnMaxX = max(mat.at(1,0),mat.at(3,0)); - mnMinY = min(mat.at(0,1),mat.at(1,1)); - mnMaxY = max(mat.at(2,1),mat.at(3,1)); + // Undistort corners + mat=mat.reshape(2); + cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK); + mat=mat.reshape(1); + + mnMinX = min(mat.at(0,0),mat.at(2,0)); + mnMaxX = max(mat.at(1,0),mat.at(3,0)); + mnMinY = min(mat.at(0,1),mat.at(1,1)); + mnMaxY = max(mat.at(2,1),mat.at(3,1)); + + } + else + { + mnMinX = 0.0f; + mnMaxX = imLeft.cols; + mnMinY = 0.0f; + mnMaxY = imLeft.rows; + } + } - } - else - { - mnMinX = 0.0f; - mnMaxX = imLeft.cols; - mnMinY = 0.0f; - mnMaxY = imLeft.rows; - } -} - -void Frame::ComputeStereoMatches() -{ - mvuRight = vector(N,-1.0f); - mvDepth = vector(N,-1.0f); + void Frame::ComputeStereoMatches() + { + mvuRight = vector(N,-1.0f); + mvDepth = vector(N,-1.0f); - const int thOrbDist = (ORBmatcher::TH_HIGH+ORBmatcher::TH_LOW)/2; + const int thOrbDist = (ORBmatcher::TH_HIGH+ORBmatcher::TH_LOW)/2; - const int nRows = mpORBextractorLeft->mvImagePyramid[0].rows; + const int nRows = mpORBextractorLeft->mvImagePyramid[0].rows; //Assign keypoints to row table - vector > vRowIndices(nRows,vector()); + vector > vRowIndices(nRows,vector()); - for(int i=0; i > vDistIdx; - vDistIdx.reserve(N); + vector > vDistIdx; + vDistIdx.reserve(N); - for(int iL=0; iL &vCandidates = vRowIndices[vL]; + const vector &vCandidates = vRowIndices[vL]; - if(vCandidates.empty()) - continue; + if(vCandidates.empty()) + continue; - const float minU = uL-maxD; - const float maxU = uL-minD; + const float minU = uL-maxD; + const float maxU = uL-minD; - if(maxU<0) - continue; + if(maxU<0) + continue; - int bestDist = ORBmatcher::TH_HIGH; - size_t bestIdxR = 0; + int bestDist = ORBmatcher::TH_HIGH; + size_t bestIdxR = 0; - const cv::Mat &dL = mDescriptors.row(iL); + const cv::Mat &dL = mDescriptors.row(iL); // Compare descriptor to right keypoints - for(size_t iC=0; iClevelL+1) - continue; + if(kpR.octavelevelL+1) + continue; - const float &uR = kpR.pt.x; + const float &uR = kpR.pt.x; - if(uR>=minU && uR<=maxU) - { - const cv::Mat &dR = mDescriptorsRight.row(iR); - const int dist = ORBmatcher::DescriptorDistance(dL,dR); + if(uR>=minU && uR<=maxU) + { + const cv::Mat &dR = mDescriptorsRight.row(iR); + const int dist = ORBmatcher::DescriptorDistance(dL,dR); - if(distmvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduL-w,scaleduL+w+1); - IL.convertTo(IL,CV_32F); - IL = IL - IL.at(w,w) *cv::Mat::ones(IL.rows,IL.cols,CV_32F); - - int bestDist = INT_MAX; - int bestincR = 0; - const int L = 5; - vector vDists; - vDists.resize(2*L+1); - - const float iniu = scaleduR0+L-w; - const float endu = scaleduR0+L+w+1; - if(iniu<0 || endu >= mpORBextractorRight->mvImagePyramid[kpL.octave].cols) - continue; - - for(int incR=-L; incR<=+L; incR++) - { - cv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduR0+incR-w,scaleduR0+incR+w+1); - IR.convertTo(IR,CV_32F); - IR = IR - IR.at(w,w) *cv::Mat::ones(IR.rows,IR.cols,CV_32F); - - float dist = cv::norm(IL,IR,cv::NORM_L1); - if(distmvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduL-w,scaleduL+w+1); + IL.convertTo(IL,CV_32F); + IL = IL - IL.at(w,w) *cv::Mat::ones(IL.rows,IL.cols,CV_32F); + + int bestDist = INT_MAX; + int bestincR = 0; + const int L = 5; + vector vDists; + vDists.resize(2*L+1); + + const float iniu = scaleduR0+L-w; + const float endu = scaleduR0+L+w+1; + if(iniu<0 || endu >= mpORBextractorRight->mvImagePyramid[kpL.octave].cols) + continue; + + for(int incR=-L; incR<=+L; incR++) + { + cv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduR0+incR-w,scaleduR0+incR+w+1); + IR.convertTo(IR,CV_32F); + IR = IR - IR.at(w,w) *cv::Mat::ones(IR.rows,IR.cols,CV_32F); + + float dist = cv::norm(IL,IR,cv::NORM_L1); + if(dist1) - continue; + if(deltaR<-1 || deltaR>1) + continue; // Re-scaled coordinate - float bestuR = mvScaleFactors[kpL.octave]*((float)scaleduR0+(float)bestincR+deltaR); - - float disparity = (uL-bestuR); - - if(disparity>=minD && disparity=minD && disparity(bestDist,iL)); + } + } + } + + sort(vDistIdx.begin(),vDistIdx.end()); + const float median = vDistIdx[vDistIdx.size()/2].first; + const float thDist = 1.5f*1.4f*median; + + for(int i=vDistIdx.size()-1;i>=0;i--) + { + if(vDistIdx[i].first(bestDist,iL)); - } - } - } - - sort(vDistIdx.begin(),vDistIdx.end()); - const float median = vDistIdx[vDistIdx.size()/2].first; - const float thDist = 1.5f*1.4f*median; - for(int i=vDistIdx.size()-1;i>=0;i--) - { - if(vDistIdx[i].first(N,-1); + mvDepth = vector(N,-1); -void Frame::ComputeStereoFromRGBD(const cv::Mat &imDepth) -{ - mvuRight = vector(N,-1); - mvDepth = vector(N,-1); + for(int i=0; i(v,u); - const float d = imDepth.at(v,u); + if(d>0) + { + mvDepth[i] = d; + mvuRight[i] = kpU.pt.x-mbf/d; + } + } + } - if(d>0) - { - mvDepth[i] = d; - mvuRight[i] = kpU.pt.x-mbf/d; - } - } -} + cv::Mat Frame::UnprojectStereo(const int &i) + { + const float z = mvDepth[i]; + if(z>0) + { + const float u = mvKeysUn[i].pt.x; + const float v = mvKeysUn[i].pt.y; + const float x = (u-cx)*z*invfx; + const float y = (v-cy)*z*invfy; + cv::Mat x3Dc = (cv::Mat_(3,1) << x, y, z); + return mRwc*x3Dc+mOw; + } + else + return cv::Mat(); + } -cv::Mat Frame::UnprojectStereo(const int &i) -{ - const float z = mvDepth[i]; - if(z>0) - { - const float u = mvKeysUn[i].pt.x; - const float v = mvKeysUn[i].pt.y; - const float x = (u-cx)*z*invfx; - const float y = (v-cy)*z*invfy; - cv::Mat x3Dc = (cv::Mat_(3,1) << x, y, z); - return mRwc*x3Dc+mOw; - } - else - return cv::Mat(); -} + bool Frame::getSceneDepth(Frame& frame, double& depth_max, double& depth_min) + { + vector depth_vec; + depth_vec.reserve(frame.mvpMapPoints.size()); + + depth_min = std::numeric_limits::max(); + for(auto it=frame.mvpMapPoints.begin(), ite=frame.mvpMapPoints.end(); it!=ite; ++it) + { + if((*it)!= NULL) + { + if((*it)->isBad()) + continue; + cv::Mat mpPosHg = cv::Mat::ones(4,1,CV_32F); + cv::Mat mpPos = (*it)->GetWorldPos(); + mpPosHg.at(0) = mpPos.at(0); + mpPosHg.at(1) = mpPos.at(1); + mpPosHg.at(2) = mpPos.at(2); + cv::Mat v3Temp = mpPosHg; + //std::cout << "Mat: " << mpPos.at(0) << " " << mpPos.at(1) << " " << mpPos.at(2) << std::endl; + const double z = v3Temp.at(2); + if (z > 0) + { + depth_vec.push_back(z); + //std::cout << "Mat: " << mpPos << std::endl; + // std::cout << "at: " << v3Temp.at(2) << std::endl; + //std::cout << "Z: " << z << std::endl; + depth_min = fmin(z, depth_min); + //depth_min = fmax(0, depth_min); + depth_max = fmax(z, depth_max); + } + } + + if(depth_vec.empty()) + { + return false; + } + // std::nth_element(depth_vec.begin(), depth_vec.begin() + depth_vec.size()/2, depth_vec.end()); + // depth_max = depth_vec[depth_vec.size()/2]; + } + return true; + } } //namespace ORB_SLAM