├── .gitattributes ├── src ├── cMap.cpp ├── cSystem.cpp ├── cViewer.cpp ├── cConverter.cpp ├── cMapPoint.cpp ├── cORBmatcher.cpp ├── cTracking.cpp ├── cLoopClosing.cpp ├── cMapPublisher.cpp ├── mdBRIEFextractorOct.cpp ├── cMultiKeyFrameDatabase.cpp ├── camDriver.cpp ├── camSubscriber.cpp ├── misc.cpp ├── g2o_MultiCol_sim3_expmap.cpp ├── imuDriver.cpp ├── cam_model_omni.cpp ├── cMultiFramePublisher.cpp ├── cam_system_omni.cpp ├── cMultiInitializer.cpp ├── cMultiFrame.cpp ├── cSim3Solver.cpp └── cOptimizerLoopStuff.cpp ├── include ├── cMapPoint.h ├── cSystem.h ├── cViewer.h ├── cORBmatcher.h ├── cSim3Solver.h ├── cLocalMapping.h ├── cLoopClosing.h ├── cMapPublisher.h ├── cORBVocabulary.h ├── cMultiInitializer.h ├── mdBRIEFextractorOct.h ├── cMultiFramePublisher.h ├── cMultiKeyFrameDatabase.h ├── cMap.h ├── cORBextractor.h ├── cOptimizer.h ├── cConverter.h ├── cMultiFrame.h ├── g2o_MultiCol_vertices_edges.h ├── misc.h ├── cam_system_omni.h ├── cTracking.h ├── cam_model_omni.h ├── g2o_MultiCol_sim3_expmap.h ├── cMultiKeyFrame.h └── mdBRIEFextractor.h ├── resources ├── MultiCol.png ├── singleCamera.png ├── BodyFrameConcept.png └── MultiCol_as_hypergraph.png ├── Examples ├── Lafida │ ├── multi_col_slam_lafida │ ├── mult_col_slam_lafida.cpp │ ├── InteriorOrientationFisheye1.yaml │ ├── InteriorOrientationFisheye2.yaml │ ├── MultiCamSys_Calibration.yaml │ ├── InteriorOrientationFisheye0.yaml │ ├── Slam_Settings_indoor3.yaml │ ├── Slam_Settings_indoor4.yaml │ ├── Slam_Settings_indoor2.yaml │ └── Slam_Settings_indoor1.yaml └── ROS │ └── MultiCol-SLAM │ ├── manifest.xml │ ├── include │ └── camNodelet.h │ ├── CMakeLists.txt │ └── src │ ├── ros_multi_backup.cc │ └── ros_multi.cc ├── README.md └── CMakeLists.txt /.gitattributes: -------------------------------------------------------------------------------- 1 | *.C++ linguist-detectable=true 2 | *.CMake linguist-detectable=false 3 | -------------------------------------------------------------------------------- /src/cMap.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/src/cMap.cpp -------------------------------------------------------------------------------- /src/cSystem.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/src/cSystem.cpp -------------------------------------------------------------------------------- /src/cViewer.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/src/cViewer.cpp -------------------------------------------------------------------------------- /include/cMapPoint.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/include/cMapPoint.h -------------------------------------------------------------------------------- /include/cSystem.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/include/cSystem.h -------------------------------------------------------------------------------- /include/cViewer.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/include/cViewer.h -------------------------------------------------------------------------------- /src/cConverter.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/src/cConverter.cpp -------------------------------------------------------------------------------- /src/cMapPoint.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/src/cMapPoint.cpp -------------------------------------------------------------------------------- /src/cORBmatcher.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/src/cORBmatcher.cpp -------------------------------------------------------------------------------- /src/cTracking.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/src/cTracking.cpp -------------------------------------------------------------------------------- /include/cORBmatcher.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/include/cORBmatcher.h -------------------------------------------------------------------------------- /include/cSim3Solver.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/include/cSim3Solver.h -------------------------------------------------------------------------------- /src/cLoopClosing.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/src/cLoopClosing.cpp -------------------------------------------------------------------------------- /src/cMapPublisher.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/src/cMapPublisher.cpp -------------------------------------------------------------------------------- /include/cLocalMapping.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/include/cLocalMapping.h -------------------------------------------------------------------------------- /include/cLoopClosing.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/include/cLoopClosing.h -------------------------------------------------------------------------------- /include/cMapPublisher.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/include/cMapPublisher.h -------------------------------------------------------------------------------- /include/cORBVocabulary.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/include/cORBVocabulary.h -------------------------------------------------------------------------------- /resources/MultiCol.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/resources/MultiCol.png -------------------------------------------------------------------------------- /resources/singleCamera.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/resources/singleCamera.png -------------------------------------------------------------------------------- /include/cMultiInitializer.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/include/cMultiInitializer.h -------------------------------------------------------------------------------- /include/mdBRIEFextractorOct.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/include/mdBRIEFextractorOct.h -------------------------------------------------------------------------------- /src/mdBRIEFextractorOct.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/src/mdBRIEFextractorOct.cpp -------------------------------------------------------------------------------- /include/cMultiFramePublisher.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/include/cMultiFramePublisher.h -------------------------------------------------------------------------------- /resources/BodyFrameConcept.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/resources/BodyFrameConcept.png -------------------------------------------------------------------------------- /src/cMultiKeyFrameDatabase.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/src/cMultiKeyFrameDatabase.cpp -------------------------------------------------------------------------------- /include/cMultiKeyFrameDatabase.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/include/cMultiKeyFrameDatabase.h -------------------------------------------------------------------------------- /resources/MultiCol_as_hypergraph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/resources/MultiCol_as_hypergraph.png -------------------------------------------------------------------------------- /Examples/Lafida/multi_col_slam_lafida: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/Examples/Lafida/multi_col_slam_lafida -------------------------------------------------------------------------------- /Examples/Lafida/mult_col_slam_lafida.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Varun-Haris/Multi-Camera-SLAM/HEAD/Examples/Lafida/mult_col_slam_lafida.cpp -------------------------------------------------------------------------------- /Examples/ROS/MultiCol-SLAM/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | MultiCol 5 | 6 | 7 | Abhihsek Ravi 8 | GPLv3 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /Examples/Lafida/InteriorOrientationFisheye1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # Camera Parameters. Adjust them! 4 | # Camera calibration parameters cam right 5 | Camera.Iw: 754 6 | Camera.Ih: 480 7 | 8 | Camera.nrpol: 5 9 | Camera.nrinvpol: 12 10 | 11 | Camera.a0:-209.071570423477 12 | Camera.a1: 0.0 13 | Camera.a2: 0.00216428966944475 14 | Camera.a3: -4.82076123755912e-06 15 | Camera.a4: 1.91902225328321e-08 16 | 17 | Camera.pol0: 294.569975726124 18 | Camera.pol1: 148.608451179358 19 | Camera.pol2: -12.8584962393538 20 | Camera.pol3: 28.8889327437629 21 | Camera.pol4: 6.33883105861485 22 | Camera.pol5:-0.89043629163529 23 | Camera.pol6: 12.3786448516946 24 | Camera.pol7: 0.165754338732521 25 | Camera.pol8: -7.71927140996498 26 | Camera.pol9: 0.995360127219927 27 | Camera.pol10: 3.74959014924028 28 | Camera.pol11: 1.01240838118284 29 | 30 | Camera.c: 1.00025900216253 31 | Camera.d: -0.0123492665428806 32 | Camera.e: 0.012594227848688 33 | 34 | Camera.u0: 378.722262294544 35 | Camera.v0: 253.62515782861 36 | 37 | # create a mirror mask for fisheye cameras 38 | Camera.mirrorMask: 1 -------------------------------------------------------------------------------- /Examples/Lafida/InteriorOrientationFisheye2.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # Camera Parameters. Adjust them! 4 | # Camera calibration parameters cam left 5 | Camera.Iw: 754 6 | Camera.Ih: 480 7 | 8 | Camera.nrpol: 5 9 | Camera.nrinvpol: 12 10 | 11 | Camera.a0: -208.409533164304 12 | Camera.a1: 0.0 13 | Camera.a2: 0.00225243935068085 14 | Camera.a3: -5.55430532694086e-06 15 | Camera.a4: 2.07875001352451e-08 16 | 17 | Camera.pol0: 293.775776115273 18 | Camera.pol1: 147.35852428463 19 | Camera.pol2: -14.3964307215767 20 | Camera.pol3: 29.078338589251 21 | Camera.pol4: 6.03461468718202 22 | Camera.pol5:-2.07410652721192 23 | Camera.pol6: 13.4359111214281 24 | Camera.pol7: 0.984397095348803 25 | Camera.pol8: -8.64924421339311 26 | Camera.pol9: 0.491570292475276 27 | Camera.pol10: 3.92296807229949 28 | Camera.pol11: 1.10563269391099 29 | 30 | Camera.c: 0.99992772614797 31 | Camera.d: 0.0334716840387143 32 | Camera.e: -0.0332451389561491 33 | 34 | Camera.u0: 372.796912405235 35 | Camera.v0: 225.645071214684 36 | 37 | # create a mirror mask for fisheye cameras 38 | Camera.mirrorMask: 1 39 | -------------------------------------------------------------------------------- /Examples/Lafida/MultiCamSys_Calibration.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # Multi-camera calibration matrices (M_c) 4 | # rotation elements are in cayley parametrization 5 | CameraSystem.nrCams: 3 6 | # camera back 7 | CameraSystem.cam1_1: -0.0238361786473007 #r1 8 | CameraSystem.cam1_2: -2.05998171167958 #r2 9 | CameraSystem.cam1_3: 0.695126790868671 #r3 10 | CameraSystem.cam1_4: -0.140202124607334 #t1 11 | CameraSystem.cam1_5: 0.0219677971160655 #t2 12 | CameraSystem.cam1_6: -0.0226322662838432 #t3 13 | # camera right 14 | CameraSystem.cam2_1: -0.0103943566650926 #r1 15 | CameraSystem.cam2_2: 1.12505249943085 #r2 16 | CameraSystem.cam2_3: -0.402901183146028 #r3 17 | CameraSystem.cam2_4: 0.108753418890423 #t1 18 | CameraSystem.cam2_5: 0.0636197216520153 #t2 19 | CameraSystem.cam2_6: 0.0657911760105832 #t3 20 | # camera left 21 | CameraSystem.cam3_1: 0 #r1 22 | CameraSystem.cam3_2: 0 #r2 23 | CameraSystem.cam3_3: 0 #r3 24 | CameraSystem.cam3_4: -0.00157612288268783 #t1 25 | CameraSystem.cam3_5: 0.103615531247527 #t2 26 | CameraSystem.cam3_6: 0.201416323496156 #t3 27 | 28 | 29 | -------------------------------------------------------------------------------- /Examples/Lafida/InteriorOrientationFisheye0.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # Camera Parameters. Adjust them! 4 | # Camera calibration parameters camera back 5 | Camera.Iw: 754 6 | Camera.Ih: 480 7 | 8 | # hyperparameters 9 | Camera.nrpol: 5 10 | Camera.nrinvpol: 12 11 | 12 | # forward polynomial f(\rho) 13 | Camera.a0: -209.200757992065 14 | Camera.a1: 0.0 15 | Camera.a2: 0.00213741670953883 16 | Camera.a3: -4.2203617319086e-06 17 | Camera.a4: 1.77146086919594e-08 18 | 19 | # backward polynomial rho(\theta) 20 | Camera.pol0: 293.667187375663 21 | Camera.pol1: 149.982043337335 22 | Camera.pol2: -10.448650568161 23 | Camera.pol3: 28.2295300683376 24 | Camera.pol4: 7.13365723186292 25 | Camera.pol5: 0.056303218962532 26 | Camera.pol6: 10.4144677485333 27 | Camera.pol7: 0.166354960773665 28 | Camera.pol8: -5.86858687381081 29 | Camera.pol9: 1.18165998645705 30 | Camera.pol10: 3.1108311354746 31 | Camera.pol11: 0.810799620714366 32 | 33 | # affine matrix 34 | Camera.c: 0.999626131079017 35 | Camera.d: -0.0034775192597376 36 | Camera.e: 0.00385134991673147 37 | 38 | # principal point 39 | Camera.u0: 392.219508388648 40 | Camera.v0: 243.494438476351 41 | 42 | # create a mirror mask for fisheye cameras 43 | Camera.mirrorMask: 1 44 | 45 | 46 | -------------------------------------------------------------------------------- /Examples/ROS/MultiCol-SLAM/include/camNodelet.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMNODELET_H 2 | #define CAMNODELET_H 3 | 4 | // Standard c++ headers 5 | #include 6 | #include 7 | #include 8 | 9 | // ROS related headers 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | // OpenCV headers 18 | #include 19 | #include 20 | #include "opencv2/features2d.hpp" 21 | #include "opencv2/xfeatures2d.hpp" 22 | #include "opencv2/core/core.hpp" 23 | #include "opencv2/imgproc/imgproc.hpp" 24 | 25 | using namespace std; 26 | using namespace cv; 27 | 28 | class camSubs{ 29 | public: 30 | ros::NodeHandle nh; 31 | image_transport::Subscriber sub, sub1, sub2; 32 | image_transport::ImageTransport it; 33 | cv::Mat frame1, frame2, frame3; 34 | std_msgs::Header header1, header2, header3; 35 | 36 | camSubs(); 37 | void imageCbWeb(const sensor_msgs::ImageConstPtr& msg); 38 | void imageCbUsb1(const sensor_msgs::ImageConstPtr& msg); 39 | void imageCbUsb2(const sensor_msgs::ImageConstPtr& msg); 40 | }; //End of class 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /src/camDriver.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace cv; 7 | 8 | class driver{ 9 | public: 10 | ros::NodeHandle nh; 11 | image_transport::Publisher pub, pub1, pub2; 12 | 13 | driver(){ 14 | image_transport::ImageTransport it(nh); 15 | this->pub = it.advertise("camera/image/webcam", 1); 16 | this->pub1 = it.advertise("camera/image/usb1", 1); 17 | this->pub2 = it.advertise("camera/image/usb2", 1); 18 | } 19 | 20 | void publishMsg(sensor_msgs::ImagePtr msg, sensor_msgs::ImagePtr msg1, sensor_msgs::ImagePtr msg2){ 21 | this->pub.publish(msg); 22 | this->pub1.publish(msg1); 23 | this->pub2.publish(msg2); 24 | } 25 | }; 26 | 27 | int main(int argc, char** argv){ 28 | ros::init(argc, argv, "Driver"); //Driver node 29 | ros::NodeHandle n; 30 | uint32_t i=0; 31 | VideoCapture cap, cam1, cam2; 32 | 33 | cap.open("/dev/video0"); 34 | cam1.open("/dev/video1"); 35 | cam2.open("/dev/video2"); 36 | if(!cap.isOpened() || !cam1.isOpened() || !cam2.isOpened()){ 37 | return -1; 38 | } 39 | 40 | driver *camDrive = new driver; 41 | Mat frame, frame1, frame2; 42 | while(ros::ok()){ 43 | cap >> frame; 44 | cam1 >> frame1; 45 | cam2 >> frame2; 46 | 47 | sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg(); 48 | msg->header.stamp = ros::Time::now(); //Adding timestamp 49 | msg->header.seq = i; //Adding sequence number 50 | msg->header.frame_id = "Webcam integrated"; 51 | 52 | sensor_msgs::ImagePtr msg1 = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame1).toImageMsg(); 53 | msg1->header.stamp = ros::Time::now(); //Adding timestamp 54 | msg1->header.seq = i; //Adding sequence number 55 | msg1->header.frame_id = "USB cam 1"; 56 | 57 | sensor_msgs::ImagePtr msg2 = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame2).toImageMsg(); 58 | msg2->header.stamp = ros::Time::now(); //Adding timestamp 59 | msg2->header.seq = i; //Adding sequence number 60 | msg2->header.frame_id = "USB cam 2"; 61 | 62 | camDrive->publishMsg(msg, msg1, msg2); 63 | i++; 64 | } 65 | return 0; 66 | } 67 | -------------------------------------------------------------------------------- /Examples/Lafida/Slam_Settings_indoor3.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | # Camera frames per second 3 | Camera.fps: 25.0 4 | 5 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 6 | Camera.RGB: 1 7 | 8 | #-------------------------------------------------------------------------------------------- 9 | ### Changing the parameters below could seriously degradate the performance of the system 10 | 11 | # 0-> ORB, 1->dBRIEF 12 | extractor.usemdBRIEF: 0 13 | 14 | # 1-> learn masks, 0 don't (mdBRIEF) 15 | extractor.masks: 0 16 | 17 | # 1-> use Agast, 0 don't 18 | extractor.useAgast: 0 19 | 20 | # 0,1,2,(3 is only possible for agast) 21 | extractor.fastAgastType: 2 22 | 23 | # Extractor: 32 -> ORB , (16/32/64) -> dBRIEF and mdBRIEF 24 | extractor.descSize: 32 25 | 26 | # Extractor: Number of features per image 27 | extractor.nFeatures: 400 28 | 29 | # Extractor: Scale factor between levels in the scale pyramid 30 | extractor.scaleFactor: 1.2 31 | 32 | # Extractor: Number of levels in the scale pyramid 33 | extractor.nLevels: 8 34 | 35 | # Extractor: FAST threshold (lower less restrictive) 36 | extractor.fastTh: 20 37 | 38 | # Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score 39 | extractor.nScoreType: 0 40 | 41 | # Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended]) 42 | UseMotionModel: 1 43 | 44 | 45 | 46 | #-------------------------------------------------------------------------------------------- 47 | # current trajectory to get evaluated 48 | traj2Eval: 3 49 | # run x (we need multiple runs to evaluate the accuracy because SLAM is not deterministic) 50 | trajrun: 1 51 | 52 | #------------------------------ 53 | ### initial frame Traj3 54 | traj.StartFrame: 1 55 | traj.EndFrame: 1015 56 | #------------------------------ 57 | 58 | #-------------------------------------------------------------------------------------------- 59 | # Viewer settings 60 | Viewer.MultiKeyFrameSize: 0.05 61 | Viewer.MultiKeyFrameLineWidth: 1 62 | Viewer.GraphLineWidth: 0.9 63 | Viewer.PointSize: 2 64 | Viewer.CameraSize: 0.08 65 | Viewer.CameraLineWidth: 3 66 | Viewer.ViewpointX: 1.0 67 | Viewer.ViewpointY: 1.0 68 | Viewer.ViewpointZ: 1.0 69 | Viewer.ViewpointF: 500.0 70 | Viewer.DrawNrCams: 2 -------------------------------------------------------------------------------- /Examples/Lafida/Slam_Settings_indoor4.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | # Camera frames per second 3 | Camera.fps: 25.0 4 | 5 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 6 | Camera.RGB: 1 7 | 8 | #-------------------------------------------------------------------------------------------- 9 | ### Changing the parameters below could seriously degradate the performance of the system 10 | 11 | # 0-> ORB, 1->dBRIEF 12 | extractor.usemdBRIEF: 0 13 | 14 | # 1-> learn masks, 0 don't (mdBRIEF) 15 | extractor.masks: 0 16 | 17 | # 1-> use Agast, 0 don't 18 | extractor.useAgast: 0 19 | 20 | # 0,1,2,(3 is only possible for agast) 21 | extractor.fastAgastType: 2 22 | 23 | # Extractor: 32 -> ORB , (16/32/64) -> dBRIEF and mdBRIEF 24 | extractor.descSize: 32 25 | 26 | # Extractor: Number of features per image 27 | extractor.nFeatures: 400 28 | 29 | # Extractor: Scale factor between levels in the scale pyramid 30 | extractor.scaleFactor: 1.2 31 | 32 | # Extractor: Number of levels in the scale pyramid 33 | extractor.nLevels: 8 34 | 35 | # Extractor: FAST threshold (lower less restrictive) 36 | extractor.fastTh: 20 37 | 38 | # Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score 39 | extractor.nScoreType: 0 40 | 41 | # Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended]) 42 | UseMotionModel: 1 43 | 44 | 45 | 46 | #-------------------------------------------------------------------------------------------- 47 | # current trajectory to get evaluated 48 | traj2Eval: 4 49 | # run x (we need multiple runs to evaluate the accuracy because SLAM is not deterministic) 50 | trajrun: 1 51 | 52 | ### trajectory options 53 | #------------------------------ 54 | ### initial frame Traj 1 55 | traj.StartFrame: 1 56 | traj.EndFrame: 1629 57 | #------------------------------ 58 | 59 | #-------------------------------------------------------------------------------------------- 60 | # Viewer settings 61 | Viewer.MultiKeyFrameSize: 0.08 62 | Viewer.MultiKeyFrameLineWidth: 2 63 | Viewer.GraphLineWidth: 1 64 | Viewer.PointSize: 3 65 | Viewer.CameraSize: 0.1 66 | Viewer.CameraLineWidth: 3 67 | Viewer.ViewpointX: 1.0 68 | Viewer.ViewpointY: 1.0 69 | Viewer.ViewpointZ: 1.0 70 | Viewer.ViewpointF: 500.0 71 | Viewer.DrawNrCams: 2 -------------------------------------------------------------------------------- /Examples/Lafida/Slam_Settings_indoor2.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | # Camera frames per second 3 | Camera.fps: 25.0 4 | 5 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 6 | Camera.RGB: 1 7 | 8 | #-------------------------------------------------------------------------------------------- 9 | ### Changing the parameters below could seriously degradate the performance of the system 10 | 11 | # 0-> ORB, 1->dBRIEF 12 | extractor.usemdBRIEF: 0 13 | 14 | # 1-> learn masks, 0 don't (mdBRIEF) 15 | extractor.masks: 0 16 | 17 | # 1-> use Agast, 0 don't 18 | extractor.useAgast: 0 19 | 20 | # 0,1,2,(3 is only possible for agast) 21 | extractor.fastAgastType: 2 22 | 23 | # Extractor: 32 -> ORB , (16/32/64) -> dBRIEF and mdBRIEF 24 | extractor.descSize: 32 25 | 26 | # Extractor: Number of features per image 27 | extractor.nFeatures: 400 28 | 29 | # Extractor: Scale factor between levels in the scale pyramid 30 | extractor.scaleFactor: 1.2 31 | 32 | # Extractor: Number of levels in the scale pyramid 33 | extractor.nLevels: 8 34 | 35 | # Extractor: FAST threshold (lower less restrictive) 36 | extractor.fastTh: 20 37 | 38 | # Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score 39 | extractor.nScoreType: 0 40 | 41 | # Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended]) 42 | UseMotionModel: 1 43 | 44 | 45 | 46 | #-------------------------------------------------------------------------------------------- 47 | # current trajectory to get evaluated 48 | traj2Eval: 2 49 | # run x (we need multiple runs to evaluate the accuracy because SLAM is not deterministic) 50 | trajrun: 1 51 | 52 | ### trajectory options 53 | #------------------------------ 54 | ### initial frame Traj 2 55 | traj.StartFrame: 1 56 | traj.EndFrame: 899 57 | #------------------------------ 58 | 59 | #-------------------------------------------------------------------------------------------- 60 | # Viewer settings 61 | Viewer.MultiKeyFrameSize: 0.05 62 | Viewer.MultiKeyFrameLineWidth: 1 63 | Viewer.GraphLineWidth: 0.9 64 | Viewer.PointSize: 2 65 | Viewer.CameraSize: 0.08 66 | Viewer.CameraLineWidth: 3 67 | Viewer.ViewpointX: 1.0 68 | Viewer.ViewpointY: 1.0 69 | Viewer.ViewpointZ: 1.0 70 | Viewer.ViewpointF: 500.0 71 | Viewer.DrawNrCams: 2 -------------------------------------------------------------------------------- /Examples/Lafida/Slam_Settings_indoor1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | # Camera frames per second 3 | Camera.fps: 25.0 4 | 5 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 6 | Camera.RGB: 1 7 | 8 | #-------------------------------------------------------------------------------------------- 9 | ### Changing the parameters below could seriously degradate the performance of the system 10 | 11 | # 0-> ORB, 1->dBRIEF 12 | extractor.usemdBRIEF: 0 13 | 14 | # 1-> learn masks, 0 don't (mdBRIEF) 15 | extractor.masks: 0 16 | 17 | # 1-> use Agast, 0 don't 18 | extractor.useAgast: 0 19 | 20 | # 0,1,2,(3 is only possible for agast) 21 | extractor.fastAgastType: 2 22 | 23 | # Extractor: 32 -> ORB , (16/32/64) -> dBRIEF and mdBRIEF 24 | extractor.descSize: 32 25 | 26 | # Extractor: Number of features per image 27 | extractor.nFeatures: 400 28 | 29 | # Extractor: Scale factor between levels in the scale pyramid 30 | extractor.scaleFactor: 1.2 31 | 32 | # Extractor: Number of levels in the scale pyramid 33 | extractor.nLevels: 8 34 | 35 | # Extractor: FAST threshold (lower less restrictive) 36 | extractor.fastTh: 20 37 | 38 | # Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score 39 | extractor.nScoreType: 0 40 | 41 | # Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended]) 42 | UseMotionModel: 1 43 | 44 | 45 | 46 | #-------------------------------------------------------------------------------------------- 47 | # current trajectory to get evaluated 48 | traj2Eval: 1 49 | # run x (we need multiple runs to evaluate the accuracy because SLAM is not deterministic) 50 | trajrun: 1 51 | 52 | ### trajectory options 53 | #------------------------------ 54 | ### initial frame Traj 1 55 | traj.StartFrame: 25 56 | traj.EndFrame: 759 57 | #traj.EndFrame: 100 58 | #------------------------------ 59 | 60 | #-------------------------------------------------------------------------------------------- 61 | # Viewer settings 62 | Viewer.MultiKeyFrameSize: 0.05 63 | Viewer.MultiKeyFrameLineWidth: 1 64 | Viewer.GraphLineWidth: 0.9 65 | Viewer.PointSize: 2 66 | Viewer.CameraSize: 0.08 67 | Viewer.CameraLineWidth: 3 68 | Viewer.ViewpointX: 1.0 69 | Viewer.ViewpointY: 1.0 70 | Viewer.ViewpointZ: 1.0 71 | Viewer.ViewpointF: 500.0 72 | Viewer.DrawNrCams: 3 -------------------------------------------------------------------------------- /src/camSubscriber.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | class camSubs{ 9 | public: 10 | ros::NodeHandle nh; 11 | image_transport::Subscriber sub, sub1, sub2; 12 | image_transport::ImageTransport it; 13 | 14 | camSubs():it(nh){ 15 | this->sub = it.subscribe("camera/image/webcam", 1, &camSubs::imageCbWeb, this); 16 | this->sub1 = it.subscribe("camera/image/usb1", 1, &camSubs::imageCbUsb1, this); 17 | this->sub2 = it.subscribe("camera/image/usb2", 1, &camSubs::imageCbUsb2, this); 18 | ROS_INFO("Subscriber setup!!"); 19 | } 20 | 21 | void imageCbWeb(const sensor_msgs::ImageConstPtr& msg){ 22 | std::cout << "webcam header : \n" << msg->header << std::endl; 23 | try{ 24 | cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image); 25 | cv::waitKey(1); 26 | } 27 | catch(cv_bridge::Exception& e){ 28 | ROS_ERROR("Couldn't get image due to %s",e.what()); 29 | } 30 | } 31 | 32 | void imageCbUsb1(const sensor_msgs::ImageConstPtr& msg){ 33 | std::cout << "usb1 header : \n" << msg->header << std::endl; 34 | try{ 35 | cv::imshow("view1", cv_bridge::toCvShare(msg, "bgr8")->image); 36 | cv::waitKey(1); 37 | } 38 | catch(cv_bridge::Exception& e){ 39 | ROS_ERROR("Couldn't get image due to %s",e.what()); 40 | } 41 | } 42 | 43 | void imageCbUsb2(const sensor_msgs::ImageConstPtr& msg){ 44 | std::cout << "usb2 header : \n" << msg->header << std::endl; 45 | try{ 46 | cv::imshow("view2", cv_bridge::toCvShare(msg, "bgr8")->image); 47 | cv::waitKey(1); 48 | } 49 | catch(cv_bridge::Exception& e){ 50 | ROS_ERROR("Couldn't get image due to %s",e.what()); 51 | } 52 | } 53 | }; 54 | 55 | int main(int argc, char** argv){ 56 | ros::init(argc, argv, "Image_Listener"); 57 | ros::NodeHandle n; 58 | 59 | cv::namedWindow("view"); 60 | cv::startWindowThread(); 61 | 62 | cv::namedWindow("view1"); 63 | cv::startWindowThread(); 64 | 65 | cv::namedWindow("view2"); 66 | cv::startWindowThread(); 67 | 68 | camSubs *Subs = new camSubs; 69 | while(ros::ok()){ 70 | ros::spinOnce(); 71 | } 72 | 73 | cv::destroyWindow("view"); 74 | cv::destroyWindow("view1"); 75 | cv::destroyWindow("view2"); 76 | } 77 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Multi-Camera-SLAM 2 | 3 | ROS-Kinetic implementation of Urbstate MultiCol-SLAM 4 | 5 | This is a stable ROS implementation of the MultiCol-SLAM algorithm (original author: Stephan Urban). Please refer to the 6 | "https://github.com/urbste/MultiCol-SLAM" for initial implementation. This enables the user to run MultiCol-SLAM algorithm using live camera feed and perform real-time SLAM. 7 | 8 | Please note that the calibration files are very crucial for efficient tracking. Please refer to https://github.com/urbste/MultiCol-SLAM/issues/9, for concerns on multiple camera calibration setup. 9 | 10 | # Third Party dependencies needed 11 | 12 | 1) Pangolin 13 | For visualization. Get the instructions here: https://github.com/stevenlovegrove/Pangolin. This isn't present in the ./ThirdParty folder, needs to be installed off-the-shelf. 14 | 15 | 2) Eigen3 16 | Required by g2o Version 3.2.9 is included in the ./ThirdParty folder. Other version can be found at http://eigen.tuxfamily.org/index.php?title=Main_Page. 17 | 18 | 3) OpenGV 19 | OpenGV can be found at: https://github.com/laurentkneip/opengv. It is also included in the ./ThirdParty folder. OpenGV is used for re-localization (GP3P) and relative orientation estimation during initialization (Stewenius). 20 | 21 | 4) DBoW2 and g2o 22 | As ORB-SLAM2 modified versions of DBoW2 and g2o are used for place recognition and optimization respectively. Both are included in the ./ThirdParty folder. The original versions can be found here: https://github.com/dorian3d/DBoW2, https://github.com/RainerKuemmerle/g2o. 23 | 24 | 5) OpenCV 3 25 | ROS-Kinetic has its own version of OpenCV-3.3.1-Dev. This version works fine with the MultiCol-SLAM, and I would recommend not to mend with the OpenCV version. In any case OpenCV can be found and installed from: https://github.com/opencv/opencv. 26 | 27 | # Building and running ROS nodes 28 | 29 | Build the package:- 30 | 1) Add the MultiCol-SLAM package path to the ROS_PACKAGE_PATH envirnoment variable as shown below 31 | 32 | export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:/*path_to_directory*/Examples/ROS/MultiCol-SLAM 33 | 34 | 2) Compile the MultiCol-SLAM source codes into a condensed library with *bash build.sh* command. 35 | 3) Compile the ROS executable with the condensed libraries with *bash build_ros.sh* command. 36 | 37 | Run the SLAM algorithm using the ROS toolchain:- 38 | 1) Initiate the ros master (roscore or an existing roslaunch file) 39 | 2) Run the camDriver node (rosrun *your_package_name* camDriver) 40 | 3) Run the SLAM node (rosrun *your_package_name* Multi) 41 | 42 | To change the calibration or setting files, go to Examples/ROS/MultiCol-SLAM/src/ros_multi.cpp, change the file paths appropriately 43 | -------------------------------------------------------------------------------- /include/cMap.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * 4 | * Copyright (C) 2015-2016 Steffen Urban 5 | * For more information see 6 | * 7 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * MultiCol-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with MultiCol-SLAM . If not, see . 19 | */ 20 | 21 | /* 22 | * MultiCol-SLAM is based on ORB-SLAM2 which was also released under GPLv3 23 | * For more information see 24 | * Raúl Mur-Artal (University of Zaragoza) 25 | */ 26 | 27 | #ifndef MAP_H 28 | #define MAP_H 29 | 30 | // own includes 31 | #include "cMapPoint.h" 32 | #include "cMultiKeyFrame.h" 33 | // external includes 34 | #include 35 | #include 36 | namespace MultiColSLAM 37 | { 38 | 39 | class cMapPoint; 40 | class cMultiKeyFrame; 41 | 42 | class cMap 43 | { 44 | public: 45 | cMap(); 46 | 47 | void AddKeyFrame(cMultiKeyFrame* pKF); 48 | void AddMapPoint(cMapPoint* pMP); 49 | void EraseMapPoint(cMapPoint* pMP); 50 | void EraseKeyFrame(cMultiKeyFrame* pKF); 51 | void SetCurrentCameraPose(cv::Mat Tcw); 52 | void SetReferenceKeyFrames(const std::vector &vpKFs); 53 | void SetReferenceMapPoints(const std::vector &vpMPs); 54 | 55 | std::vector GetAllKeyFrames(); 56 | std::vector GetAllMapPoints(); 57 | 58 | std::vector GetReferenceKeyFrames(); 59 | std::vector GetReferenceMapPoints(); 60 | 61 | void GetModelPoints(std::vector& modelPts, 62 | std::vector& modelCompanionPts); 63 | void SetModelPoints(const std::vector& _modelPts, 64 | const std::vector& _modelCompanionPts); 65 | 66 | int MapPointsInMap(); 67 | int KeyFramesInMap(); 68 | 69 | void SetFlagAfterBA(); 70 | bool isMapUpdated(); 71 | void ResetUpdated(); 72 | 73 | unsigned int GetMaxKFid(); 74 | 75 | void clear(); 76 | 77 | //std::mutex mMutexMapUpdate; 78 | 79 | protected: 80 | std::set mspMapPoints; 81 | std::set mspKeyFrames; 82 | std::vector mvpReferenceMapPoints; 83 | 84 | unsigned int mnMaxKFid; 85 | 86 | std::mutex mMutexMap; 87 | bool mbMapUpdated; 88 | 89 | }; 90 | 91 | } 92 | #endif // MAP_H 93 | -------------------------------------------------------------------------------- /Examples/ROS/MultiCol-SLAM/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | #project(MultiCol-SLAM-ROS) 4 | 5 | rosbuild_init() 6 | 7 | IF(NOT ROS_BUILD_TYPE) 8 | SET(ROS_BUILD_TYPE Release) 9 | ENDIF() 10 | 11 | MESSAGE("Build type: " ${ROS_BUILD_TYPE}) 12 | 13 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 14 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 15 | 16 | # Check C++11 or C++0x support 17 | include(CheckCXXCompilerFlag) 18 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 19 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 20 | if(COMPILER_SUPPORTS_CXX11) 21 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 22 | add_definitions(-DCOMPILEDWITHC11) 23 | message(STATUS "Using flag -std=c++11.") 24 | elseif(COMPILER_SUPPORTS_CXX0X) 25 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 26 | add_definitions(-DCOMPILEDWITHC0X) 27 | message(STATUS "Using flag -std=c++0x.") 28 | else() 29 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 30 | endif() 31 | 32 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../../cmake_modules) 33 | 34 | find_package(OpenCV 3.0 QUIET) 35 | if(NOT OpenCV_FOUND) 36 | find_package(OpenCV 2.4.3 QUIET) 37 | if(NOT OpenCV_FOUND) 38 | message(FATAL_ERROR "OpenCV > 2.4.3 not found.") 39 | endif() 40 | endif() 41 | 42 | IF(NOT DEFINED EIGEN_INCLUDE_DIR_HINTS) 43 | SET(EIGEN_INCLUDE_DIR ${PROJECT_SOURCE_DIR}/../../../ThirdParty/Eigen) 44 | MESSAGE("Eigen include DIR ${EIGEN_INCLUDE_DIR}") 45 | #SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/ThirdParty/Eigen/cmake) 46 | #FIND_PACKAGE(Eigen3 REQUIRED) 47 | #MESSAGE("-- Eigen version ${EIGEN3_VERSION}: ${EIGEN3_INCLUDE_DIR}") 48 | #INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR}) 49 | SET(MultiCol-SLAM_USE_INTERNAL_EIGEN ON) 50 | INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIR}) 51 | ENDIF() 52 | 53 | find_package(Pangolin REQUIRED) 54 | 55 | include_directories( 56 | ${PROJECT_SOURCE_DIR} 57 | ${PROJECT_SOURCE_DIR}/include 58 | ${PROJECT_SOURCE_DIR}/../../../ 59 | ${PROJECT_SOURCE_DIR}/../../../include 60 | ${PROJECT_SOURCE_DIR}/../../../ThirdParty 61 | ${PROJECT_SOURCE_DIR}/../../../ThirdParty/g2o 62 | ${PROJECT_SOURCE_DIR}/../../../ThirdParty/OpenGV/include 63 | ${PROJECT_SOURCE_DIR}/../../../ThirdParty/DBoW2 64 | ${Pangolin_INCLUDE_DIRS} 65 | ) 66 | 67 | SET (G2OLIBS ${PROJECT_SOURCE_DIR}/../../../ThirdParty/g2o/lib/libg2o.so) 68 | SET (DBOW2LIBS ${PROJECT_SOURCE_DIR}/../../../ThirdParty/DBoW2/lib/libDBoW2.so) 69 | SET (OPENGVLIBS ${PROJECT_SOURCE_DIR}/../../../ThirdParty/OpenGV/build/lib/librandom_generators.a 70 | ${PROJECT_SOURCE_DIR}/../../../ThirdParty/OpenGV/build/lib/libopengv.a) 71 | 72 | set(LIBS 73 | ${OpenCV_LIBS} 74 | ${Pangolin_LIBRARIES} 75 | ${G2OLIBS} 76 | ${DBOW2LIBS} 77 | ${OPENGVLIBS} 78 | ${RANDOMGENLIBS} 79 | #-lboost_system 80 | ) 81 | 82 | # Node for multi camera 83 | rosbuild_add_executable(Multi 84 | src/ros_multi.cc 85 | ) 86 | 87 | #add_dependencies(Multi g2o DBoW2) 88 | 89 | target_link_libraries(Multi 90 | ${LIBS} 91 | /home/varunharis/MultiCol-SLAM/lib/libMultiCol-SLAM.so 92 | ) 93 | -------------------------------------------------------------------------------- /include/cORBextractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM. 3 | * 4 | * Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM. If not, see . 19 | */ 20 | 21 | #ifndef ORBEXTRACTOR_H 22 | #define ORBEXTRACTOR_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | 29 | //namespace ORB_SLAM 30 | //{ 31 | 32 | class ExtractorNode 33 | { 34 | public: 35 | ExtractorNode() :bNoMore(false){} 36 | 37 | void DivideNode(ExtractorNode &n1, 38 | ExtractorNode &n2, 39 | ExtractorNode &n3, 40 | ExtractorNode &n4); 41 | 42 | std::vector vKeys; 43 | cv::Point2i UL, UR, BL, BR; 44 | std::list::iterator lit; 45 | bool bNoMore; 46 | }; 47 | 48 | class ORBextractor 49 | { 50 | public: 51 | 52 | enum {HARRIS_SCORE=0, FAST_SCORE=1 }; 53 | 54 | ORBextractor(int nfeatures = 1000, 55 | double scaleFactor = 1.2, 56 | int nlevels = 8, 57 | int scoreType = FAST_SCORE, 58 | int fastTh = 20); 59 | 60 | ~ORBextractor(){} 61 | 62 | // Compute the ORB features and descriptors on an image 63 | void operator()( cv::InputArray image, cv::InputArray mask, 64 | std::vector& keypoints, 65 | cv::OutputArray descriptors); 66 | 67 | int inline GetLevels(){ 68 | return nlevels;} 69 | 70 | double inline GetScaleFactor(){ 71 | return scaleFactor;} 72 | 73 | 74 | protected: 75 | void ComputePyramid(cv::Mat image, cv::Mat Mask = cv::Mat()); 76 | void ComputeKeyPointsOctTree(std::vector >& allKeypoints); 77 | std::vector DistributeOctTree( 78 | const std::vector& vToDistributeKeys, 79 | const int &minX, 80 | const int &maxX, 81 | const int &minY, 82 | const int &maxY, 83 | const int &nFeatures, 84 | const int &level); 85 | 86 | void ComputeKeyPointsOld(std::vector >& allKeypoints); 87 | 88 | //void ComputePyramid(cv::Mat image, cv::Mat Mask=cv::Mat()); 89 | //void ComputeKeyPoints(std::vector >& allKeypoints); 90 | 91 | std::vector pattern; 92 | 93 | int nfeatures; 94 | double scaleFactor; 95 | int nlevels; 96 | int scoreType; 97 | int fastTh; 98 | 99 | std::vector mnFeaturesPerLevel; 100 | 101 | std::vector umax; 102 | 103 | std::vector mvScaleFactor; 104 | std::vector mvInvScaleFactor; 105 | 106 | std::vector mvImagePyramid; 107 | std::vector mvMaskPyramid; 108 | 109 | }; 110 | 111 | //} //namespace ORB_SLAM 112 | 113 | #endif 114 | 115 | -------------------------------------------------------------------------------- /src/misc.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * It is based on the file orb.cpp from the OpenCV library (see BSD license below). 4 | * 5 | * Copyright (C) 2015-2016 Steffen Urban 6 | * For more information see 7 | * 8 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * MultiCol-SLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with MultiCol-SLAM . If not, see . 20 | */ 21 | #include "misc.h" 22 | 23 | namespace MultiColSLAM 24 | { 25 | cv::Vec3d triangulate_point( 26 | const cv::Matx31d& relOri_t12, 27 | const cv::Matx33d& relOri_R12, 28 | const cv::Vec3d& v1, 29 | const cv::Vec3d& v2) 30 | { 31 | // this one is adapted to opencv from opengv::triangulate2 32 | cv::Vec3d pt3(0, 0, 0); 33 | cv::Vec3d f2_unrotated = relOri_R12 * v2; 34 | cv::Vec2d b; 35 | b[0] = relOri_t12.dot(v1); 36 | b[1] = relOri_t12.dot(f2_unrotated); 37 | cv::Matx22d A; 38 | A(0, 0) = v1.dot(v1); 39 | A(1, 0) = v1.dot(f2_unrotated); 40 | A(0, 1) = -A(1, 0); 41 | A(1, 1) = -f2_unrotated.dot(f2_unrotated); 42 | cv::Vec2d lambda = A.inv() * b; 43 | cv::Matx31d xm = lambda[0] * v1; 44 | cv::Matx31d xn = relOri_t12 + lambda[1] * f2_unrotated; 45 | cv::Matx31d tmpres = (xm + xn); 46 | pt3(0) = tmpres(0, 0) / 2.0; 47 | pt3(1) = tmpres(1, 0) / 2.0; 48 | pt3(2) = tmpres(2, 0) / 2.0; 49 | return pt3; 50 | } 51 | 52 | 53 | bool CheckDistEpipolarLine(const cv::Vec3d &ray1, 54 | const cv::Vec3d &ray2, 55 | const cv::Matx33d &E12, 56 | const double& thresh) 57 | { 58 | cv::Vec nom = ray2.t()*E12*ray1; 59 | cv::Vec3d Ex1 = E12*ray1; 60 | cv::Vec3d Etx2 = E12.t()*ray2; 61 | 62 | const double den = Ex1(0)*Ex1(0) + Ex1(1)*Ex1(1) + Ex1(2)*Ex1(2) + 63 | Etx2(0)*Etx2(0) + Etx2(1)*Etx2(1) + Etx2(2)*Etx2(2); 64 | 65 | if (den == 0.0) 66 | return false; 67 | const double dsqr = (nom(0)*nom(0)) / den; 68 | return dsqr < thresh; 69 | } 70 | 71 | cv::Matx33d ComputeE(const cv::Matx44d& T1, const cv::Matx44d& T2) 72 | { 73 | cv::Matx33d R1w = T1.get_minor<3, 3>(0, 0); 74 | cv::Matx33d R2w = T2.get_minor<3, 3>(0, 0); 75 | 76 | cv::Vec3d t1w = cv::Vec3d(T1(0, 3), T1(1, 3), T1(2, 3)); 77 | cv::Vec3d t2w = cv::Vec3d(T2(0, 3), T2(1, 3), T2(2, 3)); 78 | 79 | cv::Matx33d R12 = R1w*R2w.t(); 80 | cv::Vec3d t12 = -R1w*R2w.t()*t2w + t1w; 81 | t12 /= cv::norm(t12); 82 | cv::Matx33d t12x = Skew(t12); 83 | 84 | return t12x*R12; 85 | } 86 | 87 | double T_in_ms(HResClk::time_point start, 88 | HResClk::time_point end) 89 | { 90 | return static_cast( 91 | std::chrono::duration_cast(end - start).count()); 92 | } 93 | 94 | double T_in_ns(HResClk::time_point start, 95 | HResClk::time_point end) 96 | { 97 | return static_cast( 98 | std::chrono::duration_cast(end - start).count()); 99 | } 100 | 101 | } -------------------------------------------------------------------------------- /include/cOptimizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * 4 | * Copyright (C) 2015-2016 Steffen Urban 5 | * For more information see 6 | * 7 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * MultiCol-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with MultiCol-SLAM . If not, see . 19 | */ 20 | 21 | /* 22 | * MultiCol-SLAM is based on ORB-SLAM2 which was also released under GPLv3 23 | * For more information see 24 | * Raúl Mur-Artal (University of Zaragoza) 25 | */ 26 | 27 | #ifndef OPTIMIZER_H 28 | #define OPTIMIZER_H 29 | 30 | #include "cMap.h" 31 | #include "cMapPoint.h" 32 | #include "cMultiKeyFrame.h" 33 | #include "cLoopClosing.h" 34 | #include "cMultiFrame.h" 35 | 36 | namespace MultiColSLAM 37 | { 38 | const double huberK = 3; 39 | const double huberK2 = huberK*huberK; 40 | 41 | 42 | //namespace ORB_SLAM 43 | //{ 44 | 45 | class cLoopClosing; 46 | 47 | class cOptimizer 48 | { 49 | public: 50 | void static BundleAdjustment(const std::vector &vpKF, 51 | const std::vector &vpMP, 52 | bool poseOnly, 53 | int nIterations = 5, 54 | bool *pbStopFlag = NULL); 55 | 56 | void static GlobalBundleAdjustment(cMap* pMap, 57 | bool poseOnly = false, 58 | int nIterations = 5, 59 | bool *pbStopFlag = NULL); 60 | 61 | static std::list LocalBundleAdjustment(cMultiKeyFrame* pKF, 62 | cMap* pMap, 63 | int nrIters = 10, 64 | bool getCovMats = false, 65 | bool *pbStopFlag = NULL); 66 | 67 | int static PoseOptimization(cMultiFrame* pFrame, 68 | double& inliers, 69 | const double& huberMultiplier = 2); 70 | 71 | bool static StructureOnly(cMultiKeyFrame* pKF, 72 | cMapPoint* &vpMP, 73 | vector >& obs, 74 | const double& reprErr); 75 | 76 | void static OptimizeEssentialGraph(cMap* pMap, 77 | cMultiKeyFrame* pLoopMKF, cMultiKeyFrame* pCurMKF, 78 | g2o::Sim3 &Scurw, 79 | const cLoopClosing::KeyFrameAndPose &NonCorrectedSim3, 80 | const cLoopClosing::KeyFrameAndPose &CorrectedSim3, 81 | const std::map > &LoopConnections); 82 | 83 | static int OptimizeSim3(cMultiKeyFrame* pKF1, 84 | cMultiKeyFrame* pKF2, 85 | std::vector &vpMatches1, 86 | g2o::Sim3 &g2oS12); 87 | 88 | 89 | static std::list MotionOnlyBA(cMultiKeyFrame* pKF, 90 | cMap* pMap); 91 | 92 | static int StructureOnlyBA(cMultiKeyFrame* pKF, 93 | cMap* pMap); 94 | 95 | static int AdjustMKF2Model(cMultiKeyFrame* pKF, cMap* pMap); 96 | 97 | static int AdjustFrame2Model(cMultiFrame* frame, cMap* pMap); 98 | 99 | static int CalibrateMKSonModel(cMultiKeyFrame* pKF, cMap* pMap); 100 | 101 | static double stdRecon; 102 | static double stdPose; 103 | static double stdSim; 104 | static double stdModel; 105 | }; 106 | 107 | } 108 | #endif // OPTIMIZER_H 109 | -------------------------------------------------------------------------------- /src/g2o_MultiCol_sim3_expmap.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * 4 | * Copyright (C) 2015-2016 Steffen Urban 5 | * For more information see 6 | * 7 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * MultiCol-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with MultiCol-SLAM . If not, see . 19 | */ 20 | #include "g2o_MultiCol_sim3_expmap.h" 21 | 22 | namespace MultiColSLAM 23 | { 24 | 25 | VertexSim3Expmap_Multi::VertexSim3Expmap_Multi( 26 | std::unordered_map& kp_to_cam1, 27 | std::unordered_map& kp_to_cam2) 28 | : g2o::BaseVertex<7, g2o::Sim3>(), 29 | keypoint_to_cam1(kp_to_cam1), 30 | keypoint_to_cam2(kp_to_cam2) 31 | { 32 | _marginalized = false; 33 | _fix_scale = false; 34 | } 35 | 36 | simpleVertexSim3Expmap::simpleVertexSim3Expmap() : BaseVertex<7, g2o::Sim3>() 37 | { 38 | _fix_scale = false; 39 | } 40 | 41 | 42 | edgeSim3::edgeSim3() : 43 | BaseBinaryEdge<7, g2o::Sim3, simpleVertexSim3Expmap, simpleVertexSim3Expmap>() 44 | { 45 | } 46 | 47 | bool simpleVertexSim3Expmap::read(std::istream& is) 48 | { 49 | g2o::Vector7d cam2world; 50 | //for (int i = 0; i<6; i++){ 51 | // is >> cam2world[i]; 52 | //} 53 | //is >> cam2world[6]; 54 | //// if (! is) { 55 | //// // if the scale is not specified we set it to 1; 56 | //// std::cerr << "!s"; 57 | //// cam2world[6]=0.; 58 | //// } 59 | 60 | //for (int i = 0; i<2; i++) 61 | //{ 62 | // is >> _focal_length1[i]; 63 | //} 64 | //for (int i = 0; i<2; i++) 65 | //{ 66 | // is >> _principle_point1[i]; 67 | //} 68 | 69 | setEstimate(g2o::Sim3(cam2world).inverse()); 70 | return true; 71 | } 72 | 73 | bool simpleVertexSim3Expmap::write(std::ostream& os) const 74 | { 75 | g2o::Sim3 cam2world(estimate().inverse()); 76 | g2o::Vector7d lv = cam2world.log(); 77 | //for (int i = 0; i<7; i++){ 78 | // os << lv[i] << " "; 79 | //} 80 | //for (int i = 0; i<2; i++) 81 | //{ 82 | // os << _focal_length1[i] << " "; 83 | //} 84 | //for (int i = 0; i<2; i++) 85 | //{ 86 | // os << _principle_point1[i] << " "; 87 | //} 88 | return os.good(); 89 | } 90 | 91 | 92 | bool edgeSim3::read(std::istream& is) 93 | { 94 | g2o::Vector7d v7; 95 | //for (int i = 0; i<7; i++){ 96 | // is >> v7[i]; 97 | //} 98 | 99 | g2o::Sim3 cam2world(v7); 100 | setMeasurement(cam2world.inverse()); 101 | 102 | //for (int i = 0; i<7; i++) 103 | // for (int j = i; j<7; j++) 104 | // { 105 | // is >> information()(i, j); 106 | // if (i != j) 107 | // information()(j, i) = information()(i, j); 108 | // } 109 | return true; 110 | } 111 | 112 | bool edgeSim3::write(std::ostream& os) const 113 | { 114 | g2o::Sim3 cam2world(measurement().inverse()); 115 | g2o::Vector7d v7 = cam2world.log(); 116 | //for (int i = 0; i<7; i++) 117 | //{ 118 | // os << v7[i] << " "; 119 | //} 120 | //for (int i = 0; i<7; i++) 121 | // for (int j = i; j<7; j++){ 122 | // os << " " << information()(i, j); 123 | // } 124 | return os.good(); 125 | } 126 | } -------------------------------------------------------------------------------- /include/cConverter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * 4 | * Copyright (C) 2015-2016 Steffen Urban 5 | * For more information see 6 | * 7 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * MultiCol-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with MultiCol-SLAM . If not, see . 19 | */ 20 | 21 | /* 22 | * MultiCol-SLAM is based on ORB-SLAM2 which was also released under GPLv3 23 | * For more information see 24 | */ 25 | 26 | #ifndef CONVERTER_H 27 | #define CONVERTER_H 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | namespace MultiColSLAM 37 | { 38 | 39 | class cConverter 40 | { 41 | public: 42 | static std::vector toDescriptorVector(const cv::Mat &Descriptors); 43 | static std::vector toDescriptorVector(const std::vector& Descriptors); 44 | 45 | static g2o::SE3Quat toSE3Quat(const cv::Matx44d& homCV); 46 | 47 | static cv::Matx44d toCvMat(const g2o::SE3Quat& SE3); 48 | static cv::Matx44d toCvMat(const g2o::Sim3& Sim3); 49 | static cv::Matx44d toCvMat(const Eigen::Matrix& m); 50 | static cv::Matx33d toCvMat(const Eigen::Matrix3d& m); 51 | static cv::Vec3d toCvVec3d(const Eigen::Matrix& m) { 52 | return cv::Vec3d(m(0), m(1), m(2)); } 53 | static cv::Matx44d toCvSE3(const Eigen::Matrix& R, 54 | const Eigen::Matrix& t); 55 | static cv::Matx ogv2ocv(const Eigen::Matrix& ogv_mat); 56 | 57 | // cv to eigen 58 | static Eigen::Matrix toVector3d(const cv::Vec4d& cvVector); 59 | static Eigen::Matrix toVector3d(const cv::Vec3d& cvVector); 60 | static Eigen::Matrix toVector2d(const cv::Vec2d& cvVector) { 61 | return Eigen::Matrix(cvVector(0), cvVector(1)); } 62 | 63 | static Eigen::Matrix toMatrix3d(const cv::Matx33d& cvMat3); 64 | 65 | static std::vector toQuaternion(const cv::Matx33d& M); 66 | // between mats and vecs 67 | static cv::Matx33d Hom2R(const cv::Matx44d& homCV) { 68 | return cv::Matx33d(homCV(0, 0), homCV(0, 1), homCV(0, 2), 69 | homCV(1, 0), homCV(1, 1), homCV(1, 2), 70 | homCV(2, 0), homCV(2, 1), homCV(2, 2)); } 71 | 72 | static cv::Vec3d Hom2T(const cv::Matx44d& homCV) { 73 | return cv::Vec3d(homCV(0, 3), homCV(1, 3), homCV(2, 3)); } 74 | 75 | static cv::Matx44d Rt2Hom(const cv::Matx33d& R, const cv::Vec3d& t){ 76 | return cv::Matx44d(R(0, 0), R(0, 1), R(0, 2), t(0), 77 | R(1, 0), R(1, 1), R(1, 2), t(1), 78 | R(2, 0), R(2, 1), R(2, 2), t(2), 79 | 0.0, 0.0, 0.0, 1.0); } 80 | 81 | // between vecs 82 | static cv::Vec4d toVec4d(const cv::Vec3d& in) { 83 | return cv::Vec4d(in(0), in(1), in(2), 1.0); } 84 | static cv::Vec4d toVec4d(const Eigen::Matrix& in){ 85 | return cv::Vec4d(in(0), in(1), in(2), 1.0); } 86 | 87 | // between matx and mat 88 | static cv::Mat toMat(const cv::Matx44d& matx44d); 89 | 90 | // hom 91 | static cv::Matx44d invMat(const cv::Matx44d& M); 92 | 93 | }; 94 | } 95 | #endif // cCONVERTER_H 96 | -------------------------------------------------------------------------------- /src/imuDriver.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | class imuDriver{ 12 | public: 13 | ros::NodeHandle nh; 14 | ros::Subscriber sub; 15 | ros::Publisher pub; 16 | 17 | geometry_msgs::Vector3 gyro, acc, gyroInit; 18 | geometry_msgs::Quaternion quat; 19 | sensor_msgs::Imu msg; 20 | float roll, pitch, yaw, tSample; 21 | 22 | imuDriver(){ 23 | sub = nh.subscribe("/ecoprt/imu",10,&imuDriver::getIMUData, this); 24 | pub = nh.advertise("/slam/imu", 10); 25 | gyroInit.x = 0; 26 | gyroInit.y = 0; 27 | gyroInit.z = 0; 28 | } 29 | 30 | void getIMUData(const custom_msgs::IMUInfoPtr& msg){ 31 | gyro.x = msg->DataGyro[0]; 32 | gyro.y = msg->DataGyro[1]; 33 | gyro.z = msg->DataGyro[2]; 34 | 35 | acc.x = msg->DataAcc[0]; 36 | acc.y = msg->DataAcc[1]; 37 | acc.z = msg->DataAcc[2]; 38 | 39 | tSample = msg->IMUSampFreq; 40 | //std::cout << gyro << "\n" << acc << std::endl; 41 | } 42 | 43 | void getOrientation(){ 44 | geometry_msgs::Vector3 unitAcc; 45 | // Unit vector for linear acceleration 46 | unitAcc.x = acc.x/sqrt(pow(acc.x,2) + pow(acc.y,2) + pow(acc.z,2)); 47 | unitAcc.y = acc.y/sqrt(pow(acc.x,2) + pow(acc.y,2) + pow(acc.z,2)); 48 | unitAcc.z = acc.z/sqrt(pow(acc.x,2) + pow(acc.y,2) + pow(acc.z,2)); 49 | 50 | geometry_msgs::Vector3 angAcc; 51 | // Angular acceleration = derivative of angular velocity wrt time 52 | angAcc.x = (gyro.x - gyroInit.x)/tSample; 53 | angAcc.y = (gyro.y - gyroInit.y)/tSample; 54 | angAcc.z = (gyro.z - gyroInit.z)/tSample; 55 | 56 | roll = -atan2(-unitAcc.x,unitAcc.y); 57 | pitch = -atan2(unitAcc.z,sgn(unitAcc.y)*sqrt(pow(unitAcc.x,2)+pow(unitAcc.y,2))); 58 | yaw = angAcc.z*(gyroInit.z + gyro.z*tSample) + (1-angAcc.z)*atan2(acc.x,acc.y); 59 | 60 | gyroInit.x = gyro.x; 61 | gyroInit.y = gyro.y; 62 | gyroInit.z = gyro.z; 63 | //std::cout << "Roll " << roll*180/M_PI << "\n" << "Pitch " << pitch*180/M_PI << "\n" << "Yaw " << yaw*180/M_PI << std::endl; 64 | 65 | euler_to_quaternion(roll, pitch, yaw); 66 | } 67 | 68 | void euler_to_quaternion(float roll, float pitch, float yaw){ 69 | float cy = cos(yaw*0.5); 70 | float sy = sin(yaw*0.5); 71 | float cp = cos(pitch*0.5); 72 | float sp = sin(pitch*0.5); 73 | float cr = cos(roll*0.5); 74 | float sr = sin(roll*0.5); 75 | 76 | quat.w = cy * cp * cr + sy * sp * sr; 77 | quat.x = cy * cp * sr - sy * sp * cr; 78 | quat.y = sy * cp * sr + cy * sp * cr; 79 | quat.z = sy * cp * cr - cy * sp * sr; 80 | 81 | publishImuData(); 82 | } 83 | 84 | void publishImuData(){ 85 | uint32_t seq_id = 0; 86 | 87 | if(seq_id == UINT32_MAX) seq_id = 0; 88 | // We set all zeros for covariance matrices since they're unknown 89 | msg.header.seq = seq_id; 90 | msg.header.stamp = ros::Time::now(); 91 | msg.header.frame_id = "IMU"; 92 | 93 | msg.orientation.x = quat.x; 94 | msg.orientation.y = quat.y; 95 | msg.orientation.z = quat.z; 96 | msg.orientation.w = quat.w; 97 | msg.orientation_covariance = {0,0,0,0,0,0,0,0,0}; 98 | 99 | msg.angular_velocity.x = gyro.x; 100 | msg.angular_velocity.y = gyro.y; 101 | msg.angular_velocity.z = gyro.z; 102 | msg.angular_velocity_covariance = {0,0,0,0,0,0,0,0,0}; 103 | 104 | msg.linear_acceleration.x = acc.x; 105 | msg.linear_acceleration.y = acc.x; 106 | msg.linear_acceleration.z = acc.x; 107 | msg.linear_acceleration_covariance = {0,0,0,0,0,0,0,0,0}; 108 | 109 | pub.publish(msg); 110 | } 111 | 112 | int sgn(float v) { 113 | if (v < 0) return -1; 114 | if (v > 0) return 1; 115 | return 0; 116 | } 117 | }; 118 | 119 | int main(int argc, char** argv){ 120 | ros::init(argc, argv, "IMUDriver"); 121 | ros::NodeHandle n; 122 | 123 | imuDriver *imu = new imuDriver; 124 | while(ros::ok()){ 125 | imu->getOrientation(); 126 | ros::spinOnce(); 127 | } 128 | } 129 | -------------------------------------------------------------------------------- /Examples/ROS/MultiCol-SLAM/src/ros_multi_backup.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is the ROS implementation of MultiCol SLAM2 3 | */ 4 | 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include "../../../include/cSystem.h" 20 | 21 | #include "geometry_msgs/TransformStamped.h" 22 | #include "tf/transform_datatypes.h" 23 | #include 24 | 25 | using namespace std; 26 | 27 | class ImageGrabber 28 | { 29 | public: 30 | ImageGrabber(MultiColSLAM::cSystem* pSLAM):mpSLAM(pSLAM){} 31 | 32 | void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD); 33 | 34 | MultiColSLAM::cSystem* mpSLAM; 35 | }; 36 | 37 | int main(int argc, char **argv) 38 | { 39 | ros::init(argc, argv, "Multi"); 40 | ros::start(); 41 | 42 | if(argc != 4) 43 | { 44 | cerr << endl << "Usage: rosrun MultiCol Multi path_to_vocabulary path_to_settings path_to_calibration [1|0](save map?)" << endl; 45 | ros::shutdown(); 46 | return 1; 47 | } 48 | 49 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 50 | MultiColSLAM::cSystem SLAM(argv[1],argv[2],argv[3],true); 51 | 52 | ImageGrabber igb(&SLAM); 53 | 54 | ros::NodeHandle nh; 55 | 56 | message_filters::Subscriber rgb_sub(nh, "/camera/rgb/image_raw", 1); 57 | message_filters::Subscriber depth_sub(nh, "camera/depth_registered/image_raw", 1); 58 | typedef message_filters::sync_policies::ApproximateTime sync_pol; 59 | message_filters::Synchronizer sync(sync_pol(10), rgb_sub,depth_sub); 60 | sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2)); 61 | 62 | ros::spin(); 63 | 64 | // Stop all threads 65 | SLAM.Shutdown(); 66 | 67 | // Save camera trajectory 68 | SLAM.SaveMKFTrajectoryLAFIDA("KeyFrameTrajectory.txt"); 69 | //SLAM.SaveMap("map.bin"); 70 | 71 | ros::shutdown(); 72 | 73 | return 0; 74 | } 75 | 76 | void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD) 77 | { 78 | // Copy the ros image message to cv::Mat. 79 | cv_bridge::CvImageConstPtr cv_ptrRGB; 80 | try 81 | { 82 | cv_ptrRGB = cv_bridge::toCvShare(msgRGB); 83 | } 84 | catch (cv_bridge::Exception& e) 85 | { 86 | ROS_ERROR("cv_bridge exception: %s", e.what()); 87 | return; 88 | } 89 | 90 | cv_bridge::CvImageConstPtr cv_ptrD; 91 | try 92 | { 93 | cv_ptrD = cv_bridge::toCvShare(msgD); 94 | } 95 | catch (cv_bridge::Exception& e) 96 | { 97 | ROS_ERROR("cv_bridge exception: %s", e.what()); 98 | return; 99 | } 100 | 101 | //mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec()); 102 | cv::Mat pose = mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec()); 103 | 104 | if (pose.empty()) 105 | return; 106 | 107 | /* global left handed coordinate system */ 108 | static cv::Mat pose_prev = cv::Mat::eye(4,4, CV_32F); 109 | static cv::Mat world_lh = cv::Mat::eye(4,4, CV_32F); 110 | // matrix to flip signs of sinus in rotation matrix, not sure why we need to do that 111 | static const cv::Mat flipSign = (cv::Mat_(4,4) << 1,-1,-1, 1, 112 | -1, 1,-1, 1, 113 | -1,-1, 1, 1, 114 | 1, 1, 1, 1); 115 | 116 | //prev_pose * T = pose 117 | cv::Mat translation = (pose * pose_prev.inv()).mul(flipSign); 118 | world_lh = world_lh * translation; 119 | pose_prev = pose.clone(); 120 | 121 | tf::Matrix3x3 tf3d; 122 | tf3d.setValue(pose.at(0,0), pose.at(0,1), pose.at(0,2), 123 | pose.at(1,0), pose.at(1,1), pose.at(1,2), 124 | pose.at(2,0), pose.at(2,1), pose.at(2,2)); 125 | 126 | tf::Vector3 cameraTranslation_rh( world_lh.at(0,3),world_lh.at(1,3), - world_lh.at(2,3) ); 127 | 128 | //rotate 270deg about x and 270deg about x to get ENU: x forward, y left, z up 129 | const tf::Matrix3x3 rotation270degXZ( 0, 1, 0, 130 | 0, 0, 1, 131 | 1, 0, 0); 132 | 133 | static tf::TransformBroadcaster br; 134 | 135 | tf::Matrix3x3 globalRotation_rh = tf3d; 136 | tf::Vector3 globalTranslation_rh = cameraTranslation_rh * rotation270degXZ; 137 | 138 | tf::Quaternion tfqt; 139 | globalRotation_rh.getRotation(tfqt); 140 | 141 | double aux = tfqt[0]; 142 | tfqt[0]=-tfqt[2]; 143 | tfqt[2]=tfqt[1]; 144 | tfqt[1]=aux; 145 | 146 | tf::Transform transform; 147 | transform.setOrigin(globalTranslation_rh); 148 | transform.setRotation(tfqt); 149 | 150 | br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "camera_pose")); 151 | } 152 | -------------------------------------------------------------------------------- /Examples/ROS/MultiCol-SLAM/src/ros_multi.cc: -------------------------------------------------------------------------------- 1 | // The original version was released under the following license 2 | /** 3 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 4 | * For more information see 5 | * 6 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM2 is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with ORB-SLAM2. If not, see . 18 | */ 19 | 20 | // All modifications are released under the following license 21 | /** 22 | * This file is part of MultiCol-SLAM 23 | * 24 | * Copyright (C) 2015-2016 Steffen Urban 25 | * For more information see 26 | * 27 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 28 | * it under the terms of the GNU General Public License as published by 29 | * the Free Software Foundation, either version 3 of the License, or 30 | * (at your option) any later version. 31 | * 32 | * MultiCol-SLAM is distributed in the hope that it will be useful, 33 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 34 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 35 | * GNU General Public License for more details. 36 | * 37 | * You should have received a copy of the GNU General Public License 38 | * along with MultiCol-SLAM . If not, see . 39 | */ 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #include "cTracking.h" 55 | #include "cConverter.h" 56 | #include "cam_model_omni.h" 57 | #include "cSystem.h" 58 | #include "camNodelet.h" 59 | 60 | using namespace cv; 61 | using namespace std; 62 | 63 | camSubs::camSubs():it(nh){ 64 | sub = it.subscribe("/camera/image/webcam", 1, &camSubs::imageCbWeb, this); 65 | sub1 = it.subscribe("/camera/image/usb1", 1, &camSubs::imageCbUsb1, this); 66 | sub2 = it.subscribe("/camera/image/usb2", 1, &camSubs::imageCbUsb2, this); 67 | ROS_INFO("Subscriber setup!!"); 68 | } 69 | 70 | void camSubs::imageCbWeb(const sensor_msgs::ImageConstPtr& msg){ 71 | try{ 72 | header1 = msg->header; 73 | cv_bridge::CvImagePtr img = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); 74 | frame1 = img->image; 75 | } 76 | catch(cv_bridge::Exception& e){ 77 | ROS_ERROR("Couldn't get image due to %s",e.what()); 78 | } 79 | } 80 | 81 | void camSubs::imageCbUsb1(const sensor_msgs::ImageConstPtr& msg){ 82 | try{ 83 | header2 = msg->header; 84 | cv_bridge::CvImagePtr img = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); 85 | frame2 = img->image; 86 | } 87 | catch(cv_bridge::Exception& e){ 88 | ROS_ERROR("Couldn't get image due to %s",e.what()); 89 | } 90 | } 91 | 92 | void camSubs::imageCbUsb2(const sensor_msgs::ImageConstPtr& msg){ 93 | try{ 94 | header3 = msg->header; 95 | cv_bridge::CvImagePtr img = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); 96 | frame3 = img->image; 97 | } 98 | catch(cv_bridge::Exception& e){ 99 | ROS_ERROR("Couldn't get image due to %s",e.what()); 100 | } 101 | } 102 | 103 | int main(int argc, char **argv){ 104 | ros::init(argc, argv, "slam"); 105 | ros::NodeHandle nh; 106 | image_transport::ImageTransport it(nh); 107 | 108 | const string path2voc = "/home/varunharis/MultiCol-SLAM/Examples/small_orb_omni_voc_9_6.yml"; 109 | const string path2settings = "/home/varunharis/MultiCol-SLAM/Examples/Lafida/Slam_Settings_indoor1.yaml"; 110 | const string path2calibrations = "/home/varunharis/MultiCol-SLAM/Examples/Lafida"; 111 | 112 | camSubs *cam = new camSubs; 113 | 114 | cout << endl << "MultiCol-SLAM Copyright (C) 2016 Steffen Urban" << endl << endl; 115 | 116 | cv::FileStorage frameSettings(path2settings, cv::FileStorage::READ); 117 | 118 | cv::Mat frame1, frame2, frame3; 119 | std::vector images(3); 120 | MultiColSLAM::cSystem MultiSLAM(path2voc, path2settings, path2calibrations, true); 121 | 122 | // Main loop 123 | while(ros::ok()){ 124 | ros::spinOnce(); 125 | try{ 126 | frame1 = cam->frame1; 127 | frame2 = cam->frame2; 128 | frame3 = cam->frame3; 129 | 130 | cvtColor(frame1, frame1, CV_BGR2GRAY); 131 | cvtColor(frame2, frame2, CV_BGR2GRAY); 132 | cvtColor(frame3, frame3, CV_BGR2GRAY); 133 | 134 | images = {frame1, frame2, frame3}; 135 | // Pass through the MultiCol-SLAM system 136 | MultiSLAM.TrackMultiColSLAM(images, double(cam->header1.stamp.sec)); 137 | } 138 | catch(cv::Exception& e){ 139 | cout << "hold on" << endl; 140 | } 141 | } 142 | 143 | // Stop all threads 144 | MultiSLAM.Shutdown(); 145 | ros::shutdown(); 146 | 147 | // Save camera trajectory 148 | MultiSLAM.SaveMKFTrajectoryLAFIDA("MKFTrajectory.txt"); 149 | cv::destroyAllWindows(); 150 | return 0; 151 | } 152 | -------------------------------------------------------------------------------- /include/cMultiFrame.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * 4 | * Copyright (C) 2015-2016 Steffen Urban 5 | * For more information see 6 | * 7 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * MultiCol-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with MultiCol-SLAM . If not, see . 19 | */ 20 | 21 | /* 22 | * MultiCol-SLAM is based on ORB-SLAM2 which was also released under GPLv3 23 | * For more information see 24 | * Raúl Mur-Artal (University of Zaragoza) 25 | */ 26 | 27 | #ifndef MULTIFRAME_H 28 | #define MULTIFRAME_H 29 | 30 | #include "cMapPoint.h" 31 | #include "DBoW2/DBoW2/BowVector.h" 32 | #include "DBoW2/DBoW2/FeatureVector.h" 33 | #include "cORBVocabulary.h" 34 | #include "cMultiKeyFrame.h" 35 | #include "cORBextractor.h" 36 | #include "mdBRIEFextractorOct.h" 37 | #include "cam_system_omni.h" 38 | 39 | // external 40 | #include 41 | #include 42 | #include 43 | 44 | 45 | namespace MultiColSLAM 46 | { 47 | #define FRAME_GRID_ROWS 48 48 | #define FRAME_GRID_COLS 64 49 | 50 | class cTracking; 51 | class cMapPoint; 52 | class cMultiKeyFrame; 53 | class cMultiKeyFrameDatabase; 54 | class cCamModelGeneral_; 55 | 56 | class cMultiFrame 57 | { 58 | public: 59 | cMultiFrame(); 60 | cMultiFrame(const cMultiFrame &frame); // copy constructor 61 | 62 | cMultiFrame(const std::vector& images_, 63 | const double &timeStamp, 64 | std::vector extractor, 65 | ORBVocabulary* voc, 66 | cMultiCamSys_& camSystem_, 67 | int imgCnt); 68 | 69 | ORBVocabulary* mpORBvocabulary; 70 | 71 | std::vector mp_mdBRIEF_extractorOct; 72 | 73 | // images 74 | std::vector images; 75 | 76 | // Frame timestamp 77 | double mTimeStamp; 78 | 79 | // contains all cameras 80 | cMultiCamSys_ camSystem; 81 | 82 | // Number of KeyPoints 83 | std::vector N; 84 | // total number of Keypoints 85 | size_t totalN; 86 | 87 | // Vector of keypoints 2d 88 | // and corresponding bearing vectors 3d 89 | // for each camera 90 | std::vector mvKeys; 91 | std::vector mvKeysRays; // 3D observation rays 92 | 93 | // Bag of Words Vector structures 94 | DBoW2::BowVector mBowVec; 95 | std::vector mBowVecs; 96 | DBoW2::FeatureVector mFeatVec; 97 | std::vector mFeatVecs; 98 | // ORB descriptor, each row associated to a keypoint 99 | // descriptors for each camera 100 | std::vector mDescriptors; 101 | // learned descriptor masks 102 | std::vector mDescriptorMasks; 103 | 104 | // MapPoints associated to keypoints, NULL pointer if not association 105 | std::vector mvpMapPoints; 106 | 107 | // Flag to identify outlier associations 108 | std::vector mvbOutlier; 109 | 110 | // Keypoints are assigned to cells in a grid 111 | // to reduce matching complexity when projecting MapPoints 112 | std::vector mfGridElementWidthInv; 113 | std::vector mfGridElementHeightInv; 114 | // a grid for each camera, thus 3 vectors 115 | std::vector > > > mGrids; 116 | 117 | // Current and Next multi frame id 118 | static long unsigned int nNextId; 119 | long unsigned int mnId; 120 | 121 | cMultiKeyFrame* mpReferenceKF; 122 | 123 | void ComputeBoW(); 124 | 125 | void UpdatePoseMatrices(); 126 | 127 | // Check if a MapPoint is in the frustum of the camera 128 | // and also fills variables of the MapPoint to be used by the tracking 129 | bool isInFrustum(int cam, cMapPoint* pMP, double viewingCosLimit); 130 | 131 | // Compute the cell of a keypoint (return false if outside the grid) 132 | bool PosInGrid(const int& cam, cv::KeyPoint &kp, int &posX, int &posY); 133 | 134 | std::vector GetFeaturesInArea(const int& cam, 135 | const double &x, const double &y, 136 | const double &r, 137 | const int minLevel = -1, const int maxLevel = -1) const; 138 | 139 | // Scale Pyramid Info 140 | int mnScaleLevels; 141 | double mfScaleFactor; 142 | std::vector mvScaleFactors; 143 | std::vector mvLevelSigma2; 144 | std::vector mvInvLevelSigma2; 145 | 146 | // Undistorted Image Bounds (computed once) 147 | static std::vector mnMinX; 148 | static std::vector mnMaxX; 149 | static std::vector mnMinY; 150 | static std::vector mnMaxY; 151 | 152 | static bool mbInitialComputations; 153 | 154 | // this variable holds the mapping between keypoint ID and camera 155 | // it was observed in 156 | // [key_id : cam_id] 157 | std::unordered_map keypoint_to_cam; 158 | // this variable holds the mapping between the continous indexing of all 159 | // descriptors and keypoints and the image wise indexes 160 | // it was observed in 161 | // [cont_id : local_image_id] 162 | std::unordered_map cont_idx_to_local_cam_idx; 163 | 164 | cv::Matx GetPose() { return camSystem.Get_M_t(); } 165 | cv::Matx GetPoseMin() { return camSystem.Get_M_t_min(); } 166 | 167 | void SetPose(cv::Matx44d& T) { camSystem.Set_M_t(T); } 168 | void SetPoseMin(cv::Matx61d& Tmin) { camSystem.Set_M_t_from_min(Tmin); } 169 | 170 | bool HavingMasks() { return masksLearned; } 171 | int DescDims() { return descDimension; } 172 | int Doing_mdBRIEF() { return mdBRIEF; } 173 | int GetImgCnt() { return imgCnt; } 174 | 175 | private: 176 | bool mdBRIEF; 177 | bool masksLearned; 178 | int descDimension; 179 | int imgCnt; 180 | }; 181 | } 182 | #endif // MULTIFRAME_H 183 | -------------------------------------------------------------------------------- /include/g2o_MultiCol_vertices_edges.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * 4 | * Copyright (C) 2015-2016 Steffen Urban 5 | * For more information see 6 | * 7 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * MultiCol-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with MultiCol-SLAM . If not, see . 19 | */ 20 | #ifndef G2O_MULTICOL_VERTICES_EDGES_H 21 | #define G2O_MULTICOL_VERTICES_EDGES_H 22 | 23 | #include 24 | #include 25 | 26 | #include "g2o/types/types_seven_dof_expmap.h" 27 | #include "g2o/core/base_multi_edge.h" 28 | #include 29 | 30 | #include "misc.h" 31 | #include "cam_model_omni.h" 32 | 33 | namespace MultiColSLAM 34 | { 35 | void mcsJacs1(const cv::Vec3d& pt3, 36 | const cv::Matx61d& M_t, 37 | const cv::Matx61d& M_c, 38 | const Eigen::Matrix& camModelData, 39 | cv::Matx& jacs); 40 | 41 | class VertexOmniCameraParameters : public g2o::BaseVertex<5 + 12, Eigen::Matrix> 42 | { 43 | public: 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 45 | VertexOmniCameraParameters() {} 46 | 47 | VertexOmniCameraParameters(cCamModelGeneral_ camModel_) 48 | : camModel(camModel_) 49 | { 50 | 51 | } 52 | 53 | virtual void setToOriginImpl() 54 | { 55 | Eigen::Matrix tmpEstimate = camModel.toVector(); 56 | _estimate = tmpEstimate; 57 | } 58 | virtual void oplusImpl(const double* update_) 59 | { 60 | Eigen::Map> update(update_); 61 | // update function x + dx, it is overloaded to update the camera parameters 62 | // see cam_model_omni.h 63 | setEstimate(estimate() + update); 64 | camModel.fromVector(_estimate); 65 | } 66 | 67 | bool read(std::istream& is) 68 | { 69 | std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl; 70 | return false; 71 | } 72 | 73 | bool write(std::ostream& os) const 74 | { 75 | std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl; 76 | return false; 77 | } 78 | cCamModelGeneral_ camModel; 79 | }; 80 | /** 81 | * \brief 82 | */ 83 | class VertexMt_cayley : public g2o::BaseVertex<6, cv::Matx61d>{ 84 | public: 85 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 86 | 87 | VertexMt_cayley() {} 88 | 89 | virtual void setToOriginImpl() 90 | { 91 | _estimate = cv::Matx61d(0, 0, 0, 0, 0, 0); 92 | } 93 | virtual void oplusImpl(const double* update_) 94 | { 95 | //Eigen::Map update(update_); 96 | cv::Matx61d update(update_); 97 | setEstimate(update + estimate()); // update function x + dx 98 | } 99 | 100 | bool read(std::istream& is) 101 | { 102 | std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl; 103 | return false; 104 | } 105 | 106 | bool write(std::ostream& os) const 107 | { 108 | std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl; 109 | return false; 110 | } 111 | 112 | }; 113 | 114 | /** 115 | * \brief 116 | */ 117 | class VertexMc_cayley : public g2o::BaseVertex<6, cv::Matx61d>{ 118 | public: 119 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 120 | 121 | VertexMc_cayley() {} 122 | 123 | virtual void setToOriginImpl() 124 | { 125 | _estimate = cv::Matx61d(0, 0, 0, 0, 0, 0); 126 | } 127 | virtual void oplusImpl(const double* update_) 128 | { 129 | cv::Matx61d update(update_); 130 | setEstimate(update + estimate()); // update function x + dx 131 | } 132 | 133 | bool read(std::istream& is) 134 | { 135 | std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl; 136 | return false; 137 | } 138 | 139 | bool write(std::ostream& os) const 140 | { 141 | std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl; 142 | return false; 143 | } 144 | 145 | }; 146 | 147 | /** 148 | * \brief Point vertex, XYZ 149 | */ 150 | class VertexPointXYZ : public g2o::BaseVertex<3, cv::Vec3d> 151 | { 152 | public: 153 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 154 | VertexPointXYZ() {} 155 | 156 | virtual void setToOriginImpl() 157 | { 158 | _estimate = cv::Vec3d(0, 0, 0); 159 | } 160 | 161 | virtual void oplusImpl(const double* update) 162 | { 163 | cv::Vec3d v(update); 164 | _estimate += v; 165 | } 166 | 167 | virtual bool read(std::istream& /*is*/) 168 | { 169 | std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl; 170 | return false; 171 | } 172 | 173 | virtual bool write(std::ostream& /*os*/) const 174 | { 175 | std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl; 176 | return false; 177 | } 178 | void SetID(int id) { ptID = id; } 179 | int GetID() { return ptID; } 180 | int ptID; // need this id for the Sim3 optimization 181 | }; 182 | 183 | 184 | // edge, that projects a point to a multi camera system 185 | // vertex 0 : t -> Mt, which is the MCS pose 186 | // vertex 1 : i -> 3D point 187 | // vertex 2 : c -> Mc, transformation from MCS to camera 188 | // vertex 3 : c -> interior orientation 189 | class EdgeProjectXYZ2MCS : public g2o::BaseMultiEdge<2, g2o::Vector2D> 190 | { 191 | public: 192 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 193 | 194 | EdgeProjectXYZ2MCS() 195 | { 196 | information().setIdentity(); 197 | resize(4); 198 | } 199 | 200 | virtual bool read(std::istream& /*is*/) 201 | { 202 | std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl; 203 | return false; 204 | } 205 | 206 | virtual bool write(std::ostream& /*os*/) const 207 | { 208 | std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl; 209 | return false; 210 | } 211 | 212 | void computeError(); 213 | 214 | void linearizeOplus(); 215 | }; 216 | 217 | 218 | 219 | } 220 | #endif -------------------------------------------------------------------------------- /include/misc.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * 4 | * Copyright (C) 2015-2016 Steffen Urban 5 | * For more information see 6 | * 7 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * MultiCol-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with MultiCol-SLAM . If not, see . 19 | */ 20 | #ifndef MISC_H 21 | #define MISC_H 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace MultiColSLAM 30 | { 31 | // some misc defines 32 | #ifndef M_PID 33 | #define M_PID 3.1415926535897932384626433832795028841971693993 34 | #endif 35 | 36 | #ifndef M_PIf 37 | #define M_PIf 3.1415926535897932384626f 38 | #endif 39 | 40 | const double RHOd = 180.0 / M_PID; 41 | const float RHOf = 180.0f / M_PIf; 42 | 43 | typedef std::chrono::high_resolution_clock HResClk; 44 | 45 | const float pi2 = M_PIf; 46 | const float pi32 = 2.0 * M_PIf; 47 | const float deg2radf = M_PIf / 180.0f; 48 | const float rad2degf = 180.0f / M_PIf; 49 | const float pihalf = M_PIf / 2.0f; 50 | 51 | cv::Vec3d triangulate_point( 52 | const cv::Matx31d& relOri_t12, 53 | const cv::Matx33d& relOri_R12, 54 | const cv::Vec3d& v1, 55 | const cv::Vec3d& v2); 56 | 57 | 58 | inline cv::Matx33d Skew(const cv::Vec3d& v) 59 | { 60 | return cv::Matx33d( 61 | 0.0, -v(2), v(1), 62 | v(2), 0.0, -v(0), 63 | -v(1), v(0), 0.0); 64 | } 65 | 66 | /** 67 | * Returns time in milliseconds in double 68 | * 69 | * @param start start time 70 | * @param end end time 71 | * 72 | * @return time in milliseconds 73 | */ 74 | double T_in_ms(HResClk::time_point start, 75 | HResClk::time_point end); 76 | /** 77 | * Returns time in nanoseconds in double 78 | * 79 | * @param start start time 80 | * @param end end time 81 | * 82 | * @return time in nanoseconds 83 | */ 84 | double T_in_ns(HResClk::time_point start, 85 | HResClk::time_point end); 86 | 87 | 88 | /** 89 | * median 90 | * 91 | * @param data vector of elements 92 | * 93 | * @return median value 94 | */ 95 | template 96 | T median(std::vector &v) 97 | { 98 | if (v.size() > 0) 99 | { 100 | std::nth_element(v.begin(), v.begin() + v.size() / 2, v.end()); 101 | return v[v.size() / 2]; 102 | } 103 | else return T(0); 104 | } 105 | 106 | /** 107 | * evaluate a polynom using Horner 108 | * 109 | * @param coeffs T* of coefficients 110 | * @param s number of polynomial coefficients 111 | * @param x x value to evaluate 112 | * 113 | * @return function value 114 | */ 115 | inline double horner( 116 | const double* coeffs, const int& s, const double& x) 117 | { 118 | double res = 0.0; 119 | for (int i = s - 1; i >= 0; i--) 120 | res = res * x + coeffs[i]; 121 | return res; 122 | } 123 | 124 | 125 | /** 126 | * Cayley representation to 3x3 rotation matrix 127 | * 128 | * @param cayParamIn Cayley parameter 129 | * 130 | * @return rotation matrix 131 | */ 132 | template 133 | cv::Matx cayley2rot(const cv::Matx& cayParamIn) 134 | { 135 | cv::Matx R = cv::Matx::eye(); 136 | 137 | T c1 = cayParamIn(0, 0); 138 | T c2 = cayParamIn(1, 0); 139 | T c3 = cayParamIn(2, 0); 140 | 141 | T c1sqr = c1*c1; 142 | T c2sqr = c2*c2; 143 | T c3sqr = c3*c3; 144 | 145 | T scale = T(1) + c1sqr + c2sqr + c3sqr; 146 | 147 | R(0, 0) = 1 + c1sqr - c2sqr - c3sqr; 148 | R(0, 1) = 2 * (c1*c2 - c3); 149 | R(0, 2) = 2 * (c1*c3 + c2); 150 | R(1, 0) = 2 * (c1*c2 + c3); 151 | R(1, 1) = 1 - c1sqr + c2sqr - c3sqr; 152 | R(1, 2) = 2 * (c2*c3 - c1); 153 | R(2, 0) = 2 * (c1*c3 - c2); 154 | R(2, 1) = 2 * (c2*c3 + c1); 155 | R(2, 2) = 1 - c1sqr - c2sqr + c3sqr; 156 | 157 | R = (1 / scale) * R; 158 | 159 | return R; 160 | } 161 | 162 | 163 | /** 164 | * 3x3 rotation matrix to Cayley representation 165 | * 166 | * @param R rotation matrix 167 | * 168 | * @return Cayley parameters 169 | */ 170 | 171 | template 172 | cv::Matx rot2cayley(const cv::Matx& R) 173 | { 174 | cv::Matx eyeM = cv::Matx::eye(); 175 | 176 | cv::Matx C1 = R - eyeM; 177 | cv::Matx C2 = R + eyeM; 178 | cv::Matx C = C1 * C2.inv(); 179 | 180 | cv::Matx cayley(-C(1, 2), C(0, 2), -C(0, 1)); 181 | 182 | return cayley; 183 | } 184 | 185 | /** 186 | * 4x4 homogeneous transformation matrix to Cayley + translation representation 187 | * 188 | * @param T 4x4 homogeneous transformation matrix 189 | * 190 | * @return c 6x1 Cayley parameters and translation 191 | */ 192 | template 193 | cv::Matx hom2cayley(const cv::Matx& M) 194 | { 195 | cv::Matx R(M(0, 0), M(0, 1), M(0, 2), 196 | M(1, 0), M(1, 1), M(1, 2), 197 | M(2, 0), M(2, 1), M(2, 2)); 198 | cv::Matx C = rot2cayley(R); 199 | 200 | return cv::Matx(C(0, 0), C(1, 0), C(2, 0), 201 | M(0, 3), M(1, 3), M(2, 3)); 202 | } 203 | 204 | /** 205 | * 6x1 minimal homogeneous transformation vector to homogeneous 4x4 transformation matrix 206 | * 207 | * @param c 6x1 Cayley parameters and translation 208 | * 209 | * @return T 4x4 homogeneous transformation matrix 210 | */ 211 | template 212 | cv::Matx cayley2hom(const cv::Matx& cayleyRep) 213 | { 214 | cv::Matx cayleyR(cayleyRep(0, 0), cayleyRep(1, 0), cayleyRep(2, 0)); 215 | cv::Matx R = cayley2rot(cayleyR); 216 | 217 | cv::Matx homM( 218 | R(0, 0), R(0, 1), R(0, 2), cayleyRep(3, 0), 219 | R(1, 0), R(1, 1), R(1, 2), cayleyRep(4, 0), 220 | R(2, 0), R(2, 1), R(2, 2), cayleyRep(5, 0), 221 | T(0), T(0), T(0), T(1)); 222 | 223 | return homM; 224 | } 225 | 226 | bool CheckDistEpipolarLine(const cv::Vec3d &ray1, 227 | const cv::Vec3d &ray2, 228 | const cv::Matx33d &E12, 229 | const double& thresh); 230 | 231 | cv::Matx33d ComputeE(const cv::Matx44d& T1, const cv::Matx44d& T2); 232 | 233 | inline cv::Matx33d ComputeE(const cv::Matx44d& Trel) 234 | { 235 | cv::Matx33d R = Trel.get_minor<3, 3>(0, 0); 236 | cv::Vec3d t = cv::Vec3d(Trel(0, 3), Trel(1, 3), Trel(2, 3)); 237 | t /= cv::norm(t); 238 | cv::Matx33d t12x = Skew(t); 239 | 240 | return t12x*R; 241 | } 242 | } 243 | #endif -------------------------------------------------------------------------------- /include/cam_system_omni.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * 4 | * Copyright (C) 2015-2016 Steffen Urban 5 | * For more information see 6 | * 7 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * MultiCol-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with MultiCol-SLAM . If not, see . 19 | */ 20 | 21 | /* 22 | multi_cam_system_omni.h 23 | 24 | @brief: 25 | This class represents arbitrary, rigidly coupled multi-(fisheye) camera systems 26 | It holds the calibration matrices M_c and N attached cameras 27 | 28 | See also: 29 | MultiCol Bundle Adjustment: 30 | A Generic Method for Pose Estimation, Simultaneous Self-Calibration 31 | and Reconstruction for Arbitrary Multi-Camera Systems 32 | http://link.springer.com/article/10.1007/s11263-016-0935-0 33 | 34 | ToDo: 35 | Author: Steffen Urban 36 | Date: 23.08.2016 37 | */ 38 | 39 | #ifndef MULTI_CAMERA_SYSTEM_H 40 | #define MULTI_CAMERA_SYSTEM_H 41 | 42 | // external includes 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | // own includes 49 | #include "misc.h" 50 | #include "cam_model_omni.h" 51 | #include "cConverter.h" 52 | namespace MultiColSLAM 53 | { 54 | class cMultiCamSys_ 55 | { 56 | public: 57 | cMultiCamSys_() : nrCams(0), flagMcMt(false), 58 | M_t(cv::Matx44d::eye()), M_t_min(cv::Matx61d::zeros()){} 59 | 60 | // constructor initalizes MCS pose and MCS calibration 61 | cMultiCamSys_(cv::Matx M_t_, 62 | std::vector> M_c_, 63 | std::vector camModels_) : 64 | M_t(M_t_), M_c(M_c_), camModels(camModels_), flagMcMt(true) 65 | { 66 | nrCams = (int)M_c_.size(); 67 | M_t_min = hom2cayley(M_t); 68 | //M_t_min = hom2rodrigues(M_t); 69 | M_t_inv = cConverter::invMat(M_t); 70 | 71 | MtMc = std::vector>(nrCams); 72 | MtMc_inv = std::vector>(nrCams); 73 | for (int c = 0; c < nrCams; ++c) 74 | { 75 | MtMc[c] = M_t_ * M_c_[c]; 76 | MtMc_inv[c] = cConverter::invMat(MtMc[c]); 77 | M_c_min.push_back(hom2cayley(M_c[c])); 78 | //M_c_min.push_back(hom2rodrigues(M_c[c])); 79 | // opengv conversion 80 | opengv::rotation_t R; 81 | opengv::translation_t t; 82 | cv::Mat Rcv(M_c[c].get_minor<3, 3>(0, 0)); 83 | cv::cv2eigen(Rcv, R); 84 | cv::Mat_ tcv = 85 | (cv::Mat_(3, 1) << M_c[c](0, 3), M_c[c](1, 3), M_c[c](2, 3)); 86 | cv::cv2eigen(tcv, t); 87 | this->camRotations.push_back(R); 88 | this->camOffsets.push_back(t); 89 | } 90 | } 91 | 92 | void WorldToCam(int c, 93 | cv::Point3_& pt3, 94 | cv::Point_& pt2); 95 | 96 | void WorldToCam(int c, 97 | cv::Point3_& pt3, 98 | cv::Vec2d& pt2); 99 | 100 | void WorldToCamHom(int c, 101 | cv::Vec& pt4, 102 | cv::Vec2d& pt2); 103 | 104 | bool WorldToCamHom_fast(int c, 105 | cv::Vec& pt4, 106 | cv::Vec& pt2); 107 | 108 | void WorldToCamHom_fast(int c, 109 | cv::Vec& pt3, 110 | cv::Vec& pt2); 111 | 112 | void CamToWorld_ogv(int c, 113 | opengv::bearingVector_t& bearingV, 114 | cv::Point_ pt2); 115 | 116 | void CamToWorld(int c, cv::Point3_& pt3, cv::Point_& pt2); 117 | void CamToWorld(int c, cv::Vec& pt3, cv::Point_& pt2); 118 | 119 | // set pose 120 | void Set_M_t_from_min(cv::Matx M_t_minRep); 121 | void Set_M_t(cv::Matx M_t_); 122 | 123 | // set calibration 124 | void Set_M_c_from_min(int c, cv::Matx M_c_minRep); 125 | void Set_M_c(int c, cv::Matx M_c_); 126 | void Set_All_M_c(std::vector> M_c_); 127 | void Set_All_M_c_from_min(std::vector> M_c_min_); 128 | 129 | 130 | // add MCS camera pose 131 | // and set the last one to the actual pose 132 | void Add_M_c(cv::Matx M_c_); 133 | void Add_M_c_from_min(cv::Matx M_c_min_); 134 | 135 | void Add_M_c_from_min_and_IO(cv::Matx M_c_min_, 136 | cCamModelGeneral_ camM); 137 | 138 | void Set_M_c_from_min_and_IO(int c, cv::Matx M_c_min_, cCamModelGeneral_ camM); 139 | 140 | void Set_IO(int c, cCamModelGeneral_ camM) { camModels[c] = camM; } 141 | void Set_IOs(std::vector camModels_) { camModels = camModels_; } 142 | 143 | // get functions 144 | int GetNrCams() { return static_cast(camModels.size()); } 145 | 146 | cCamModelGeneral_ GetCamModelObj(int c) { return camModels[c]; } 147 | cv::Matx Get_M_t_min() { return M_t_min; } 148 | 149 | cv::Matx Get_M_c_min(int c) { return M_c_min[c]; } 150 | 151 | cv::Matx Get_M_t() { return M_t; } 152 | cv::Matx Get_M_c(int c) { return M_c[c]; } 153 | 154 | Eigen::Matrix3d Get_R_c_ogv(int c) { return camRotations[c]; } 155 | Eigen::Vector3d Get_t_c_ogv(int c) { return camOffsets[c]; } 156 | 157 | opengv::rotations_t Get_All_R_c_ogv() { return camRotations; } 158 | opengv::translations_t Get_All_t_c_ogv() { return camOffsets; } 159 | 160 | std::vector> Get_All_M_c() { return M_c; } 161 | 162 | cv::Matx Get_MtMc(int cam) 163 | { 164 | if (!flagMcMt) 165 | return (M_t*M_c[cam]); 166 | else 167 | return MtMc[cam]; 168 | } 169 | 170 | cv::Matx Get_MtMc_inv(int cam) 171 | { 172 | if (!flagMcMt) 173 | return cConverter::invMat(M_t*M_c[cam]); 174 | else 175 | return MtMc_inv[cam]; 176 | } 177 | 178 | private: 179 | int nrCams; 180 | 181 | // current camera pose 182 | cv::Matx M_t; // MCS pose 183 | cv::Matx M_t_inv; // inverse MCS pose 184 | cv::Matx M_t_min; // MCS pose cayley 185 | 186 | // opencv 187 | std::vector> M_c; // MCS calibration data 188 | std::vector> M_c_min; // MCS calibration data as cayley rep 189 | // for opengv 190 | opengv::rotations_t camRotations; 191 | opengv::translations_t camOffsets; 192 | 193 | std::vector camModels; // specific camera model 194 | 195 | // transformation (MtMc)^-1 (Mc^-1*Mt^-1) at all time 196 | std::vector > MtMc; 197 | std::vector > MtMc_inv; 198 | 199 | bool flagMcMt; 200 | }; 201 | 202 | 203 | 204 | } 205 | #endif 206 | 207 | 208 | 209 | 210 | -------------------------------------------------------------------------------- /src/cam_model_omni.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * 4 | * Copyright (C) 2015-2016 Steffen Urban 5 | * For more information see 6 | * 7 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * MultiCol-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with MultiCol-SLAM . If not, see . 19 | */ 20 | #include "cam_model_omni.h" 21 | // own includes 22 | #include "misc.h" 23 | 24 | namespace MultiColSLAM 25 | { 26 | using namespace cv; 27 | using namespace std; 28 | 29 | void cCamModelGeneral_::ImgToWorld(cv::Point3_& X, // 3D scene point 30 | const cv::Point_& m) // 2D image point 31 | { 32 | //double invAff = c - d*e; 33 | const double u_t = m.x - u0; 34 | const double v_t = m.y - v0; 35 | // inverse affine matrix image to sensor plane conversion 36 | X.x = (1 * u_t - d * v_t) / this->invAffine; 37 | X.y = (-e * u_t + c * v_t) / this->invAffine; 38 | const double X2 = X.x * X.x; 39 | const double Y2 = X.y * X.y; 40 | X.z = -horner((double*)p.data, p_deg, sqrt(X2 + Y2)); 41 | 42 | // normalize vectors spherically 43 | const double norm = sqrt(X2 + Y2 + X.z*X.z); 44 | X.x /= norm; 45 | X.y /= norm; 46 | X.z /= norm; 47 | } 48 | 49 | void cCamModelGeneral_::ImgToWorld(double& x, double& y, double& z, // 3D scene point 50 | const double& u, const double& v) // 2D image point 51 | { 52 | //double invAff = c - d*e; 53 | const double u_t = u - u0; 54 | const double v_t = v - v0; 55 | // inverse affine matrix image to sensor plane conversion 56 | x = (u_t - d * v_t) / this->invAffine; 57 | y = (-e * u_t + c * v_t) / this->invAffine; 58 | const double X2 = x * x; 59 | const double Y2 = y * y; 60 | z = -horner((double*)p.data, p_deg, sqrt(X2 + Y2)); 61 | 62 | // normalize vectors spherically 63 | double norm = sqrt(X2 + Y2 + z*z); 64 | x /= norm; 65 | y /= norm; 66 | z /= norm; 67 | } 68 | 69 | void cCamModelGeneral_::ImgToWorld(cv::Vec3d& X, // 3D scene point 70 | const cv::Vec2d& m) // 2D image point 71 | { 72 | //double invAff = c - d*e; 73 | const double u_t = m(0) - u0; 74 | const double v_t = m(1) - v0; 75 | // inverse affine matrix image to sensor plane conversion 76 | X(0) = (u_t - d * v_t) / this->invAffine; 77 | X(1) = (-e * u_t + c * v_t) / this->invAffine; 78 | const double X2 = X(0) * X(0); 79 | const double Y2 = X(1) * X(1); 80 | X(2) = -horner((double*)p.data, p_deg, sqrt(X2 + Y2)); 81 | 82 | // normalize vectors spherically 83 | double norm = sqrt(X2 + Y2 + X(2)*X(2)); 84 | X(0) /= norm; 85 | X(1) /= norm; 86 | X(2) /= norm; 87 | } 88 | 89 | 90 | void cCamModelGeneral_::WorldToImg(const cv::Point3_& X, // 3D scene point 91 | cv::Point_& m) // 2D image point 92 | { 93 | double norm = sqrt(X.x*X.x + X.y*X.y); 94 | 95 | if (norm == 0.0) 96 | norm = 1e-14; 97 | 98 | const double theta = atan(-X.z / norm); 99 | const double rho = horner((double*)invP.data, invP_deg, theta); 100 | 101 | const double uu = X.x / norm * rho; 102 | const double vv = X.y / norm * rho; 103 | 104 | m.x = uu*c + vv*d + u0; 105 | m.y = uu*e + vv + v0; 106 | } 107 | 108 | void cCamModelGeneral_::WorldToImg(const cv::Vec3d& X, // 3D scene point 109 | cv::Vec2d& m) // 2D image point 110 | { 111 | 112 | double norm = cv::sqrt(X(0)*X(0) + X(1)*X(1)); 113 | 114 | if (norm == 0.0) 115 | norm = 1e-14; 116 | 117 | const double theta = atan(-X(2) / norm); 118 | const double rho = horner((double*)invP.data, invP_deg, theta); 119 | 120 | const double uu = X(0) / norm * rho; 121 | const double vv = X(1) / norm * rho; 122 | 123 | m(0) = uu*c + vv*d + u0; 124 | m(1) = uu*e + vv + v0; 125 | } 126 | 127 | void cCamModelGeneral_::WorldToImg(const cv::Vec3d& X, // 3D scene point 128 | cv::Vec2f& m) // 2D image point 129 | { 130 | double norm = cv::sqrt(X(0)*X(0) + X(1)*X(1)); 131 | 132 | if (norm == 0.0) 133 | norm = 1e-14; 134 | 135 | const double theta = atan(-X(2) / norm); 136 | 137 | const double rho = horner((double*)invP.data, invP_deg, theta); 138 | 139 | const double uu = X(0) / norm * rho; 140 | const double vv = X(1) / norm * rho; 141 | 142 | m(0) = uu*c + vv*d + u0; 143 | m(1) = uu*e + vv + v0; 144 | } 145 | 146 | void cCamModelGeneral_::WorldToImg(const double& x, const double& y, const double& z, // 3D scene point 147 | double& u, double& v) const // 2D image point 148 | { 149 | double norm = sqrt(x*x + y*y); 150 | if (norm == 0.0) 151 | norm = 1e-14; 152 | 153 | const double theta = atan(-z / norm); 154 | const double rho = horner((double*)invP.data, invP_deg, theta); 155 | 156 | const double uu = x / norm * rho; 157 | const double vv = y / norm * rho; 158 | 159 | u = uu*c + vv*d + u0; 160 | v = uu*e + vv + v0; 161 | } 162 | 163 | bool cCamModelGeneral_::isPointInMirrorMask( 164 | const double& u, 165 | const double& v, 166 | int pyr) 167 | { 168 | const int ur = cvRound(u); 169 | const int vr = cvRound(v); 170 | // check image bounds 171 | if (ur >= mirrorMasks[pyr].cols || ur <= 0 || 172 | vr >= mirrorMasks[pyr].rows || vr <= 0) 173 | return false; 174 | // check mirror 175 | if (mirrorMasks[pyr].ptr(vr)[ur] > 0) 176 | return true; 177 | else return false; 178 | } 179 | 180 | 181 | void CreateMirrorMask(cCamModelGeneral_ camera, 182 | int pyrLevel, 183 | vector& mirror_masks) 184 | { 185 | int w = (int)camera.GetWidth(); 186 | int h = (int)camera.GetHeight(); 187 | float u0 = (float)camera.Get_v0(); 188 | float v0 = (float)camera.Get_u0(); 189 | Mat sizeDef = Mat::zeros(h, w, CV_8UC1); 190 | vector sizeDefvec; 191 | buildPyramid(sizeDef, sizeDefvec, pyrLevel); 192 | // Mirror mask for pyramid 193 | float offset[4] = { 22.0f, 10.0f, 5.0f, 1.0f }; 194 | //float offset[4] = { 50.0f, 50.0f, 50.0f, 50.0f}; 195 | for (int mIdx = 0; mIdx < pyrLevel; mIdx++) 196 | { 197 | if (mIdx != 0) 198 | { 199 | w = sizeDefvec[mIdx].cols; 200 | h = sizeDefvec[mIdx].rows; 201 | 202 | u0 = ceil(u0 / 2.0f); 203 | v0 = ceil(v0 / 2.0f); 204 | } 205 | Mat tempMask = Mat::zeros(h, w, CV_8UC1); 206 | for (int i = 0; i < h; ++i) 207 | { 208 | for (int j = 0; j < w; ++j) 209 | { 210 | float ans = sqrt((float)pow(i - u0, 2) + (float)pow(j - v0, 2)); 211 | if (ans < (u0 + offset[mIdx])) 212 | tempMask.at(i, j) = 255; 213 | else 214 | tempMask.at(i, j) = 0; 215 | } 216 | 217 | } 218 | mirror_masks.push_back(tempMask); 219 | } 220 | } 221 | 222 | } -------------------------------------------------------------------------------- /include/cTracking.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * 4 | * Copyright (C) 2015-2016 Steffen Urban 5 | * For more information see 6 | * 7 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * MultiCol-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with MultiCol-SLAM . If not, see . 19 | */ 20 | 21 | /* 22 | * MultiCol-SLAM is based on ORB-SLAM2 which was also released under GPLv3 23 | * For more information see 24 | * Raúl Mur-Artal (University of Zaragoza) 25 | */ 26 | 27 | #ifndef TRACKING_H 28 | #define TRACKING_H 29 | 30 | 31 | #include "cMultiFramePublisher.h" 32 | #include "cMap.h" 33 | #include "cLocalMapping.h" 34 | #include "cLoopClosing.h" 35 | #include "cMultiFrame.h" 36 | #include "cORBVocabulary.h" 37 | #include "cMultiKeyFrameDatabase.h" 38 | #include "mdBRIEFextractorOct.h" 39 | #include "cMultiInitializer.h" 40 | #include "cMapPublisher.h" 41 | #include "cSystem.h" 42 | #include "cViewer.h" 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | #include 50 | 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | 57 | namespace MultiColSLAM 58 | { 59 | class cMultiFramePublisher; 60 | class cMap; 61 | class cLocalMapping; 62 | class cLoopClosing; 63 | class cSystem; 64 | class cViewer; 65 | 66 | class cTracking 67 | { 68 | 69 | public: 70 | cTracking(cSystem* pSys, 71 | ORBVocabulary* pVoc, 72 | cMultiFramePublisher *pFramePublisher, 73 | cMapPublisher *pMapPublisher, 74 | cMap *pMap, 75 | cMultiKeyFrameDatabase* pKFDB, 76 | cMultiCamSys_ camSystem, 77 | std::string settingsPath_); 78 | 79 | enum eTrackingState 80 | { 81 | SYSTEM_NOT_READY = -1, 82 | NO_IMAGES_YET = 0, 83 | NOT_INITIALIZED = 1, 84 | INITIALIZING = 2, 85 | WORKING = 3, 86 | LOST = 4 87 | }; 88 | 89 | void SetLocalMapper(cLocalMapping* pLocalMapper); 90 | void SetLoopClosing(cLoopClosing* pLoopClosing); 91 | void SetKeyFrameDatabase(cMultiKeyFrameDatabase* pKFDB); 92 | void SetViewer(cViewer* pViewer); 93 | 94 | cv::Matx44d GrabImageSet(const std::vector& imgSet, 95 | const double& timestamp); 96 | 97 | void ForceRelocalisation(); 98 | 99 | eTrackingState mState; 100 | eTrackingState mLastProcessedState; 101 | 102 | // Current Frame 103 | cMultiFrame mCurrentFrame; 104 | 105 | // Initialization Variables 106 | std::vector mvIniLastMatches; 107 | std::vector mvIniMatches; 108 | std::vector mvbPrevMatched; 109 | std::vector mvIniP3D; 110 | cMultiFrame mInitialFrame; 111 | 112 | void CheckResetByPublishers(); 113 | 114 | std::vector GetAllPoses() { return this->allPoses; } 115 | std::vector GetAllPosesBool() { return this->allPosesBool; } 116 | std::vector GetNrTrackedPts() { return nrTrackedPts; } 117 | std::vector GetInlierRatio() { return inlierRatio; } 118 | 119 | std::vector timingFeatureExtraction; 120 | std::vector timingTrackLocalMap; 121 | std::vector timingInitalPoseEst; 122 | 123 | bool CheckFinished(); 124 | void Reset(); 125 | 126 | int GetNrCams(); 127 | protected: 128 | 129 | std::vector> imagePaths; 130 | int nrImages2Track; 131 | std::string imgPath; 132 | int imgCounter; 133 | bool finished; 134 | bool grab; 135 | int numberCameras; 136 | 137 | cv::Matx44d initPose; 138 | double curBaseline2MKF; 139 | bool Track(); 140 | 141 | void FirstInitialization(); 142 | void Initialize(); 143 | void CreateInitialMap(cv::Matx33d &Rcw, 144 | cv::Vec3d &tcw, int leadingCam); 145 | 146 | bool TrackPreviousFrame(); 147 | bool TrackWithMotionModel(); 148 | 149 | bool RelocalisationRequested(); 150 | bool Relocalisation(); 151 | 152 | void UpdateReference(); 153 | void UpdateReferencePoints(); 154 | void UpdateReferenceKeyFrames(); 155 | 156 | bool TrackLocalMap(); 157 | int SearchReferencePointsInFrustum(); 158 | 159 | bool NeedNewKeyFrame(); 160 | void CreateNewKeyFrame(); 161 | void CountNumberTrackedPointsPerCam(); 162 | std::vector nbTrackedPtsInCam; 163 | std::vector nbTrackedRatios; 164 | 165 | //Other Thread Pointers 166 | cLocalMapping* mpLocalMapper; 167 | cLoopClosing* mpLoopClosing; 168 | cSystem* mpSystem; 169 | 170 | // mdBRIEF with octree 171 | std::vector mp_mdBRIEF_extractorOct; 172 | std::vector mp_mdBRIEF_init_extractorOct; 173 | 174 | //BoW 175 | ORBVocabulary* mpORBVocabulary; 176 | cMultiKeyFrameDatabase* mpKeyFrameDB; 177 | 178 | // Initalization 179 | cMultiInitializer* mpInitializer; 180 | 181 | //Local Map 182 | cMultiKeyFrame* mpReferenceKF; 183 | std::vector mvpLocalKeyFrames; 184 | std::vector mvpLocalKeyFramesCovWeights; 185 | std::vector mvpLocalKeyFramesDistance2Frame; 186 | std::vector mvpLocalMapPoints; 187 | 188 | //Publishers 189 | cMultiFramePublisher* mpFramePublisher; 190 | cMapPublisher* mpMapPublisher; 191 | cViewer* mpViewer; 192 | 193 | //Map 194 | cMap* mpMap; 195 | 196 | // camera system class 197 | cMultiCamSys_ camSystem; 198 | 199 | //New KeyFrame rules (according to fps) 200 | int mMinFrames; 201 | int mMaxFrames; 202 | 203 | //Current matches in frame 204 | int mnMatchesInliers; 205 | 206 | //Last Frame, KeyFrame and Relocalisation Info 207 | cMultiKeyFrame* mpLastKeyFrame; 208 | cMultiFrame mLastFrame; 209 | unsigned int mnLastKeyFrameId; 210 | unsigned int mnLastRelocFrameId; 211 | 212 | //Mutex 213 | std::mutex mMutexTrack; 214 | std::mutex mMutexForceRelocalisation; 215 | 216 | //Reset 217 | bool mbPublisherStopped; 218 | bool mbReseting; 219 | std::mutex mMutexReset; 220 | 221 | //Is relocalisation requested by an external thread? (loop closing) 222 | bool mbForceRelocalisation; 223 | 224 | //Motion Model 225 | bool mbMotionModel; 226 | cv::Matx44d mVelocity; 227 | 228 | //Color order (true RGB, false BGR, ignored if grayscale) 229 | bool mbRGB; 230 | 231 | string settingsPath; 232 | 233 | // evaluation 234 | std::vector allPoses; 235 | std::vector allPosesBool; 236 | std::vector nrTrackedPts; 237 | std::vector inlierRatio; 238 | // features 239 | bool use_mdBRIEF; 240 | bool loopAndMapperSet; 241 | }; 242 | } 243 | #endif // TRACKING_H 244 | -------------------------------------------------------------------------------- /include/cam_model_omni.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * 4 | * Copyright (C) 2015-2016 Steffen Urban 5 | * For more information see 6 | * 7 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * MultiCol-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with MultiCol-SLAM . If not, see . 19 | */ 20 | 21 | /* 22 | cam_model_omni.h 23 | 24 | @brief: 25 | This class implements Scaramuzzas general atan camera model. 26 | Calibrated with: https://github.com/urbste/ImprovedOcamCalib 27 | 28 | ToDo: 29 | 30 | Author: Steffen Urban 31 | Date: 23.08.2016 32 | */ 33 | 34 | #ifndef CAM_MODEL_GENERAL_H 35 | #define CAM_MODEL_GENERAL_H 36 | 37 | // extern includes 38 | #include 39 | #include 40 | #include 41 | 42 | 43 | namespace MultiColSLAM 44 | { 45 | class cCamModelGeneral_ 46 | { 47 | public: 48 | // construtors 49 | cCamModelGeneral_() : 50 | c(1), 51 | d(0), 52 | e(0), 53 | u0(0), 54 | v0(0), 55 | p((cv::Mat_(1, 1) << 1)), 56 | invP((cv::Mat_(1, 1) << 1)), 57 | p_deg(1), 58 | invP_deg(1), 59 | Iwidth(0), 60 | Iheight(0), 61 | p1(1) 62 | {} 63 | 64 | cCamModelGeneral_(double cdeu0v0[], cv::Mat_ p_, cv::Mat_ invP_) : 65 | c(cdeu0v0[0]), 66 | d(cdeu0v0[1]), 67 | e(cdeu0v0[2]), 68 | u0(cdeu0v0[3]), 69 | v0(cdeu0v0[4]), 70 | p(p_), 71 | invP(invP_) 72 | { 73 | // initialize degree of polynomials 74 | p_deg = (p_.rows > 1) ? p_.rows : p_deg = p_.cols; 75 | invP_deg = (p_.rows > 1) ? invP_deg = invP_.rows : invP_deg = invP_.cols; 76 | 77 | cde1 = (cv::Mat_(2, 2) << c, d, e, 1.0); 78 | p1 = p.at(0); 79 | invAffine = c - d*e; 80 | } 81 | 82 | cCamModelGeneral_(double cdeu0v0[], 83 | cv::Mat_ p_, cv::Mat_ invP_, 84 | double Iw_, double Ih_) : 85 | c(cdeu0v0[0]), 86 | d(cdeu0v0[1]), 87 | e(cdeu0v0[2]), 88 | u0(cdeu0v0[3]), 89 | v0(cdeu0v0[4]), 90 | p(p_), 91 | invP(invP_), 92 | Iwidth(Iw_), 93 | Iheight(Ih_) 94 | { 95 | // initialize degree of polynomials 96 | p_deg = (p_.rows > 1) ? p_.rows : p_deg = p_.cols; 97 | invP_deg = (p_.rows > 1) ? invP_deg = invP_.rows : invP_deg = invP_.cols; 98 | 99 | cde1 = (cv::Mat_(2, 2) << c, d, e, 1.0); 100 | p1 = p.at(0); 101 | invAffine = c - d*e; 102 | } 103 | 104 | ~cCamModelGeneral_(){} 105 | 106 | void WorldToImg(const double& x, const double& y, const double& z, // 3D scene point 107 | double& u, double& v) const; 108 | 109 | void WorldToImg(const cv::Point3_& X, // 3D scene point 110 | cv::Point_& m); 111 | 112 | void WorldToImg(const cv::Vec3d& X, // 3D scene point 113 | cv::Vec2d& m); 114 | 115 | void WorldToImg(const cv::Vec3d& X, // 3D scene point 116 | cv::Vec2f& m); 117 | 118 | void ImgToWorld(double& x, double& y, double& z, // 3D scene point 119 | const double& u, const double& v); 120 | 121 | void ImgToWorld(cv::Point3_& X, // 3D scene point 122 | const cv::Point_& m); 123 | 124 | void ImgToWorld(cv::Vec3d& X, // 3D scene point 125 | const cv::Vec2d& m); 126 | 127 | void undistortPointsOcam( 128 | const double& ptx, const double& pty, 129 | const double& undistScaleFactor, 130 | double& out_ptx, double& out_pty) 131 | { 132 | double x = 0.0; 133 | double y = 0.0; 134 | double z = 0.0; 135 | this->ImgToWorld(x, y, z, ptx, pty); 136 | out_ptx = -x / z * undistScaleFactor; 137 | out_pty = -y / z * undistScaleFactor; 138 | } 139 | 140 | void distortPointsOcam( 141 | const double& ptx, const double& pty, 142 | double& dist_ptx, double& dist_pty) 143 | { 144 | WorldToImg(ptx, pty, -p1, dist_ptx, dist_pty); 145 | } 146 | 147 | // get functions 148 | double Get_c() { return c; } 149 | double Get_d() { return d; } 150 | double Get_e() { return e; } 151 | 152 | double Get_u0() { return u0; } 153 | double Get_v0() { return v0; } 154 | 155 | int GetInvDeg() { return invP_deg; } 156 | int GetPolDeg() { return p_deg; } 157 | 158 | cv::Mat_ Get_invP() { return invP; } 159 | cv::Mat_ Get_P() { return p; } 160 | 161 | double GetWidth() { return Iwidth; } 162 | double GetHeight() { return Iheight; } 163 | 164 | cv::Mat GetMirrorMask(int pyrL) { return mirrorMasks[pyrL]; } 165 | void SetMirrorMasks(std::vector mirrorMasks_) { mirrorMasks = mirrorMasks_; } 166 | 167 | bool isPointInMirrorMask(const double& u, const double& v, int pyr); 168 | 169 | 170 | inline double operator [](int i) const 171 | { 172 | assert(i < (12 + 5)); 173 | if (i > 4) 174 | return invP.at(i - 5, 0); 175 | if (i == 0) 176 | return c; 177 | if (i == 1) 178 | return d; 179 | if (i == 2) 180 | return e; 181 | if (i == 3) 182 | return u0; 183 | if (i == 4) 184 | return v0; 185 | } 186 | 187 | // ATTENTION, used fixed size here!! 188 | // this will not work in general!! 189 | inline Eigen::Matrix toVector() const 190 | { 191 | Eigen::Matrix tmp = 192 | Eigen::Matrix::Zero(); 193 | tmp(0, 0) = c; 194 | tmp(1, 0) = d; 195 | tmp(2, 0) = e; 196 | tmp(3, 0) = u0; 197 | tmp(4, 0) = v0; 198 | for (int i = 0; i < invP.rows; ++i) 199 | tmp(5 + i, 0) = invP.at(i, 0); 200 | 201 | return tmp; 202 | } 203 | 204 | inline void fromVector(const Eigen::Matrix tmp) 205 | { 206 | c = tmp(0, 0); 207 | d = tmp(1, 0); 208 | e = tmp(2, 0); 209 | u0 = tmp(3, 0); 210 | v0 = tmp(4, 0); 211 | for (int i = 0; i < invP.rows; ++i) 212 | invP.at(i, 0) = tmp(5 + i, 0); 213 | } 214 | 215 | inline cCamModelGeneral_ operator+ (const Eigen::Matrix& values2add) const 216 | { 217 | cCamModelGeneral_ result(*this); 218 | Eigen::Matrix valuesOld = result.toVector(); 219 | result.fromVector(valuesOld + values2add); 220 | return result; 221 | } 222 | 223 | protected: 224 | 225 | // affin 226 | double c; 227 | double d; 228 | double e; 229 | double invAffine; 230 | cv::Mat_ cde1; 231 | // principal 232 | double u0; 233 | double v0; 234 | // polynomial 235 | double p1; 236 | cv::Mat_ p; 237 | int p_deg; 238 | // inverse polynomial 239 | cv::Mat_ invP; 240 | 241 | int invP_deg; 242 | // image width and height 243 | double Iwidth; 244 | double Iheight; 245 | // mirror mask on pyramid levels 246 | std::vector mirrorMasks; 247 | }; 248 | 249 | 250 | void CreateMirrorMask(cCamModelGeneral_ camera, 251 | int pyrLevel, 252 | std::vector& mirror_masks); 253 | 254 | } 255 | #endif -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This file is part of MultiCol-SLAM 2 | # 3 | # Copyright (C) 2015-2016 Steffen Urban 4 | # For more information see 5 | # 6 | # MultiCol-SLAM is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # MultiCol-SLAM is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU General Public License 17 | # along with MultiCol-SLAM . If not, see . 18 | 19 | project (MultiCol-SLAM) 20 | 21 | IF(NOT CMAKE_BUILD_TYPE) 22 | SET(CMAKE_BUILD_TYPE Release) 23 | ENDIF() 24 | 25 | if (WIN32) 26 | SET (MCSSLAM_LIB_TYPE ) 27 | else() 28 | SET (MCSSLAM_LIB_TYPE SHARED) 29 | endif(WIN32) 30 | 31 | MESSAGE("Build type: " ${CMAKE_BUILD_TYPE}) 32 | 33 | IF(WIN32) 34 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -openmp -W0 -MP") 35 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -openmp -W0 -MP") 36 | ELSE() 37 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fopenmp -O3 -march=native -std=c++11") 38 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp -O3 -march=native -std=c++11") 39 | ENDIF() 40 | 41 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) 42 | 43 | find_package(OpenCV REQUIRED) 44 | find_package(Pangolin REQUIRED) 45 | 46 | # ============================================================================== 47 | # Search OpenGV 48 | # ============================================================================== 49 | #FIND_PACKAGE(OpenGV QUIET HINTS ${OpenGV_DIR_HINTS}) 50 | #IF(NOT OpenGV_FOUND) 51 | SET(OpenGV_INCLUDE_DIR ${PROJECT_SOURCE_DIR}/ThirdParty/OpenGV/include) 52 | SET(OpenGV_LIBRARIES opengv) 53 | MESSAGE("OpenGV include DIR ${OpenGV_INCLUDE_DIR}") 54 | INCLUDE_DIRECTORIES(${OpenGV_INCLUDE_DIR}) 55 | #ENDIF() 56 | 57 | # ============================================================================== 58 | # Search g2o includes 59 | # ============================================================================== 60 | #FIND_PACKAGE(g2o QUIET HINTS ${g2o_DIR_HINTS}) 61 | #IF(NOT g2o_FOUND) 62 | SET(g2o_INCLUDE_DIR ${PROJECT_SOURCE_DIR}/ThirdParty/g2o) 63 | SET(g2o_LIBRARIES g2o) 64 | MESSAGE("g2o include DIR ${g2o_INCLUDE_DIR}") 65 | INCLUDE_DIRECTORIES(${g2o_INCLUDE_DIR}) 66 | #ENDIF() 67 | 68 | # ============================================================================== 69 | # Search Eigen, borrowed from openMVG 70 | # this makes it a lot smoother for Windows 71 | # ============================================================================== 72 | # - internal by default, (Eigen version 3.2.9) 73 | # - external if EIGEN_INCLUDE_DIR_HINTS is defined 74 | # ============================================================================== 75 | IF(NOT DEFINED EIGEN_INCLUDE_DIR_HINTS) 76 | SET(EIGEN_INCLUDE_DIR ${PROJECT_SOURCE_DIR}/ThirdParty/Eigen) 77 | MESSAGE("Eigen include DIR ${EIGEN_INCLUDE_DIR}") 78 | #SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/ThirdParty/Eigen/cmake) 79 | #FIND_PACKAGE(Eigen3 REQUIRED) 80 | #MESSAGE("-- Eigen version ${EIGEN3_VERSION}: ${EIGEN3_INCLUDE_DIR}") 81 | #INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR}) 82 | SET(MultiCol-SLAM_USE_INTERNAL_EIGEN ON) 83 | INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIR}) 84 | ENDIF() 85 | 86 | 87 | SET(MultiColHeaders 88 | include/cam_model_omni.h 89 | include/cam_system_omni.h 90 | include/cConverter.h 91 | include/cMultiFrame.h 92 | include/cMultiFramePublisher.h 93 | include/cMultiKeyFrame.h 94 | include/cConverter.h 95 | include/cMapPoint.h 96 | include/cMap.h 97 | include/cMapPublisher.h 98 | include/cLocalMapping.h 99 | include/cLoopClosing.h 100 | include/cOptimizer.h 101 | include/cORBVocabulary.h 102 | include/cORBmatcher.h 103 | include/cMultiFrame.h 104 | include/cMultiKeyFrameDatabase.h 105 | include/cSim3Solver.h 106 | include/cSystem.h 107 | include/cTracking.h 108 | include/cMultiInitializer.h 109 | include/cViewer.h 110 | include/mdBRIEFextractorOct.h 111 | include/misc.h 112 | #include/MultiCol_cayley_jacobians.h 113 | include/g2o_MultiCol_vertices_edges.h 114 | include/g2o_MultiCol_sim3_expmap.h 115 | ) 116 | 117 | include_directories( 118 | ${PROJECT_SOURCE_DIR} 119 | ${PROJECT_SOURCE_DIR}/include 120 | ${PROJECT_SOURCE_DIR}/ThirdParty 121 | ${PROJECT_SOURCE_DIR}/ThirdParty/g2o 122 | ${PROJECT_SOURCE_DIR}/ThirdParty/OpenGV 123 | ${PROJECT_SOURCE_DIR}/ThirdParty/DBoW2 124 | ${Pangolin_INCLUDE_DIRS} 125 | ) 126 | 127 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) 128 | 129 | add_library(${PROJECT_NAME} ${MCSSLAM_LIB_TYPE} 130 | src/cam_model_omni.cpp 131 | src/cam_system_omni.cpp 132 | src/cConverter.cpp 133 | src/cMultiFrame.cpp 134 | src/cMultiFramePublisher.cpp 135 | src/cMultiKeyFrame.cpp 136 | src/cConverter.cpp 137 | src/cMapPoint.cpp 138 | src/cMap.cpp 139 | src/cMapPublisher.cpp 140 | src/cOptimizer.cpp 141 | src/cOptimizerLoopStuff.cpp 142 | src/cMultiFrame.cpp 143 | src/cMultiKeyFrameDatabase.cpp 144 | src/cSim3Solver.cpp 145 | src/cSystem.cpp 146 | src/cMultiInitializer.cpp 147 | src/mdBRIEFextractorOct.cpp 148 | src/cTracking.cpp 149 | src/cLocalMapping.cpp 150 | src/cLoopClosing.cpp 151 | src/cViewer.cpp 152 | src/cORBmatcher.cpp 153 | src/g2o_MultiCol_sim3_expmap.cpp 154 | src/g2o_MultiCol_vertices_edges.cpp 155 | src/misc.cpp 156 | ${MultiColHeaders} 157 | ) 158 | 159 | IF(WIN32) 160 | SET (G2OLIBS_DEBUG ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/bin/Debug/g2o.lib) 161 | SET (DBOW2LIBS_DEBUG ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/Debug/DBoW2.lib) 162 | SET (OPENGVLIBS_DEBUG ${PROJECT_SOURCE_DIR}/ThirdParty/OpenGV/build/lib/Debug/opengv.lib) 163 | SET (RANDOMGENLIBS_DEBUG ${PROJECT_SOURCE_DIR}/ThirdParty/OpenGV/build/lib/Debug/random_generators.lib) 164 | 165 | SET (G2OLIBS ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/bin/Release/g2o.lib) 166 | SET (DBOW2LIBS ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/Release/DBoW2.lib) 167 | SET (OPENGVLIBS ${PROJECT_SOURCE_DIR}/ThirdParty/OpenGV/build/lib/Release/opengv.lib) 168 | SET (RANDOMGENLIBS ${PROJECT_SOURCE_DIR}/ThirdParty/OpenGV/build/lib/Release/random_generators.lib) 169 | ELSE() 170 | SET (G2OLIBS ${PROJECT_SOURCE_DIR}/ThirdParty/g2o/lib/libg2o.so) 171 | SET (DBOW2LIBS ${PROJECT_SOURCE_DIR}/ThirdParty/DBoW2/lib/libDBoW2.so) 172 | SET (OPENGVLIBS ${PROJECT_SOURCE_DIR}/ThirdParty/OpenGV/build/lib/librandom_generators.a 173 | ${PROJECT_SOURCE_DIR}/ThirdParty/OpenGV/build/lib/libopengv.a) 174 | ENDIF() 175 | 176 | IF(WIN32) 177 | target_link_libraries(${PROJECT_NAME} 178 | ${OpenCV_LIBS} 179 | ${Pangolin_LIBRARIES} 180 | optimized ${G2OLIBS} 181 | optimized ${DBOW2LIBS} 182 | optimized ${OPENGVLIBS} 183 | optimized ${RANDOMGENLIBS} 184 | debug ${G2OLIBS_DEBUG} 185 | debug ${DBOW2LIBS_DEBUG} 186 | debug ${OPENGVLIBS_DEBUG} 187 | debug ${RANDOMGENLIBS_DEBUG}) 188 | ELSE() 189 | target_link_libraries(${PROJECT_NAME} 190 | ${OpenCV_LIBS} 191 | ${Pangolin_LIBRARIES} 192 | ${G2OLIBS} 193 | ${DBOW2LIBS} 194 | ${OPENGVLIBS} 195 | ${RANDOMGENLIBS}) 196 | ENDIF() 197 | 198 | # Build examples 199 | MESSAGE("${PROJECT_NAME}") 200 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Lafida) 201 | add_executable(multi_col_slam_lafida Examples/Lafida/mult_col_slam_lafida.cpp) 202 | target_link_libraries(multi_col_slam_lafida ${PROJECT_NAME}) 203 | -------------------------------------------------------------------------------- /src/cMultiFramePublisher.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * 4 | * Copyright (C) 2015-2016 Steffen Urban 5 | * For more information see 6 | * 7 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * MultiCol-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with MultiCol-SLAM . If not, see . 19 | */ 20 | 21 | /* 22 | * MultiCol-SLAM is based on ORB-SLAM2 which was also released under GPLv3 23 | * For more information see 24 | * Raúl Mur-Artal (University of Zaragoza) 25 | */ 26 | 27 | #include "cMultiFramePublisher.h" 28 | #include "cTracking.h" 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | namespace MultiColSLAM 36 | { 37 | cMultiFramePublisher::cMultiFramePublisher(cMap *pMap) : 38 | nrCams(1), mcsSet(false), cntWrite(0), mpMap() 39 | { 40 | mImages.push_back(cv::Mat::zeros(480, 640, CV_8UC1)); 41 | mState = cTracking::SYSTEM_NOT_READY; 42 | 43 | mbUpdated = false; 44 | } 45 | 46 | bool cMultiFramePublisher::isMapUpdated() 47 | { 48 | std::unique_lock lock(mMutex); 49 | return mbUpdated; 50 | } 51 | 52 | void cMultiFramePublisher::SetMCS(cMultiCamSys_* tmpSys) 53 | { 54 | nrCams = tmpSys->GetNrCams(); 55 | mImages.clear(); 56 | mImages.resize(nrCams); 57 | for (int c = 0; c < nrCams; ++c) 58 | mImages[c] = cv::Mat::zeros(tmpSys->GetCamModelObj(c).GetHeight(), 59 | tmpSys->GetCamModelObj(c).GetWidth(), CV_8UC3); 60 | 61 | mcsSet = true; 62 | } 63 | 64 | void cMultiFramePublisher::SetMap(cMap *pMap) 65 | { 66 | mpMap = pMap; 67 | } 68 | 69 | void cMultiFramePublisher::DrawMultiFrame(std::vector& imgs) 70 | { 71 | cv::Mat imWithInfo = cv::Mat::zeros(cv::Size(640, 480), CV_8UC1); 72 | if (mcsSet) 73 | { 74 | std::vector ims(nrCams); 75 | std::vector vIniKeys; // Initialization: KeyPoints in reference frame 76 | std::vector vMatches; // Initialization: correspondeces with reference keypoints 77 | std::vector vCurrentKeys; // KeyPoints in current frame 78 | std::vector vMatchedMapPoints; // Tracked MapPoints in current frame 79 | std::unordered_map lkeyp_to_cam; 80 | int state; // Tracking state 81 | 82 | //Copy variable to be used within scoped mutex 83 | { 84 | std::unique_lock lock(mMutex); 85 | state = mState; 86 | if (mState == cTracking::SYSTEM_NOT_READY) 87 | mState = cTracking::NO_IMAGES_YET; 88 | 89 | if (mState == cTracking::NOT_INITIALIZED) 90 | { 91 | vCurrentKeys = mvCurrentKeys; 92 | vIniKeys = mvIniKeys; 93 | vMatches = mvIniMatches; 94 | lkeyp_to_cam = keyp_to_cam; 95 | } 96 | else if (mState == cTracking::INITIALIZING) 97 | { 98 | vCurrentKeys = mvCurrentKeys; 99 | vIniKeys = mvIniKeys; 100 | vMatches = mvIniMatches; 101 | lkeyp_to_cam = keyp_to_cam; 102 | } 103 | else if (mState == cTracking::WORKING) 104 | { 105 | vCurrentKeys = mvCurrentKeys; 106 | vMatchedMapPoints = mvpMatchedMapPoints; 107 | lkeyp_to_cam = keyp_to_cam; 108 | } 109 | else if (mState == cTracking::LOST) 110 | { 111 | vCurrentKeys = mvCurrentKeys; 112 | lkeyp_to_cam = keyp_to_cam; 113 | } 114 | } // destroy scoped mutex -> release 115 | 116 | for (int c = 0; c < nrCams; ++c) 117 | { 118 | ims[c] = mImages[c]; 119 | if (ims[c].channels() < 3) 120 | cvtColor(ims[c], ims[c], CV_GRAY2BGR); 121 | 122 | //Draw 123 | if (state == cTracking::INITIALIZING) //INITIALIZING 124 | { 125 | for (unsigned int i = 0; i < vMatches.size(); ++i) 126 | { 127 | 128 | if (vMatches[i] >= 0) 129 | { 130 | int camIdx = lkeyp_to_cam.find(vMatches[i])->second; 131 | cv::line(ims[camIdx], vIniKeys[i].pt, vCurrentKeys[vMatches[i]].pt, 132 | cv::Scalar(0, 255, 0)); 133 | } 134 | } 135 | } 136 | // TRACKING 137 | else if (state == cTracking::WORKING) 138 | { 139 | mnTracked = 0; 140 | const float r = 5; 141 | for (unsigned int i = 0; i < vMatchedMapPoints.size(); ++i) 142 | { 143 | if (vMatchedMapPoints[i] || mvbOutliers[i]) 144 | { 145 | int camIdx = lkeyp_to_cam.find(i)->second; 146 | 147 | cv::Point2f pt1, pt2; 148 | pt1.x = vCurrentKeys[i].pt.x - r; 149 | pt1.y = vCurrentKeys[i].pt.y - r; 150 | pt2.x = vCurrentKeys[i].pt.x + r; 151 | pt2.y = vCurrentKeys[i].pt.y + r; 152 | if (!mvbOutliers[i]) 153 | { 154 | cv::rectangle(ims[camIdx], pt1, pt2, cv::Scalar(255, 0, 0)); 155 | cv::circle(ims[camIdx], vCurrentKeys[i].pt, 2, cv::Scalar(0, 255, 0), -1); 156 | ++mnTracked; 157 | } 158 | } 159 | } 160 | } 161 | //if (c==0) 162 | // DrawTextInfo(ims[c], state, imWithInfo); 163 | //cv::imwrite("C:/trajectories/traj8loop2/video/img"+to_string(c)+"_" + to_string(cntWrite) + ".jpg", ims[c]); 164 | } 165 | //cv::Mat allImgs = cv::Mat::zeros(cv::Size(mImages[0].cols, 3 * mImages[0].rows), CV_8UC3); 166 | //ims[0].copyTo(allImgs(cv::Rect(r*, 0, mImages[cn].cols, mImages[cn].rows))); 167 | //ims[1].copyTo(allImgs(cv::Rect(0, mImages[1].rows, mImages[1].cols, mImages[1].rows))); 168 | //ims[2].copyTo(allImgs(cv::Rect(0, mImages[2].rows * 2, mImages[2].cols, mImages[2].rows))); 169 | DrawTextInfo(ims[0], state, imWithInfo); 170 | imgs.push_back(imWithInfo); 171 | for (int c = 1; c < nrCams;++c) 172 | imgs.push_back(ims[c]); 173 | } 174 | //cv::Mat imWithInfoR; 175 | //cv::resize(imWithInfo, imWithInfoR, cv::Size(), 0.7, 0.7, cv::INTER_LINEAR); 176 | //cv::imwrite("imWithInfoR" + to_string(cntWrite) + ".jpg", imWithInfo); 177 | //++cntWrite; 178 | } 179 | 180 | void cMultiFramePublisher::DrawTextInfo(cv::Mat &im, 181 | int nState, cv::Mat &imText) 182 | { 183 | std::stringstream s; 184 | if (nState == cTracking::NO_IMAGES_YET) 185 | s << "WAITING FOR IMAGES. (Topic: /camera/image_raw)"; 186 | else if (nState == cTracking::NOT_INITIALIZED) 187 | s << " NOT INITIALIZED "; 188 | else if (nState == cTracking::INITIALIZING) 189 | s << " TRYING TO INITIALIZE "; 190 | else if (nState == cTracking::WORKING) 191 | { 192 | s << " TRACKING "; 193 | //int nKFs = mpMap->KeyFramesInMap(); 194 | //int nMPs = mpMap->MapPointsInMap(); 195 | // s << " - KFs: " << nKFs << " , MPs: " << nMPs << " , Tracked: " << mnTracked; 196 | } 197 | else if (nState == cTracking::LOST) 198 | { 199 | s << " TRACK LOST. TRYING TO RELOCALIZE "; 200 | } 201 | else if (nState == cTracking::SYSTEM_NOT_READY) 202 | { 203 | s << " LOADING ORB VOCABULARY. PLEASE WAIT..."; 204 | } 205 | 206 | int baseline = 0; 207 | cv::Size textSize = cv::getTextSize(s.str(), cv::FONT_HERSHEY_PLAIN, 1, 1, &baseline); 208 | 209 | imText = cv::Mat(im.rows + textSize.height + 10, im.cols, im.type()); 210 | im.copyTo(imText.rowRange(0, im.rows).colRange(0, im.cols)); 211 | imText.rowRange(im.rows, imText.rows) = cv::Mat::zeros(textSize.height + 10, im.cols, im.type()); 212 | cv::putText(imText, s.str(), cv::Point(5, imText.rows - 5), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(255, 255, 255), 1, 8); 213 | } 214 | 215 | void cMultiFramePublisher::Update(cTracking *pTracker) 216 | { 217 | std::unique_lock lock(mMutex); 218 | 219 | mImages = pTracker->mCurrentFrame.images; 220 | mvCurrentKeys = pTracker->mCurrentFrame.mvKeys; 221 | mvpMatchedMapPoints = pTracker->mCurrentFrame.mvpMapPoints; 222 | mvbOutliers = pTracker->mCurrentFrame.mvbOutlier; 223 | keyp_to_cam = pTracker->mCurrentFrame.keypoint_to_cam; 224 | 225 | if (pTracker->mLastProcessedState == cTracking::INITIALIZING) 226 | { 227 | mvIniKeys = pTracker->mInitialFrame.mvKeys; 228 | mvIniMatches = pTracker->mvIniMatches; 229 | } 230 | mState = static_cast(pTracker->mLastProcessedState); 231 | 232 | mbUpdated = true; 233 | } 234 | } -------------------------------------------------------------------------------- /include/g2o_MultiCol_sim3_expmap.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * 4 | * Copyright (C) 2015-2016 Steffen Urban 5 | * For more information see 6 | * 7 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * MultiCol-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with MultiCol-SLAM . If not, see . 19 | */ 20 | 21 | #ifndef G2O_MULTICAM_SIM3_EXPMAP_H 22 | #define G2O_MULTICAM_SIM3_EXPMAP_H 23 | 24 | #include 25 | #include 26 | 27 | #include "g2o/core/robust_kernel_impl.h" 28 | #include "g2o/types/types_seven_dof_expmap.h" 29 | 30 | #include 31 | 32 | #include "misc.h" 33 | #include "cam_model_omni.h" 34 | #include "cam_system_omni.h" 35 | #include "cConverter.h" 36 | #include "g2o_MultiCol_vertices_edges.h" 37 | 38 | namespace MultiColSLAM 39 | { 40 | /** 41 | * \brief Sim3 Vertex, (x,y,z,qw,qx,qy,qz) 42 | * the parameterization for the increments constructed is a 7d vector 43 | * (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion. 44 | */ 45 | class simpleVertexSim3Expmap : public g2o::BaseVertex<7, g2o::Sim3> 46 | { 47 | public: 48 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 49 | simpleVertexSim3Expmap(); 50 | virtual bool read(std::istream& is); 51 | virtual bool write(std::ostream& os) const; 52 | 53 | virtual void setToOriginImpl() { 54 | _estimate = g2o::Sim3(); 55 | } 56 | 57 | virtual void oplusImpl(const double* update_) 58 | { 59 | Eigen::Map update(const_cast(update_)); 60 | 61 | if (_fix_scale) 62 | update[6] = 0; 63 | 64 | g2o::Sim3 s(update); 65 | setEstimate(s*estimate()); 66 | } 67 | 68 | bool _fix_scale; 69 | 70 | protected: 71 | }; 72 | 73 | /** 74 | * \brief 7D edge between two Vertex7 75 | */ 76 | class edgeSim3 : public g2o::BaseBinaryEdge<7, g2o::Sim3, 77 | simpleVertexSim3Expmap, simpleVertexSim3Expmap> 78 | { 79 | public: 80 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 81 | edgeSim3(); 82 | virtual bool read(std::istream& is); 83 | virtual bool write(std::ostream& os) const; 84 | void computeError() 85 | { 86 | const simpleVertexSim3Expmap* v1 = static_cast(_vertices[0]); 87 | const simpleVertexSim3Expmap* v2 = static_cast(_vertices[1]); 88 | 89 | g2o::Sim3 C(_measurement); 90 | g2o::Sim3 error_ = C*v1->estimate()*v2->estimate().inverse(); 91 | _error = error_.log(); 92 | } 93 | 94 | virtual double initialEstimatePossible( 95 | const g2o::OptimizableGraph::VertexSet&, g2o::OptimizableGraph::Vertex*) 96 | { 97 | return 1.; 98 | } 99 | virtual void initialEstimate( 100 | const g2o::OptimizableGraph::VertexSet& from, g2o::OptimizableGraph::Vertex* /*to*/) 101 | { 102 | simpleVertexSim3Expmap* v1 = static_cast(_vertices[0]); 103 | simpleVertexSim3Expmap* v2 = static_cast(_vertices[1]); 104 | if (from.count(v1) > 0) 105 | v2->setEstimate(measurement()*v1->estimate()); 106 | else 107 | v1->setEstimate(measurement().inverse()*v2->estimate()); 108 | } 109 | }; 110 | 111 | class VertexSim3Expmap_Multi : public g2o::BaseVertex<7, g2o::Sim3> 112 | { 113 | public: 114 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 115 | VertexSim3Expmap_Multi() {} 116 | VertexSim3Expmap_Multi( 117 | std::unordered_map& kp_to_cam1, 118 | std::unordered_map& kp_to_cam2); 119 | 120 | virtual void setToOriginImpl() { 121 | _estimate = g2o::Sim3(); 122 | } 123 | 124 | virtual void oplusImpl(const double* update_) 125 | { 126 | Eigen::Map update(const_cast(update_)); 127 | 128 | if (_fix_scale) 129 | update[6] = 0; 130 | 131 | g2o::Sim3 s(update); 132 | setEstimate(s*estimate()); 133 | } 134 | 135 | cMultiCamSys_* camSys1; 136 | cMultiCamSys_* camSys2; 137 | 138 | 139 | cv::Vec2d cam_map1(const Eigen::Vector3d& v, int ptIdx) const 140 | { 141 | cv::Vec2d res; 142 | int camIdx = keypoint_to_cam1.find(ptIdx)->second; 143 | cv::Vec4d v_in_cam = cConverter::invMat(camSys1->Get_M_c(camIdx))*cConverter::toVec4d(v); 144 | // not world2to cam but only projection 145 | camSys1->GetCamModelObj(camIdx).WorldToImg( 146 | v_in_cam(0), v_in_cam(1), v_in_cam(2), res(0), res(1)); 147 | return res; 148 | } 149 | 150 | cv::Vec2d cam_map2(const Eigen::Vector3d& v, int ptIdx) const 151 | { 152 | cv::Vec2d res; 153 | int camIdx = keypoint_to_cam2.find(ptIdx)->second; 154 | cv::Vec4d v_in_cam = cConverter::invMat(camSys2->Get_M_c(camIdx))*cConverter::toVec4d(v); 155 | // not world2to cam but only projection 156 | camSys2->GetCamModelObj(camIdx).WorldToImg( 157 | v_in_cam(0), v_in_cam(1), v_in_cam(2), res(0), res(1)); 158 | return res; 159 | } 160 | 161 | bool _fix_scale; 162 | 163 | bool read(std::istream& is) 164 | { 165 | std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl; 166 | return false; 167 | } 168 | 169 | bool write(std::ostream& os) const 170 | { 171 | std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl; 172 | return false; 173 | } 174 | 175 | std::unordered_map keypoint_to_cam1; 176 | std::unordered_map keypoint_to_cam2; 177 | }; 178 | 179 | 180 | 181 | class EdgeSim3ProjectXYZ_Multi : 182 | public g2o::BaseBinaryEdge<2, g2o::Vector2D, VertexPointXYZ, VertexSim3Expmap_Multi> 183 | { 184 | public: 185 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 186 | EdgeSim3ProjectXYZ_Multi() {} 187 | 188 | void computeError() 189 | { 190 | const VertexSim3Expmap_Multi* v1 = static_cast(_vertices[1]); 191 | const VertexPointXYZ* v2 = static_cast(_vertices[0]); 192 | 193 | g2o::Vector2D obs(_measurement); 194 | cv::Vec3d est = v2->estimate(); 195 | cv::Vec2d m = v1->cam_map1( 196 | v1->estimate().map(cConverter::toVector3d(est)), 197 | v2->ptID); 198 | 199 | _error[0] = obs[0] - m(0); 200 | _error[1] = obs[1] - m(1); 201 | } 202 | bool read(std::istream& is) 203 | { 204 | std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl; 205 | return false; 206 | } 207 | 208 | bool write(std::ostream& os) const 209 | { 210 | std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl; 211 | return false; 212 | } 213 | }; 214 | /**/ 215 | class EdgeInverseSim3ProjectXYZ_Multi : 216 | public g2o::BaseBinaryEdge<2, g2o::Vector2D, VertexPointXYZ, VertexSim3Expmap_Multi> 217 | { 218 | public: 219 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 220 | EdgeInverseSim3ProjectXYZ_Multi() {} 221 | 222 | void computeError() 223 | { 224 | const VertexSim3Expmap_Multi* v1 = static_cast(_vertices[1]); 225 | const VertexPointXYZ* v2 = static_cast(_vertices[0]); 226 | 227 | g2o::Vector2D obs(_measurement); 228 | cv::Vec3d est = v2->estimate(); 229 | cv::Vec2d m = v1->cam_map2( 230 | v1->estimate().inverse().map(cConverter::toVector3d(est)), 231 | v2->ptID); 232 | 233 | _error[0] = obs[0] - m(0); 234 | _error[1] = obs[1] - m(1); 235 | //cout << "errorInv: " << _error << endl; 236 | } 237 | bool read(std::istream& is) 238 | { 239 | std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl; 240 | return false; 241 | } 242 | 243 | bool write(std::ostream& os) const 244 | { 245 | std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl; 246 | return false; 247 | } 248 | }; 249 | 250 | 251 | /** 252 | * \brief 7D edge between two Vertex7 253 | */ 254 | class EdgeSim3_Multi : 255 | public g2o::BaseBinaryEdge <7, g2o::Sim3, VertexSim3Expmap_Multi, VertexSim3Expmap_Multi> 256 | { 257 | public: 258 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 259 | EdgeSim3_Multi(); 260 | 261 | void computeError() 262 | { 263 | const VertexSim3Expmap_Multi* v1 = 264 | static_cast(_vertices[0]); 265 | const VertexSim3Expmap_Multi* v2 = 266 | static_cast(_vertices[1]); 267 | 268 | g2o::Sim3 C(_measurement); 269 | g2o::Sim3 error_ = C*v1->estimate()*v2->estimate().inverse(); 270 | _error = error_.log(); 271 | } 272 | 273 | virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet&, 274 | g2o::OptimizableGraph::Vertex*) 275 | { 276 | return 1.; 277 | } 278 | 279 | virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from, 280 | g2o::OptimizableGraph::Vertex* /*to*/) 281 | { 282 | VertexSim3Expmap_Multi* v1 = static_cast(_vertices[0]); 283 | VertexSim3Expmap_Multi* v2 = static_cast(_vertices[1]); 284 | if (from.count(v1) > 0) 285 | v2->setEstimate(measurement()*v1->estimate()); 286 | else 287 | v1->setEstimate(measurement().inverse()*v2->estimate()); 288 | } 289 | 290 | bool read(std::istream& is) 291 | { 292 | std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl; 293 | return false; 294 | } 295 | 296 | bool write(std::ostream& os) const 297 | { 298 | std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl; 299 | return false; 300 | } 301 | }; 302 | 303 | } 304 | #endif -------------------------------------------------------------------------------- /src/cam_system_omni.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * 4 | * Copyright (C) 2015-2016 Steffen Urban 5 | * For more information see 6 | * 7 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * MultiCol-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with MultiCol-SLAM . If not, see . 19 | */ 20 | #include "cam_system_omni.h" 21 | #include "cConverter.h" 22 | 23 | namespace MultiColSLAM 24 | { 25 | void cMultiCamSys_::WorldToCam(int c, 26 | cv::Point3_& pt3, 27 | cv::Point_& pt2) 28 | { 29 | cv::Matx M_ = M_t*M_c[c]; 30 | 31 | cv::Matx pt3_(pt3.x, pt3.y, pt3.z, 1.0); 32 | 33 | cv::Matx ptRot = cConverter::invMat(M_)*pt3_; 34 | pt2.x = 0.0; 35 | pt2.y = 0.0; 36 | 37 | camModels[c].WorldToImg( 38 | ptRot(0, 0), ptRot(1, 0), ptRot(2, 0), 39 | pt2.x, pt2.y); 40 | } 41 | 42 | void cMultiCamSys_::WorldToCam(int c, 43 | cv::Point3_& pt3, 44 | cv::Vec2d& pt2) 45 | { 46 | cv::Matx ptRot; 47 | cv::Matx pt3_(pt3.x, pt3.y, pt3.z, 1.0); 48 | if (!flagMcMt) 49 | { 50 | cv::Matx M_ = M_t * M_c[c]; 51 | ptRot = cConverter::invMat(M_)*pt3_; 52 | } 53 | else 54 | ptRot = MtMc_inv[c] * pt3_; 55 | 56 | pt2 = 0.0; 57 | pt2 = 0.0; 58 | 59 | cv::Point3_ ptRot3; 60 | ptRot3.x = ptRot(0, 0); 61 | ptRot3.y = ptRot(1, 0); 62 | ptRot3.z = ptRot(2, 0); 63 | 64 | camModels[c].WorldToImg(ptRot3.x, ptRot3.y, ptRot3.z, pt2(0), pt2(1)); 65 | } 66 | 67 | void cMultiCamSys_::WorldToCamHom(int c, 68 | cv::Vec& pt4, 69 | cv::Vec2d& pt2) 70 | { 71 | cv::Matx ptRot; 72 | if (!flagMcMt) 73 | { 74 | cv::Matx M_ = M_t * M_c[c]; 75 | ptRot = cConverter::invMat(M_)*pt4; 76 | } 77 | else 78 | ptRot = MtMc_inv[c] * pt4; 79 | 80 | pt2 = 0.0; 81 | pt2 = 0.0; 82 | 83 | cv::Point3_ ptRot3; 84 | ptRot3.x = ptRot(0, 0); 85 | ptRot3.y = ptRot(1, 0); 86 | ptRot3.z = ptRot(2, 0); 87 | 88 | camModels[c].WorldToImg(ptRot(0, 0), ptRot(1, 0), ptRot(2, 0), 89 | pt2(0), pt2(1)); 90 | } 91 | 92 | bool cMultiCamSys_::WorldToCamHom_fast(int c, 93 | cv::Vec& pt4, 94 | cv::Vec& pt2) 95 | { 96 | cv::Matx ptRot; 97 | if (!flagMcMt) 98 | { 99 | cv::Matx M_ = M_t * M_c[c]; 100 | 101 | ptRot = cConverter::invMat(M_) * pt4; 102 | } 103 | else 104 | ptRot = MtMc_inv[c] * pt4; 105 | 106 | pt2(0) = 0.0; 107 | pt2(1) = 0.0; 108 | 109 | camModels[c].WorldToImg(ptRot(0, 0), ptRot(1, 0), ptRot(2, 0), 110 | pt2(0), pt2(1)); 111 | return ptRot(2, 0) <= 0.0; 112 | } 113 | 114 | void cMultiCamSys_::WorldToCamHom_fast(int c, 115 | cv::Vec& pt3, 116 | cv::Vec& pt2) 117 | { 118 | cv::Matx ptRot; 119 | if (!flagMcMt) 120 | { 121 | cv::Matx M_ = M_t * M_c[c]; 122 | 123 | ptRot = cConverter::invMat(M_) * cv::Vec4d(pt3(0), pt3(1), pt3(2), 1.0); 124 | } 125 | else 126 | ptRot = MtMc_inv[c] * cv::Vec4d(pt3(0), pt3(1), pt3(2), 1.0); 127 | 128 | pt2(0) = 0.0; 129 | pt2(1) = 0.0; 130 | 131 | camModels[c].WorldToImg(ptRot(0, 0), ptRot(1, 0), ptRot(2, 0), 132 | pt2(0), pt2(1)); 133 | } 134 | 135 | void cMultiCamSys_::CamToWorld_ogv(int c, 136 | opengv::bearingVector_t& bearingV, 137 | cv::Point_ pt2) 138 | { 139 | cv::Point3_ pt3; 140 | camModels[c].ImgToWorld(pt3, 141 | pt2); 142 | 143 | bearingV[0] = pt3.x; 144 | bearingV[1] = pt3.y; 145 | bearingV[2] = pt3.z; 146 | 147 | } 148 | 149 | void cMultiCamSys_::CamToWorld(int c, cv::Point3_& pt3, cv::Point_& pt2) 150 | { 151 | cv::Point3_ pt3t; 152 | camModels[c].ImgToWorld(pt3t, pt2); 153 | 154 | pt3.x = pt3t.x; 155 | pt3.y = pt3t.y; 156 | pt3.z = pt3t.z; 157 | } 158 | 159 | void cMultiCamSys_::CamToWorld(int c, cv::Vec& pt3, cv::Point_& pt2) 160 | { 161 | cv::Point3_ pt3t; 162 | camModels[c].ImgToWorld(pt3t, pt2); 163 | 164 | pt3(0) = pt3t.x; 165 | pt3(1) = pt3t.y; 166 | pt3(2) = pt3t.z; 167 | } 168 | 169 | // set pose 170 | void cMultiCamSys_::Set_M_t_from_min(cv::Matx M_t_minRep) 171 | { 172 | M_t_min = M_t_minRep; 173 | M_t = cayley2hom(M_t_minRep); 174 | //M_t = rodrigues2hom(M_t_minRep); 175 | M_t_inv = cConverter::invMat(M_t); 176 | 177 | for (int c = 0; c < nrCams; ++c) 178 | { 179 | MtMc[c] = M_t * this->M_c[c]; 180 | MtMc_inv[c] = cConverter::invMat(MtMc[c]); 181 | } 182 | flagMcMt = true; 183 | } 184 | 185 | void cMultiCamSys_::Set_M_t(cv::Matx M_t_) 186 | { 187 | M_t_min = hom2cayley(M_t_); 188 | //M_t_min = hom2rodrigues(M_t_); 189 | M_t = M_t_; 190 | M_t_inv = cConverter::invMat(M_t); 191 | 192 | for (int c = 0; c < nrCams; ++c) 193 | { 194 | MtMc[c] = M_t*this->M_c[c]; 195 | MtMc_inv[c] = cConverter::invMat(MtMc[c]); 196 | } 197 | flagMcMt = true; 198 | } 199 | 200 | // set calibration 201 | void cMultiCamSys_::Set_M_c_from_min(int c, cv::Matx M_c_minRep) 202 | { 203 | M_c_min[c] = M_c_minRep; 204 | M_c[c] = cayley2hom(M_c_minRep); 205 | //M_c[c] = rodrigues2hom(M_c_minRep); 206 | cv::Mat rTemp = cv::Mat(M_c[c])(cv::Rect(0, 0, 3, 3)); 207 | cv::Mat tTemp = cv::Mat(M_c[c])(cv::Rect(3, 0, 1, 3)); 208 | // to opengv 209 | Eigen::Matrix3d rotTemp; 210 | cv::cv2eigen(rTemp, rotTemp); 211 | camRotations[c] = rotTemp; 212 | 213 | Eigen::Vector3d transTemp; 214 | cv::cv2eigen(tTemp, transTemp); 215 | camOffsets[c] = transTemp; 216 | } 217 | 218 | void cMultiCamSys_::Set_M_c(int c, cv::Matx M_c_) 219 | { 220 | M_c_min[c] = hom2cayley(M_c_); 221 | //M_c_min[c] = hom2rodrigues(M_c_); 222 | M_c[c] = M_c_; 223 | 224 | cv::Mat rTemp = cv::Mat(M_c_)(cv::Rect(0, 0, 3, 3)); 225 | cv::Mat tTemp = cv::Mat(M_c_)(cv::Rect(3, 0, 1, 3)); 226 | // to opengv 227 | Eigen::Matrix3d rotTemp; 228 | cv::cv2eigen(rTemp, rotTemp); 229 | camRotations[c] = rotTemp; 230 | 231 | Eigen::Vector3d transTemp; 232 | cv::cv2eigen(tTemp, transTemp); 233 | camOffsets[c] = transTemp; 234 | } 235 | void cMultiCamSys_::Set_All_M_c(std::vector> M_c_) 236 | { 237 | M_c = M_c_; 238 | for (int i = 0; i < M_c.size(); ++i) 239 | { 240 | M_c_min.push_back(hom2cayley(M_c[i])); 241 | //M_c_min.push_back(hom2rodrigues(M_c[i])); 242 | cv::Mat rTemp = cv::Mat(M_c[i])(cv::Rect(0, 0, 3, 3)); 243 | cv::Mat tTemp = cv::Mat(M_c[i])(cv::Rect(3, 0, 1, 3)); 244 | // to opengv 245 | Eigen::Matrix3d rotTemp; 246 | cv::cv2eigen(rTemp, rotTemp); 247 | camRotations.push_back(rotTemp); 248 | 249 | Eigen::Vector3d transTemp; 250 | cv::cv2eigen(tTemp, transTemp); 251 | camOffsets.push_back(transTemp); 252 | } 253 | } 254 | void cMultiCamSys_::Set_All_M_c_from_min(std::vector> M_c_min_) 255 | { 256 | M_c_min = M_c_min_; 257 | for (int i = 0; i < M_c_min.size(); ++i) 258 | { 259 | cv::Matx c2h = cayley2hom(M_c_min[i]); 260 | //cv::Matx c2h = rodrigues2hom(M_c_min[i]); 261 | M_c.push_back(c2h); 262 | cv::Mat rTemp = cv::Mat(c2h)(cv::Rect(0, 0, 3, 3)); 263 | cv::Mat tTemp = cv::Mat(c2h)(cv::Rect(3, 0, 1, 3)); 264 | 265 | // to opengv 266 | Eigen::Matrix3d rotTemp; 267 | cv::cv2eigen(rTemp, rotTemp); 268 | camRotations.push_back(rotTemp); 269 | 270 | Eigen::Vector3d transTemp; 271 | cv::cv2eigen(tTemp, transTemp); 272 | camOffsets.push_back(transTemp); 273 | } 274 | } 275 | 276 | // add MCS camera pose 277 | // and set the last one to the actual pose 278 | void cMultiCamSys_::Add_M_c(cv::Matx M_c_) 279 | { 280 | M_c.push_back(M_c_); 281 | M_c_min.push_back(hom2cayley(M_c_)); 282 | //M_c_min.push_back(hom2rodrigues(M_c_)); 283 | cv::Mat rTemp = cv::Mat(M_c_)(cv::Rect(0, 0, 3, 3)); 284 | cv::Mat tTemp = cv::Mat(M_c_)(cv::Rect(3, 0, 1, 3)); 285 | 286 | // to opengv 287 | Eigen::Matrix3d rotTemp; 288 | cv::cv2eigen(rTemp, rotTemp); 289 | camRotations.push_back(rotTemp); 290 | 291 | Eigen::Vector3d transTemp; 292 | cv::cv2eigen(tTemp, transTemp); 293 | camOffsets.push_back(transTemp); 294 | 295 | nrCams++; 296 | } 297 | void cMultiCamSys_::Add_M_c_from_min(cv::Matx M_c_min_) 298 | { 299 | M_c_min.push_back(M_c_min_); 300 | cv::Matx c2h = cayley2hom(M_c_min_); 301 | //cv::Matx c2h = rodrigues2hom(M_c_min_); 302 | M_c.push_back(c2h); 303 | 304 | cv::Mat rTemp = cv::Mat(c2h)(cv::Rect(0, 0, 3, 3)); 305 | cv::Mat tTemp = cv::Mat(c2h)(cv::Rect(3, 0, 1, 3)); 306 | 307 | // to opengv 308 | Eigen::Matrix3d rotTemp; 309 | cv::cv2eigen(rTemp, rotTemp); 310 | camRotations.push_back(rotTemp); 311 | 312 | Eigen::Vector3d transTemp; 313 | cv::cv2eigen(tTemp, transTemp); 314 | camOffsets.push_back(transTemp); 315 | 316 | nrCams++; 317 | } 318 | 319 | void cMultiCamSys_::Add_M_c_from_min_and_IO(cv::Matx M_c_min_, 320 | cCamModelGeneral_ camM) 321 | { 322 | Add_M_c_from_min(M_c_min_); 323 | 324 | camModels.push_back(camM); 325 | 326 | //nrCams++; 327 | } 328 | 329 | void cMultiCamSys_::Set_M_c_from_min_and_IO(int c, cv::Matx M_c_min_, cCamModelGeneral_ camM) 330 | { 331 | M_c_min[c] = M_c_min_; 332 | M_c[c] = cayley2hom(M_c_min_); 333 | //M_c[c] = rodrigues2hom(M_c_min_); 334 | camModels[c] = camM; 335 | 336 | cv::Mat rTemp = cv::Mat(M_c[c])(cv::Rect(0, 0, 3, 3)); 337 | cv::Mat tTemp = cv::Mat(M_c[c])(cv::Rect(3, 0, 1, 3)); 338 | 339 | // to opengv 340 | Eigen::Matrix3d rotTemp; 341 | cv::cv2eigen(rTemp, rotTemp); 342 | camRotations[c] = rotTemp; 343 | 344 | Eigen::Vector3d transTemp; 345 | cv::cv2eigen(tTemp, transTemp); 346 | camOffsets[c] = transTemp; 347 | } 348 | } -------------------------------------------------------------------------------- /src/cMultiInitializer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * 4 | * Copyright (C) 2015-2016 Steffen Urban 5 | * For more information see 6 | * 7 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * MultiCol-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with MultiCol-SLAM . If not, see . 19 | */ 20 | 21 | /* 22 | * MultiCol-SLAM is based on ORB-SLAM2 which was also released under GPLv3 23 | * For more information see 24 | * Raúl Mur-Artal (University of Zaragoza) 25 | */ 26 | 27 | #include "cMultiInitializer.h" 28 | 29 | #include "cOptimizer.h" 30 | #include "cConverter.h" 31 | #include 32 | #include 33 | #include "DBoW2/DUtils/Random.h" 34 | #include "misc.h" 35 | 36 | namespace MultiColSLAM 37 | { 38 | cMultiInitializer::cMultiInitializer(const cMultiFrame &ReferenceFrame, 39 | double sigma, int iterations) 40 | { 41 | camSystem = ReferenceFrame.camSystem; 42 | 43 | mvKeys1 = ReferenceFrame.mvKeys; 44 | mvKeysRays1 = ReferenceFrame.mvKeysRays; 45 | referenceFrame = ReferenceFrame; 46 | 47 | mSigma = sigma; 48 | mSigma2 = sigma * sigma; 49 | mMaxIterations = iterations; 50 | } 51 | 52 | bool cMultiInitializer::Initialize(cMultiFrame ¤tFrame, 53 | const std::vector &vMatches12, 54 | cv::Matx33d &R21, 55 | cv::Vec3d &t21, 56 | std::vector &vP3D, 57 | std::vector &vbTriangulated, 58 | int& bestCam) 59 | { 60 | // Fill structures with current keypoints and matches with reference frame 61 | // Reference Frame: 1, Current Frame: 2 62 | mvKeys2 = currentFrame.mvKeys; 63 | mvKeysRays2 = currentFrame.mvKeysRays; 64 | 65 | mvMatches12.clear(); 66 | mvMatches12.reserve(mvKeys2.size()); 67 | mvbMatched1.resize(mvKeys1.size()); 68 | 69 | for (int i = 0, iend = vMatches12.size(); i < iend; i++) 70 | { 71 | if (vMatches12[i] >= 0) 72 | { 73 | mvMatches12.push_back(std::make_pair(i, vMatches12[i])); 74 | mvbMatched1[i] = true; 75 | } 76 | else 77 | mvbMatched1[i] = false; 78 | } 79 | 80 | const int N = mvMatches12.size(); 81 | 82 | // Indices for minimum set selection 83 | std::vector vAllIndices; 84 | vAllIndices.reserve(N); 85 | std::vector vAvailableIndices; 86 | 87 | for (int i = 0; i < N; i++) 88 | vAllIndices.push_back(i); 89 | // pretty easy 90 | // eightpt ransac the problem, then test the median norm of the result 91 | // if it is big enough -> init the system 92 | int nrCams = currentFrame.camSystem.GetNrCams(); 93 | std::vector bear1(nrCams); 94 | std::vector bear2(nrCams); 95 | std::vector > bear1_cont_indices(nrCams); 96 | std::vector > bear2_cont_indices(nrCams); 97 | 98 | // assign matches/bearing vectors to each camera 99 | for (int i = 0; i < N; ++i) 100 | { 101 | int idx1 = mvMatches12[i].first; 102 | int idx2 = mvMatches12[i].second; 103 | // get indices for the corresponding camera 104 | int camidx1 = referenceFrame.keypoint_to_cam.find(idx1)->second; 105 | int camidx2 = currentFrame.keypoint_to_cam.find(idx2)->second; 106 | // save which index belongs to which bearing vector, so that we can recover the 107 | // observations later 108 | bear1_cont_indices[camidx1].push_back(idx1); 109 | bear1[camidx1].push_back(opengv::bearingVector_t(mvKeysRays1[idx1](0), 110 | mvKeysRays1[idx1](1), mvKeysRays1[idx1](2))); 111 | 112 | bear2_cont_indices[camidx2].push_back(idx2); 113 | bear2[camidx2].push_back(opengv::bearingVector_t(mvKeysRays2[idx2](0), 114 | mvKeysRays2[idx2](1), mvKeysRays2[idx2](2))); 115 | } 116 | 117 | vector nr_recon(nrCams); 118 | vector Rel_Rs(nrCams); 119 | vector Rel_ts(nrCams); 120 | vector > triangulated(nrCams); 121 | vector > vP3Ds(nrCams); 122 | vector normsAll(nrCams); 123 | // calculate an essential matrix for each camera separately 124 | // ge, 17pt and 6pt are just too slow 125 | // then take the one with the most 126 | // inliers? biggest norm? most reconstructed pts? 127 | for (int c = 0; c < nrCams; ++c) 128 | { 129 | nr_recon[c] = 0; 130 | normsAll[c] = 0; 131 | opengv::relative_pose::CentralRelativeAdapter adapter(bear1[c], bear2[c]); 132 | 133 | opengv::sac::Ransac< 134 | opengv::sac_problems::relative_pose::CentralRelativePoseSacProblem > ransac; 135 | 136 | std::shared_ptr< 137 | opengv::sac_problems::relative_pose::CentralRelativePoseSacProblem> relposeproblem_ptr( 138 | new opengv::sac_problems::relative_pose::CentralRelativePoseSacProblem( 139 | adapter, 140 | opengv::sac_problems::relative_pose::CentralRelativePoseSacProblem::STEWENIUS)); 141 | 142 | ransac.sac_model_ = relposeproblem_ptr; 143 | ransac.threshold_ = 0.0001; 144 | ransac.max_iterations_ = 200; 145 | 146 | ransac.computeModel(); 147 | Eigen::Matrix3d R = ransac.model_coefficients_.block<3, 3>(0, 0); 148 | Eigen::Vector3d t = ransac.model_coefficients_.block<3, 1>(0, 3); 149 | Rel_Rs[c] = cConverter::toCvMat(R); 150 | Rel_ts[c] = cConverter::toCvVec3d(t); 151 | vector norms; 152 | vector vInlierMatches12; 153 | // recover inlier measurements 154 | for (int i = 0; i < ransac.inliers_.size(); ++i) 155 | { 156 | int idx = ransac.inliers_[i]; 157 | int cam_matchidx1 = bear1_cont_indices[c][idx]; 158 | int cam_matchidx2 = bear2_cont_indices[c][idx]; 159 | vInlierMatches12.push_back(make_pair(cam_matchidx1, 160 | cam_matchidx2)); 161 | cv::Vec3d bear1 = mvKeysRays1[cam_matchidx1]; 162 | cv::Vec3d bear2 = mvKeysRays2[cam_matchidx2]; 163 | cv::Vec3d res = bear1.cross(cConverter::toCvMat(R)*bear2); 164 | norms.push_back(cv::norm(res)); 165 | } 166 | if (ransac.inliers_.size() <= 0) 167 | continue; 168 | 169 | normsAll[c] = median(norms); 170 | //if (normsAll[c] < 0.02) 171 | // continue; 172 | nr_recon[c] = CheckRT(currentFrame, Rel_Rs[c], Rel_ts[c], 173 | mvKeys1, mvKeys2, 174 | mvKeysRays1, mvKeysRays2, vInlierMatches12, 175 | vP3Ds[c], 5, triangulated[c], 1.0, c); 176 | } 177 | 178 | // find cam with most reconstructed points 179 | bool init = false; 180 | for (int c = 0; c < nrCams; ++c) 181 | { 182 | if (nr_recon[c] > 60 && 183 | normsAll[c] > 0.06) 184 | { 185 | init = true; 186 | bestCam = c; 187 | if (c > 0) 188 | if (normsAll[c] > normsAll[c - 1]) 189 | bestCam = c; 190 | } 191 | } 192 | vbTriangulated = triangulated[bestCam]; 193 | vP3D = vP3Ds[bestCam]; 194 | R21 = Rel_Rs[bestCam]; 195 | t21 = Rel_ts[bestCam]; 196 | 197 | return init; 198 | } 199 | 200 | int cMultiInitializer::CheckRT(const cMultiFrame& CurrentFrame, 201 | const cv::Matx33d& R, 202 | const cv::Vec3d& t, 203 | const std::vector& vKeys1, 204 | const std::vector& vKeys2, 205 | const std::vector& vKeysRays1, 206 | const std::vector& vKeysRays2, 207 | const std::vector& vMatches12, 208 | std::vector& vP3D, 209 | double th2, 210 | std::vector& vbGood, 211 | double parallax, 212 | int currCam) 213 | { 214 | double cosThresh = cos(parallax / RHOd); 215 | vbGood = std::vector(vKeys1.size(), false); 216 | vP3D.resize(vKeys1.size()); 217 | 218 | std::vector vCosParallax; 219 | vCosParallax.reserve(vKeys1.size()); 220 | 221 | cv::Vec3d O1(0, 0, 0); 222 | cv::Vec3d O2 = -R.t()*t; 223 | int nGood = 0; 224 | 225 | for (size_t i = 0, iend = vMatches12.size(); i < iend; ++i) 226 | { 227 | int currCam1 = CurrentFrame.keypoint_to_cam.find(vMatches12[i].second)->second; 228 | if (currCam1 != currCam) 229 | continue; 230 | const cv::Vec3d &kpRay1 = vKeysRays1[vMatches12[i].first]; 231 | const cv::Vec3d &kpRay2 = vKeysRays2[vMatches12[i].second]; 232 | 233 | const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first]; 234 | const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second]; 235 | 236 | cv::Vec3d p3dC1; 237 | p3dC1 = triangulate_point(t, R, kpRay1, kpRay2); 238 | 239 | if (!isfinite(p3dC1(0)) || !isfinite(p3dC1(1)) || !isfinite(p3dC1(2))) 240 | { 241 | vbGood[vMatches12[i].first] = false; 242 | continue; 243 | } 244 | 245 | // Check parallax 246 | cv::Vec3d normal1 = p3dC1 - O1; 247 | double dist1 = cv::norm(normal1); 248 | 249 | cv::Vec3d normal2 = p3dC1 - O2; 250 | double dist2 = cv::norm(normal2); 251 | 252 | double cosParallax = normal1.dot(normal2) / (dist1*dist2); 253 | 254 | if (p3dC1(2) <= 0 && cosParallax > cosThresh) 255 | continue; 256 | 257 | cv::Vec3d p3dC2 = R.t()*(p3dC1 - t); 258 | 259 | if (p3dC2(2) <= 0 && cosParallax > cosThresh) 260 | continue; 261 | 262 | // Check reprojection error in first image 263 | double u = 0.0; 264 | double v = 0.0; 265 | camSystem.GetCamModelObj(currCam).WorldToImg( 266 | p3dC1(0), p3dC1(1), p3dC1(2), u, v); 267 | 268 | double squareError1 = cv::pow(u - cv::saturate_cast(kp1.pt.x), 2) + 269 | cv::pow(v - cv::saturate_cast(kp1.pt.y), 2); 270 | 271 | if (squareError1 > th2) 272 | continue; 273 | 274 | // Check reprojection error in second image 275 | camSystem.GetCamModelObj(currCam).WorldToImg( 276 | p3dC2(0), p3dC2(1), p3dC2(2), u, v); 277 | 278 | double squareError2 = cv::pow(u - cv::saturate_cast(kp2.pt.x), 2) + 279 | cv::pow(v - cv::saturate_cast(kp2.pt.y), 2); 280 | 281 | if (squareError2 > th2) 282 | continue; 283 | 284 | if (cosParallax < cosThresh) 285 | { 286 | vbGood[vMatches12[i].first] = true; 287 | vCosParallax.push_back(cosParallax); 288 | vP3D[vMatches12[i].first] = p3dC1; 289 | ++nGood; 290 | } 291 | } 292 | 293 | double minParallax = 0.0; 294 | if (nGood > 0) 295 | { 296 | std::vector::iterator result = 297 | std::min_element(std::begin(vCosParallax), std::end(vCosParallax)); 298 | 299 | minParallax = acos(*result) * 180 / CV_PI; 300 | } 301 | else 302 | parallax = 0; 303 | 304 | if (minParallax < parallax) 305 | nGood = 0; 306 | return nGood; 307 | } 308 | 309 | } -------------------------------------------------------------------------------- /include/cMultiKeyFrame.h: -------------------------------------------------------------------------------- 1 | // The original version was released under the following license 2 | /** 3 | * This file is part of MultiCol-SLAM 4 | * 5 | * Copyright (C) 2015-2016 Steffen Urban 6 | * For more information see 7 | * 8 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * MultiCol-SLAM is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with MultiCol-SLAM . If not, see . 20 | */ 21 | 22 | /* 23 | * MultiCol-SLAM is based on ORB-SLAM2 which was also released under GPLv3 24 | * For more information see 25 | * Raúl Mur-Artal (University of Zaragoza) 26 | */ 27 | 28 | #ifndef MULTIKEYFRAME_H 29 | #define MULTIKEYFRAME_H 30 | 31 | 32 | // external 33 | #include 34 | #include 35 | #include 36 | #include 37 | // third party 38 | #include "DBoW2/DBoW2/BowVector.h" 39 | #include "DBoW2/DBoW2/FeatureVector.h" 40 | // ówn includes 41 | #include "cam_system_omni.h" 42 | #include "misc.h" 43 | #include "cMapPoint.h" 44 | #include "cORBVocabulary.h" 45 | #include "cMultiFrame.h" 46 | #include "cMultiKeyFrameDatabase.h" 47 | 48 | namespace MultiColSLAM 49 | { 50 | class cMap; 51 | class cMapPoint; 52 | class cMultiFrame; 53 | class cMultiKeyFrameDatabase; 54 | class DatabaseResult; 55 | class cCamModelGeneral_; 56 | class cMultiCamSys_; 57 | 58 | class cMultiKeyFrame 59 | { 60 | public: 61 | cMultiKeyFrame(cMultiFrame &F, 62 | cMap* pMap, 63 | cMultiKeyFrameDatabase* pKFDB); 64 | 65 | // Pose functions 66 | void SetPose(const cv::Matx33d &Rcw, 67 | const cv::Vec3d &tcw); 68 | void SetPose(const cv::Matx44d &Tcw); 69 | void SetPose(const cv::Matx61d &Tcw_min_); 70 | cv::Matx44d GetPose(); 71 | cv::Matx44d GetPoseInverse(); 72 | cv::Vec3d GetCameraCenter(); 73 | cv::Matx33d GetRotation(); 74 | cv::Vec3d GetTranslation(); 75 | 76 | // Calibration 77 | // Bag of Words Representation 78 | void ComputeBoW(); 79 | DBoW2::FeatureVector GetFeatureVector(); 80 | DBoW2::FeatureVector GetFeatureVector(int& c); 81 | DBoW2::BowVector GetBowVector(); 82 | DBoW2::BowVector GetBowVector(int& c); 83 | 84 | // Covisibility graph functions 85 | void AddConnection(cMultiKeyFrame* pKF, const int &weight); 86 | void EraseConnection(cMultiKeyFrame* pKF); 87 | void UpdateConnections(); 88 | void UpdateBestCovisibles(); 89 | std::set GetConnectedKeyFrames(); 90 | std::vector GetVectorCovisibleKeyFrames(); 91 | std::vector GetBestCovisibilityKeyFrames(const int &N); 92 | std::vector GetCovisiblesByWeight(const int &w); 93 | int GetWeight(cMultiKeyFrame* pKF); 94 | 95 | // Spanning tree functions 96 | void AddChild(cMultiKeyFrame* pKF); 97 | void EraseChild(cMultiKeyFrame* pKF); 98 | void ChangeParent(cMultiKeyFrame* pKF); 99 | std::set GetChilds(); 100 | cMultiKeyFrame* GetParent(); 101 | bool hasChild(cMultiKeyFrame* pKF); 102 | 103 | // Loop Edges 104 | void AddLoopEdge(cMultiKeyFrame* pKF); 105 | std::set GetLoopEdges(); 106 | 107 | // MapPoint observation functions 108 | void AddMapPoint(cMapPoint* pMP, const size_t &idx); 109 | void EraseMapPointMatch(const size_t &idx); 110 | //void EraseMapPointMatch(cMapPoint* pMP,const size_t& idx); 111 | void ReplaceMapPointMatch(const size_t &idx, cMapPoint* pMP); 112 | std::set GetMapPoints(); 113 | std::vector GetMapPointMatches(); 114 | int TrackedMapPoints(); 115 | cMapPoint* GetMapPoint(const size_t &idx); 116 | 117 | // KeyPoint functions 118 | cv::KeyPoint GetKeyPoint(const size_t &idx) const; // 2D Point 119 | cv::Vec3d GetKeyPointRay(const size_t &idx) const; // 3D ray 120 | 121 | // keypoint functions 122 | int GetKeyPointScaleLevel(const size_t &idx) const; 123 | std::vector GetKeyPoints() const; 124 | std::vector GetKeyPointsRays() const; 125 | 126 | // descriptor get functions 127 | cv::Mat GetDescriptor(const int& cam, const size_t &idx) const; 128 | const uint64_t* GetDescriptorRowPtr(const int& cam, const size_t &idx) const; // this is waaaayy faster 129 | std::vector GetAllDescriptors() const { return mDescriptors; } 130 | std::vector GetAllDescriptorMasks() const { return mDescriptorMasks; } 131 | cv::Mat GetDescriptors(int c) const { return mDescriptors[c]; } 132 | cv::Mat GetDescriptorMask(const int& cam, const size_t &idx) const; 133 | cv::Mat GetDescriptorsMasks(int c) const { return mDescriptorMasks[c]; } 134 | const uint64_t* GetDescriptorMaskRowPtr(const int& cam, const size_t &idx) const; // this is waaaayy faster 135 | 136 | std::vector GetFeaturesInArea(const int& cam, const double &x, 137 | const double &y, const double &r) const; 138 | 139 | // Image 140 | cv::Mat GetImage(const int& cam); 141 | std::vector GetAllImages() { return images; } 142 | bool IsInImage(const int& cam, const double &x, const double &y) const; 143 | 144 | // Activate/deactivate erasable flags 145 | void SetNotErase(); 146 | void SetErase(); 147 | 148 | // Set/check erased 149 | void SetBadFlag(); 150 | bool isBad(); 151 | 152 | // Scale functions 153 | double inline GetScaleFactor(int nLevel = 1) const{ 154 | return mvScaleFactors[nLevel]; 155 | } 156 | std::vector inline GetScaleFactors() const{ 157 | return mvScaleFactors; 158 | } 159 | std::vector inline GetVectorScaleSigma2() const{ 160 | return mvLevelSigma2; 161 | } 162 | double inline GetSigma2(int nLevel = 1) const{ 163 | return mvLevelSigma2[nLevel]; 164 | } 165 | double inline GetInvSigma2(int nLevel = 1) const{ 166 | return mvInvLevelSigma2[nLevel]; 167 | } 168 | int inline GetScaleLevels() const{ 169 | return mnScaleLevels; 170 | } 171 | 172 | // Median MapPoint depth 173 | double ComputeSceneMedianDepth(int q = 2); 174 | 175 | static long unsigned int nNextId; 176 | long unsigned int mnId; 177 | long unsigned int mnFrameId; 178 | 179 | double mTimeStamp; 180 | 181 | // Grid (to speed up feature matching) 182 | std::vector mnGridCols; 183 | std::vector mnGridRows; 184 | std::vector mfGridElementWidthInv; 185 | std::vector mfGridElementHeightInv; 186 | 187 | // Variables used by the tracking 188 | long unsigned int mnTrackReferenceForFrame; 189 | long unsigned int mnFuseTargetForKF; 190 | 191 | // Variables used by the local mapping 192 | long unsigned int mnBALocalForKF; 193 | long unsigned int mnBAFixedForKF; 194 | 195 | // Variables used by the keyframe database 196 | long unsigned int mnLoopQuery; 197 | int mnLoopWords; 198 | double mLoopScore; 199 | long unsigned int mnRelocQuery; 200 | int mnRelocWords; 201 | double mRelocScore; 202 | 203 | //BoW 204 | // for all cams combined 205 | DBoW2::BowVector mBowVec; 206 | // for each cam 207 | std::vector mBowVecs; 208 | 209 | static bool weightComp(int a, int b) { return a > b; } 210 | 211 | static bool lId(cMultiKeyFrame* pKF1, cMultiKeyFrame* pKF2){ 212 | return pKF1->mnId < pKF2->mnId; 213 | } 214 | 215 | // Calibration, camera model, camera system 216 | // all poses are stored in this class 217 | cMultiCamSys_ camSystem; 218 | 219 | // this hashmap holds the mapping between keypoint ID and camera 220 | // it was observed in 221 | // [key_id : cam_id] 222 | std::unordered_map keypoint_to_cam; 223 | // this hashmap holds the mapping between the continous indexing of all 224 | // descriptors and keypoints and the image wise indexes 225 | // it was observed in 226 | // [cont_id : local_image_id] 227 | std::unordered_map cont_idx_to_local_cam_idx; 228 | 229 | // other infos/statistics 230 | size_t GetValidMapPointCnt(); 231 | size_t GetNrKeypointsInFrame(); 232 | 233 | // infos about the descriptors 234 | bool HavingMasks() { return masksLearned; } 235 | int DescDims() { return descDimension; } 236 | bool IsReference(); 237 | void SetReference(const bool ref); 238 | bool IsLoopCandidate(); 239 | void SetLoopCandidate(const bool ref); 240 | int imageId; 241 | 242 | protected: 243 | 244 | bool mdBRIEF; 245 | bool masksLearned; 246 | int descDimension; 247 | bool IamTheReference; 248 | bool IamLoopCandidate; 249 | bool havingEdgeMeasurements; 250 | 251 | // Original images 252 | std::vector images; 253 | 254 | // assign those boundaries for each invdividual image 255 | std::vector mnMinX; 256 | std::vector mnMinY; 257 | std::vector mnMaxX; 258 | std::vector mnMaxY; 259 | 260 | // KeyPoints, Descriptors, MapPoints vectors (all associated by an index) 261 | // keypoints are saved contiously, i.e. they are assigned to the corresponding camera 262 | // by an unordered_map 263 | std::vector mvKeys; 264 | std::vector mvKeysRays; 265 | std::vector mDescriptors; 266 | std::vector mDescriptorMasks; 267 | std::vector mvpMapPoints; 268 | 269 | // BoW 270 | cMultiKeyFrameDatabase* mpKeyFrameDB; 271 | ORBVocabulary* mpORBvocabulary; 272 | 273 | DBoW2::FeatureVector mFeatVec; 274 | std::vector mFeatVecs; 275 | 276 | // Grid over all images to speed up feature matching 277 | std::vector< std::vector< std::vector< std::vector< std::size_t > > > > mGrids; 278 | 279 | std::map mConnectedKeyFrameWeights; 280 | std::vector mvpOrderedConnectedKeyFrames; 281 | std::vector mvOrderedWeights; 282 | 283 | // Spanning Tree and Loop Edges 284 | bool mbFirstConnection; 285 | cMultiKeyFrame* mpParent; 286 | std::set mspChildrens; 287 | std::set mspLoopEdges; 288 | 289 | // Erase flags 290 | bool mbNotErase; 291 | bool mbToBeErased; 292 | bool mbBad; 293 | 294 | // Scale 295 | int mnScaleLevels; 296 | std::vector mvScaleFactors; 297 | std::vector mvLevelSigma2; 298 | std::vector mvInvLevelSigma2; 299 | 300 | cMap* mpMap; 301 | 302 | std::mutex mMutexPose; 303 | std::mutex mMutexConnections; 304 | std::mutex mMutexFeatures; 305 | std::mutex mMutexImage; 306 | std::mutex mMutexRenderedImages; 307 | std::mutex mMutexEdgeImages; 308 | std::mutex mMutexModelPts; 309 | std::mutex mMutexProperties; 310 | }; 311 | 312 | } 313 | #endif // KEYFRAME_H 314 | -------------------------------------------------------------------------------- /include/mdBRIEFextractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM. 3 | * 4 | * Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM. If not, see . 19 | */ 20 | 21 | #ifndef MDBRIEFEXTRACTOR_H 22 | #define MDBRIEFEXTRACTOR_H 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include "BaseComponents/slam_multi_cam_fisheye/multi_cam_model_omni.h" 29 | 30 | 31 | static int learned_orb_pattern_64_fisheyeOCAM_corr2[4 * 512] = 32 | { 33 | -14, -14, -8, 4, -14, 2, -14, 9, -1, -11, 0, 14, 6, 2, 8, 15, 7, 8, 8, -11, 2, -9, 3, -14, -14, 12, -11, 10, 12, -10, 15, -12, -5, 10, -5, 14, 12, 13, 15, 15, -7, 3, -6, 0, -13, -11, -12, -14, -14, -14, -11, -13, 9, -13, 12, -11, 14, 1, 15, 4, -12, 8, -9, 11, -4, -2, -2, 1, 7, 15, 10, 14, -7, -13, -4, -14, 4, 2, 7, 4, -2, -10, -1, -3, 13, 5, 15, 2, 11, -2, 13, -5, 14, -14, 14, -7, -4, 9, -3, 5, 13, -4, 15, -1, 0, 0, 1, 4, 7, 12, 9, 15, 1, 3, 2, 0, 2, 15, 3, 12, -14, -2, -12, -5, -5, -14, -4, -11, 11, 5, 14, 6, -11, -1, -8, 0, -5, -2, -4, -5, -8, 10, -5, 8, 7, 7, 9, 4, 5, -5, 7, -2, 5, -13, 8, -14, 14, 7, 15, 10, 8, -1, 11, -2, 0, -14, 3, -13, -6, 13, -3, 15, -7, 0, -4, -1, 2, 0, 5, -4, -1, 15, 3, 15, -9, 15, -8, -10, 14, 14, 15, -10, -14, 4, -13, -3, 6, -4, 7, -8, -11, 6, -9, 3, 7, -2, 8, 1, 13, 15, 14, 12, 7, 1, 8, 5, -14, 8, -14, 13, 2, 12, 3, 15, -9, 2, -8, 5, -14, -6, -11, -4, 8, -8, 10, -11, -14, 1, -12, 4, -6, -4, -5, -1, 13, 10, 14, 7, 1, 9, 2, 12, -7, -9, -5, -6, -11, -14, -8, -11, -12, -5, -11, -8, 12, -7, 13, -4, 4, -11, 5, -8, 2, 3, 2, 7, -7, -11, -6, -14, -6, 7, -5, 10, 11, 11, 14, 10, -10, 12, -9, 9, 11, -4, 15, -4, -12, -7, -9, -8, 9, 1, 12, 3, 14, -5, 15, -8, -14, -13, -13, -9, 2, 11, 4, 7, -11, 15, -7, 15, 3, -11, 9, 15, 0, -7, 2, -10, 6, 8, 9, 10, 14, -11, 15, -14, -14, -8, -4, 12, -1, 0, 2, 1, -3, -2, 2, 15, 15, 10, 15, 14, 4, 8, 7, 7, -4, 11, 1, -5, -12, 6, -5, -14, 3, 15, 12, -12, 1, -7, 10, -6, 3, -14, 3, 4, 8, -1, 9, -4, 14, -13, 14, 6, 4, -5, 5, 9, 11, 0, 15, 12, -5, -11, -3, 8, -6, 1, -6, 8, -2, -12, -1, -8, 9, 8, 11, 11, 13, 3, 15, -3, -10, 11, -10, 15, -14, 8, -11, 5, 10, -5, 12, 8, -3, 12, -2, 9, -7, -2, -6, 3, -9, 15, -7, 12, -10, 0, -9, -3, -9, 7, -9, 11, 9, 2, 10, -1, 9, 5, 10, 2, 0, -6, 1, 5, 9, -9, 10, -6, -11, 5, -10, 8, -13, -3, -11, 0, 3, 6, 4, 9, -9, -5, -7, -2, 12, 9, 15, 10, -4, 15, -3, 12, -14, 3, -11, 2, 8, -14, 8, -10, -1, -5, 0, -2, 8, 15, 9, 12, -3, 6, -2, 3, 9, 11, 10, 8, 8, 4, 9, 7, 4, -7, 5, -4, 11, 11, 13, 14, 6, 3, 7, 0, 12, 0, 15, 1, -14, 6, -11, 7, -9, -8, -8, -11, 0, 6, 0, 10, 12, -9, 15, -8, -2, 12, -1, 15, 6, 10, 7, 13, 6, -5, 6, 0, 4, 8, 5, 5, -5, 1, -4, -2, 1, -4, 2, -7, -8, 9, -7, 6, -11, 10, -8, 9, -4, -7, -3, -10, 11, 3, 14, 2, 9, -10, 12, -11, -12, 1, -9, -1, -14, 14, -11, 15, -3, 3, -2, 6, -8, -11, -7, -8, 9, 13, 12, 14, -10, -7, -7, -6, -10, 5, -7, 4, 2, 11, 4, -12, -10, 15, -4, -2, -1, 0, 0, -3, -12, -13, -9, -14, 9, -11, 10, -14, -14, -7, -13, -3, -6, -10, -6, -1, -9, -2, -7, -5, 9, -3, 12, -1, -9, 12, -7, 15, 8, -5, 11, -7, 7, 0, 15, -14, -14, 15, -13, 11, -7, 3, -5, 6, -10, -5, -10, -1, -9, 0, -6, 2, 2, -14, 4, -10, -7, 15, -4, 14, -7, -12, -4, -10, 8, -7, 11, -6, -7, 6, -4, 3, 2, -11, 5, -14, 9, 13, 14, -2, 8, 5, 11, 4, 4, 13, 7, 15, 7, -9, 12, 3, -9, 8, -6, 9, 10, 6, 10, 10, -9, -9, -6, -10, -5, -7, -2, -4, -2, -14, 3, 10, -6, -5, -4, -8, -4, 14, -1, 12, 2, -3, 5, 3, -3, 9, -1, 12, -11, -10, -11, -6, 3, 15, 6, 14, -3, -13, 0, -14, 3, -13, 6, -12, 0, 11, 3, 14, 7, 5, 15, -6, 4, -3, 12, 12, 6, 4, 9, 5, 6, -2, 9, -1, 0, -8, 3, -5, -10, -14, 0, 5, -3, 9, 0, 7, -5, 8, -2, 9, -4, 7, 1, -13, -4, -7, -1, -8, -3, 2, 0, 0, 1, 7, 4, 8, 5, 12, 14, 6, -12, 3, -1, 15, -14, -1, -2, -8, 0, -12, 15, 10, -14, 15, 3, -1, -13, -14, 4, -11, -14, -9, -11, 9, -13, -13, -9, -6, -10, 4, -9, -1, 1, -5, 2, 15, -12, 2, -11, 6, -7, 13, -5, 8, -2, -3, -2, 10, 14, 8, 15, -1, -3, -7, -2, 2, 4, -6, 5, -11, 9, -5, 11, -9, -14, 15, -9, 3, 8, -10, 10, 13, 8, -10, 8, 1, -14, 7, -13, 10, -14, 14, -10, -4, 12, 12, 13, 9, 8, -12, 8, -5, 12, -6, 15, -9, -5, 15, -4, -7, -14, -4, -13, -7, 9, -6, 10, -3, -14, -11, -11, -9, -4, 6, -4, 11, 10, 2, 11, -5, 11, 8, 12, 4, -10, -5, -8, 3, 11, 1, 13, 5, 8, -4, 9, -1, -6, 5, -5, 2, -1, 6, -1, 15, 11, -13, 12, -10, -2, -14, -1, -11, -7, 5, -7, 9, 10, 5, 12, 8, -14, 11, -13, 14, 11, 6, 15, 14, -3, -6, -2, -3, -6, -14, -4, -5, 5, 1, 6, 9, 11, 4, 12, 0, 11, -1, 13, 2, -14, 5, -12, 1, -8, -13, -6, -10, -9, -5, -8, 9, 4, 8, 5, 11, -10, -7, -8, -4, 12, -13, 15, -14, 10, -9, 13, -13, 12, 1, 15, -10, -1, 15, 0, 12, -2, 3, -2, 9, -2, 10, -1, 7, -10, -14, -6, 11, 8, -1, 9, 3, -4, -4, -3, 5, 1, -2, 2, 1, 6, 14, 7, -14, -10, 10, -8, 7, -10, 0, -9, 4, 6, -10, 8, -14, 0, -10, 1, -7, -14, -8, -13, -11, 13, -10, 15, -7, 3, -5, 4, -8, -11, 9, -10, 12, 12, 3, 15, 6, -6, 7, -5, 4, -14, 9, -11, 8, -13, 2, -11, -1, -4, -9, -3, -6, 11, -5, 12, -1, -14, 4, -12, 7, -14, -2, -11, -1, 7, -7, 8, -4, 8, -3, 9, -6, 5, -1, 6, 2, 6, 6, 7, 9, -6, 1, -5, 4, -12, 15, -9, 13, 5, -9, 6, -12, 5, -2, 6, -6, 1, -7, 2, -4, 4, -9, 5, 15, -11, -2, -10, -6, -9, 5, -7, 2, 7, 1, 8, -3, 3, -5, 4, -1, -9, 3, -9, 7, -1, -11, 0, -14, -13, 6, -10, 5, 0, 12, 1, 9, 14, -5, 15, 3, -4, 4, -3, 1, 4, 1, 5, 4, -2, -1, -1, 2, -14, 14, -12, -14, 4, 4, 5, -5, 11, -8, 14, -9, -6, 10, -4, 7, -6, -6, -5, -3, 15, -10, 15, -4, -11, -7, -10, -10, -1, -9, 0, -12, 1, 7, 2, -1, 12, -1, 15, -2, -10, -11, -9, -14, -14, -3, -11, -4, -9, 10, -8, 13, 10, -13, 13, -14, 8, -3, 9, 12, 2, -10, 2, -5, 2, -14, 4, 14, 11, 7, 14, 9, -12, 14, -10, 10, -9, -4, -8, -7, -14, -1, -13, 3, -11, -10, -8, -8, -13, 2, -10, 3, 7, -7, 8, -10, -12, -4, -9, -3, -1, 8, 0, 5, 5, 6, 6, 0, -1, 2, -1, 6, -4, -14, -4, 14, 12, 8, 15, 7, -4, -11, -3, -14, 5, 13, 6, 10, 2, -12, 2, -8, 7, 8, 8, 11, -5, 12, -4, 15, -14, 2, -13, -9, 6, 11, 7, 8, 8, 2, 10, 5, 12, 15, 15, 14, 2, 13, 3, 2, 3, 6, 4, 3, 10, 10, 13, 12, -7, 3, -6, -5, -14, 9, -12, 12, -4, 1, -3, 5, 1, 5, 2, 8, 1, 1, 2, -7, -1, 4, 0, -2, -1, -3, 0, 0, 9, 10, 15, -14, -14, -6, -11, -7, -14, 10, -11, 11, -2, 4, -1, 1, 3, 7, 3, 14, 0, 11, 0, 15, -5, 9, -4, 12, -2, -4, -1, -14, 4, 11, 6, 14, -14, -10, -11, -11, 12, -13, 15, -11, -5, 5, -4, 8, -12, 2, -11, 14, -2, 0, -2, 4, -12, 8, -11, 4, 3, -10, 5, 8, 11, -6, 14, -4, -1, 4, 0, 7, 3, -1, 3, 3, 9, 7, 10, 15, -2, -2, -1, -8, 11, 1, 14, -1, 2, 2, 3, -1, 11, -4, 12, -13, -14, -4, -13, 0, 7, 7, 9, -4, -2, 7, -1, 10, -12, 12, -10, 15, 7, -12, 10, -14, -13, -14, -11, -11, -9, 13, -6, 11, 10, -10, 13, -9, -7, 9, -4, -4, 9, 4, 12, 6, -11, 3, -8, 4, -14, 10, -13, 6, 9, 8, 12, 9, 7, 5, 10, 9, 6, 4, 8, 7, -3, -1, -2, -4, 15, -6, 15, -2, 9, -14, 15, 12, -9, -14, -6, -13, 6, -14, 8, -11, 10, 8, 13, 7, -12, 4, -9, 7, 10, 6, 13, 3, -10, -1, -9, -14, -13, -2, -8, 4, 8, 13, 10, 10, -12, -3, -9, -5, 10, 1, 10, 6, 11, -14, 15, 0, 7, -10, 9, -7, -8, 1, -6, -2, -9, 5, -7, 8, -10, 1, -7, 0, -10, -14, -9, -10, -13, 12, -9, 12, -9, -4, -6, -3, -9, 8, -7, 11, 13, 4, 14, 7, 8, -2, 11, -5, -2, -8, 1, 10, -13, 8, -8, -6, -11, -8, -10, -5, -8, -2, -6, 15, 6, 0, 8, 3, -12, -13, -4, -2, -7, -8, -3, 12, 3, 0, 8, -14, 11, -2, 11, 2, 9, 12, 12, 11, 4, 15, 6, 12, 7, -9, 10, -10, -14, 12, -13, 9, -4, 15, 0, -1, 14, 2, 14, 15, -8, -9, -6, -12, -8, -3, -5, -1, -14, -9, -13, -6, 6, 9, 8, 6, -12, -9, -9, -12, 6, 7, 6, 11, 7, 11, 10, 12, -9, -6, -7, -9, 5, -8, 8, -11, -14, 10, -6, 3, 5, 11, 5, 15, 9, 2, 12, 0, 5, 7, 10, 15, 7, -14, 10, -13, 10, 0, 14, 0, 13, -8, 14, -11, 5, -7, 10, 9, 1, 5, 5, 15, -5, -11, -2, -8, -2, -11, 2, 2, -14, -12, -8, -11, -9, 4, -6, 5, 2, 2, 4, 7, -9, -5, -4, 4, 5, -5, 8, -8, -10, 11, -7, 13, 0, 7, 2, 3, -9, -12, -6, -14, 9, -3, 13, -4, -13, 7, -8, 8, -6, -14, 0, 14, -8, 13, -5, 14, 4, 13, 7, -5, -4, -9, -2, -12, 7, -5, 10, -2, -7, -10, -7, -6, -7, -8, -4, -7, -7, 7, -4, 6, -2, 15, 2, -13, 5, -14, 9, 0, 10, 15, 15, 10, -4, -14, -1, -13, 7, 1, 10, 3, 1, -5, 3, -2, 4, 7, 7, 10, 0, 2, 2, 5, 6, -10, 9, -9, 7, 4, 10, 1, 0, 7, 2, 10, -8, 12, -3, -11, -2, 1, 0, 4, 3, 4, 5, 1, -2, -5, 0, -9, 6, 11, 9, 10, 4, 8, 11, -13, 4, -11, 7, -9, 3, -10, 6, -11, -1, 14, 2, 11, 6, -4, 9, -5, 2, 13, 5, 14, -1, -10, 2, -13, -7, 1, -4, 3, 4, 10, 7, 11, -1, -11, 2, -8, 6, 0, 9, -3, 7, -4, 15, 6, 4, 12, 7, 10, -1, -2, 1, -5, -6, 15, 0, 5, -7, 9, -4, 11, 7, 6, 11, 7, 6, 7, 9, 6, 5, -7, 8, -5, -14, 0, -7, 14, -3, 14, 0, 15, 6, -7, 15, -13, 11, 11, 11, 15 34 | }; 35 | 36 | 37 | //namespace ORB_SLAM 38 | //{ 39 | 40 | class mdBRIEFextractor 41 | { 42 | public: 43 | 44 | enum { HARRIS_SCORE=0, FAST_SCORE=1 }; 45 | 46 | //mdBRIEFextractor( 47 | // int _nfeatures = 1000, 48 | // double _scaleFactor = 1.2, 49 | // int _nlevels = 8, 50 | // int _scoreType = HARRIS_SCORE, 51 | // int _fastTh = 20, 52 | // bool _learnMask = false, 53 | // bool _useAgast = false, 54 | // int _fastAgastType = 2, 55 | // int _descSize = 32); 56 | mdBRIEFextractor(int _nfeatures = 1000, 57 | float _scaleFactor = 1.2, 58 | int _nlevels = 8, 59 | int _edgeThreshold = 25, 60 | int _firstLevel = 0, 61 | int _scoreType = HARRIS_SCORE, 62 | int _patchSize = 32, 63 | int _fastThreshold = 20, 64 | bool _useAgast = false, 65 | int _fastAgastType = 2, 66 | bool _learnMasks = false, 67 | int _descSize = 32); 68 | 69 | ~mdBRIEFextractor(){} 70 | 71 | // Compute the ORB features and descriptors on an image 72 | void operator()( 73 | cv::InputArray _image, 74 | cv::InputArray _mask, 75 | std::vector& _keypoints, 76 | cCamModelGeneral_& camModel, 77 | cv::OutputArray _descriptors, 78 | cv::OutputArray _descriptorMasks); 79 | 80 | int inline GetLevels() { return numlevels; } 81 | 82 | double inline GetScaleFactor() { return scaleFactor; } 83 | 84 | bool GetMasksLearned() { return learnMasks; } 85 | int GetDescriptorSize() { return descSize; } 86 | 87 | static int HALF_PATCH_SIZE; 88 | static int EDGE_THRESHOLD; 89 | 90 | protected: 91 | // returns the descriptor size in bytes 92 | int descriptorSize() const; 93 | // returns the descriptor type 94 | int descriptorType() const; 95 | // returns the default norm type 96 | int defaultNorm() const; 97 | 98 | void ComputePyramid(cv::Mat image, cv::Mat Mask=cv::Mat()); 99 | void ComputeKeyPoints(std::vector >& allKeypoints); 100 | 101 | std::vector pattern; 102 | 103 | // int nfeatures; 104 | // double scaleFactor; 105 | // int nlevels; 106 | // int scoreType; 107 | // int fastTh; 108 | 109 | //int descSize; 110 | //bool learnMask; 111 | //bool useAgast; 112 | //int fastAgastType; 113 | int nfeatures; 114 | double scaleFactor; 115 | int numlevels; 116 | int edgeThreshold; 117 | int firstLevel; 118 | int descSize; 119 | int scoreType; 120 | int patchSize; 121 | int fastThreshold; 122 | bool useAgast; 123 | int fastAgastType; 124 | bool learnMasks; 125 | int half_patch_size; 126 | 127 | std::vector mnFeaturesPerLevel; 128 | 129 | std::vector umax; 130 | 131 | std::vector mvScaleFactor; 132 | std::vector mvInvScaleFactor; 133 | 134 | std::vector mvImagePyramid; 135 | std::vector mvMaskPyramid; 136 | 137 | }; 138 | 139 | //} //namespace ORB_SLAM 140 | 141 | #endif 142 | 143 | -------------------------------------------------------------------------------- /src/cMultiFrame.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * 4 | * Copyright (C) 2015-2016 Steffen Urban 5 | * For more information see 6 | * 7 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * MultiCol-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with MultiCol-SLAM . If not, see . 19 | */ 20 | 21 | /* 22 | * MultiCol-SLAM is based on ORB-SLAM2 which was also released under GPLv3 23 | * For more information see 24 | * Raúl Mur-Artal (University of Zaragoza) 25 | */ 26 | 27 | #include "cMultiFrame.h" 28 | #include "cConverter.h" 29 | 30 | 31 | namespace MultiColSLAM 32 | { 33 | long unsigned int cMultiFrame::nNextId = 0; 34 | bool cMultiFrame::mbInitialComputations = true; 35 | std::vector cMultiFrame::mnMinX, cMultiFrame::mnMinY; 36 | std::vector cMultiFrame::mnMaxX, cMultiFrame::mnMaxY; 37 | 38 | cMultiFrame::cMultiFrame() 39 | {} 40 | 41 | //Copy Constructor 42 | cMultiFrame::cMultiFrame(const cMultiFrame& mframe) 43 | : 44 | mpORBvocabulary(mframe.mpORBvocabulary), 45 | images(mframe.images), 46 | mTimeStamp(mframe.mTimeStamp), 47 | camSystem(mframe.camSystem), 48 | N(mframe.N), 49 | totalN(mframe.totalN), 50 | mvKeys(mframe.mvKeys), 51 | mvKeysRays(mframe.mvKeysRays), 52 | mBowVec(mframe.mBowVec), 53 | mFeatVec(mframe.mFeatVec), 54 | mDescriptors(mframe.mDescriptors), 55 | mDescriptorMasks(mframe.mDescriptorMasks), 56 | mvpMapPoints(mframe.mvpMapPoints), 57 | mvbOutlier(mframe.mvbOutlier), 58 | mfGridElementWidthInv(mframe.mfGridElementWidthInv), 59 | mfGridElementHeightInv(mframe.mfGridElementHeightInv), 60 | mnId(mframe.mnId), 61 | mpReferenceKF(mframe.mpReferenceKF), 62 | mnScaleLevels(mframe.mnScaleLevels), 63 | mfScaleFactor(mframe.mfScaleFactor), 64 | mvScaleFactors(mframe.mvScaleFactors), 65 | mvLevelSigma2(mframe.mvLevelSigma2), 66 | mvInvLevelSigma2(mframe.mvInvLevelSigma2), 67 | keypoint_to_cam(mframe.keypoint_to_cam), 68 | cont_idx_to_local_cam_idx(mframe.cont_idx_to_local_cam_idx), 69 | mdBRIEF(mframe.mdBRIEF), 70 | masksLearned(mframe.masksLearned), 71 | descDimension(mframe.descDimension), 72 | imgCnt(mframe.imgCnt), 73 | mp_mdBRIEF_extractorOct(mframe.mp_mdBRIEF_extractorOct) 74 | { 75 | int nrCams = camSystem.GetNrCams(); 76 | mGrids.resize(nrCams); 77 | mnMinX.resize(nrCams); 78 | mnMaxX.resize(nrCams); 79 | mnMinY.resize(nrCams); 80 | mnMaxY.resize(nrCams); 81 | for (int c = 0; c < camSystem.GetNrCams(); ++c) 82 | { 83 | mGrids[c] = mframe.mGrids[c]; 84 | 85 | mnMinX[c] = 0; 86 | mnMaxX[c] = camSystem.GetCamModelObj(c).GetWidth(); 87 | mnMinY[c] = 0; 88 | mnMaxY[c] = camSystem.GetCamModelObj(c).GetHeight(); 89 | } 90 | } 91 | 92 | cMultiFrame::cMultiFrame(const std::vector& images_, 93 | const double &timeStamp, 94 | std::vector extractor, 95 | ORBVocabulary* voc, 96 | cMultiCamSys_& camSystem_, 97 | int _imgCnt) 98 | : 99 | mp_mdBRIEF_extractorOct(extractor), 100 | mpORBvocabulary(voc), 101 | images(images_), 102 | mTimeStamp(timeStamp), 103 | camSystem(camSystem_), 104 | mdBRIEF(true), 105 | imgCnt(_imgCnt) 106 | { 107 | HResClk::time_point begin = HResClk::now(); 108 | 109 | int nrCams = camSystem.GetNrCams(); 110 | mDescriptors.resize(nrCams); 111 | mDescriptorMasks.resize(nrCams); 112 | N.resize(nrCams); 113 | mnMinX.resize(nrCams); 114 | mnMaxX.resize(nrCams); 115 | mnMinY.resize(nrCams); 116 | mnMaxY.resize(nrCams); 117 | mfGridElementWidthInv.resize(nrCams); 118 | mfGridElementHeightInv.resize(nrCams); 119 | 120 | mGrids = std::vector > > >(nrCams); 121 | 122 | totalN = 0; 123 | 124 | std::vector> keyPtsTemp(nrCams); 125 | std::vector> keyRaysTemp(nrCams); 126 | int laufIdx = 0; 127 | 128 | for (int c = 0; c < nrCams; ++c) 129 | { 130 | cCamModelGeneral_ camModel = camSystem.GetCamModelObj(c); 131 | mnMinX[c] = 0; 132 | mnMaxX[c] = camModel.GetWidth(); 133 | mnMinY[c] = 0; 134 | mnMaxY[c] = camModel.GetHeight(); 135 | 136 | // First step feature extraction ORB in the mirror mask 137 | (*mp_mdBRIEF_extractorOct[c])(images[c], camModel.GetMirrorMask(0), 138 | keyPtsTemp[c], camModel, mDescriptors[c], mDescriptorMasks[c]); 139 | 140 | N[c] = (int)keyPtsTemp[c].size(); 141 | 142 | // calculate rays as observations 143 | double x = 0.0, y = 0.0, z = 0.0; 144 | keyRaysTemp[c].resize(keyPtsTemp[c].size()); 145 | for (unsigned int i = 0; i < keyPtsTemp[c].size(); i++) 146 | { 147 | camModel.ImgToWorld(x, y, z, 148 | static_cast(keyPtsTemp[c][i].pt.x), 149 | static_cast(keyPtsTemp[c][i].pt.y)); 150 | keyRaysTemp[c][i] = cv::Vec3d(x, y, z); 151 | } 152 | 153 | mfGridElementWidthInv[c] = static_cast(FRAME_GRID_COLS) / 154 | static_cast(mnMaxX[c] - mnMinX[c]); 155 | mfGridElementHeightInv[c] = static_cast(FRAME_GRID_ROWS) / 156 | static_cast(mnMaxY[c] - mnMinY[c]); 157 | //Scale Levels Info 158 | mnScaleLevels = mp_mdBRIEF_extractorOct[c]->GetLevels(); 159 | mfScaleFactor = mp_mdBRIEF_extractorOct[c]->GetScaleFactor(); 160 | 161 | mvScaleFactors.resize(mnScaleLevels); 162 | mvLevelSigma2.resize(mnScaleLevels); 163 | mvScaleFactors[0] = 1.0; 164 | mvLevelSigma2[0] = 1.0; 165 | 166 | for (int i = 1; i < mnScaleLevels; ++i) 167 | { 168 | mvScaleFactors[i] = mvScaleFactors[i - 1] * mfScaleFactor; 169 | mvLevelSigma2[i] = mvScaleFactors[i] * mvScaleFactors[i]; 170 | } 171 | 172 | mvInvLevelSigma2.resize(mvLevelSigma2.size()); 173 | for (int i = 0; i < mnScaleLevels; ++i) 174 | mvInvLevelSigma2[i] = 1 / mvLevelSigma2[i]; 175 | 176 | // Assign Features to Grid Cells 177 | mGrids[c] = std::vector > >(FRAME_GRID_COLS); 178 | for (unsigned int j = 0; j < FRAME_GRID_COLS; ++j) 179 | mGrids[c][j] = std::vector >(FRAME_GRID_ROWS); 180 | } 181 | 182 | // now save the image points in an order and fill the keypoints_to_cam variable 183 | int currPtIdx = 0; 184 | for (int c = 0; c < nrCams; ++c) 185 | { 186 | totalN += N[c]; 187 | for (int i = 0; i < keyRaysTemp[c].size(); ++i) 188 | { 189 | mvKeys.push_back(keyPtsTemp[c][i]); 190 | mvKeysRays.push_back(keyRaysTemp[c][i]); 191 | keypoint_to_cam[currPtIdx] = c; 192 | cont_idx_to_local_cam_idx[currPtIdx] = i; 193 | cv::KeyPoint &kp = keyPtsTemp[c][i]; 194 | // fill grids 195 | int nGridPosX, nGridPosY; 196 | if (PosInGrid(c, kp, nGridPosX, nGridPosY)) 197 | mGrids[c][nGridPosX][nGridPosY].push_back(currPtIdx); 198 | ++currPtIdx; 199 | } 200 | } 201 | 202 | mvbOutlier = std::vector(totalN, false); 203 | // this must not be done for each image, only for each multi keyframe 204 | mvpMapPoints = std::vector(totalN, static_cast(NULL)); 205 | // next id 206 | mnId = nNextId++; 207 | 208 | this->masksLearned = extractor[0]->GetMasksLearned(); 209 | this->descDimension = extractor[0]->GetDescriptorSize(); 210 | 211 | HResClk::time_point end = HResClk::now(); 212 | cout << "---Feature Extraction (" << T_in_ms(begin, end) << "ms) - ImageId: " << mnId << "---" << endl; 213 | } 214 | 215 | bool cMultiFrame::isInFrustum(int cam, cMapPoint *pMP, double viewingCosLimit) 216 | { 217 | pMP->mbTrackInView[cam] = false; 218 | 219 | // 3D in absolute coordinates 220 | cv::Vec3d P = pMP->GetWorldPos(); 221 | 222 | // Project in image and check it is not outside 223 | cv::Vec2d uv(0.0, 0.0); 224 | cv::Vec4d pt3 = cv::Vec4d(P(0), P(1), P(2), 1.0); 225 | camSystem.WorldToCamHom_fast(cam, pt3, uv); 226 | // check mirror border 227 | if (!camSystem.GetCamModelObj(cam).isPointInMirrorMask(uv(0), uv(1), 0)) 228 | return false; 229 | 230 | // Check distance is in the scale invariance region of the MapPoint 231 | const double maxDistance = pMP->GetMaxDistanceInvariance(); 232 | const double minDistance = pMP->GetMinDistanceInvariance(); 233 | 234 | cv::Matx44d Tcw = camSystem.Get_MtMc(cam); 235 | const cv::Vec3d PO = P - cv::Vec3d(Tcw(0, 3), Tcw(1, 3), Tcw(2, 3)); 236 | const double dist = cv::norm(PO); 237 | 238 | if (dist < minDistance || dist > maxDistance) 239 | return false; 240 | 241 | // Check viewing angle 242 | cv::Vec3d Pn = pMP->GetNormal(); 243 | 244 | double viewCos = PO.dot(Pn) / dist; 245 | 246 | //if(viewCos < viewingCosLimit) 247 | // return false; 248 | 249 | // Predict scale level according to the distance 250 | double ratio = dist / minDistance; 251 | 252 | std::vector::iterator it = std::lower_bound(mvScaleFactors.begin(), 253 | mvScaleFactors.end(), ratio); 254 | int nPredictedLevel = it - mvScaleFactors.begin(); 255 | 256 | if (nPredictedLevel >= mnScaleLevels) 257 | nPredictedLevel = mnScaleLevels - 1; 258 | 259 | // Data used by the tracking 260 | pMP->mbTrackInView[cam] = true; 261 | pMP->mTrackProjX[cam] = uv(0); 262 | pMP->mTrackProjY[cam] = uv(1); 263 | pMP->mnTrackScaleLevel[cam] = nPredictedLevel; 264 | pMP->mTrackViewCos[cam] = viewCos; 265 | 266 | return true; 267 | } 268 | 269 | std::vector cMultiFrame::GetFeaturesInArea(const int& cam, 270 | const double &x, 271 | const double &y, 272 | const double &r, 273 | int minLevel, int maxLevel) const 274 | { 275 | std::vector vIndices; 276 | 277 | int nMinCellX = floor((x - mnMinX[cam] - r)*mfGridElementWidthInv[cam]); 278 | nMinCellX = std::max(0, nMinCellX); 279 | if (nMinCellX >= FRAME_GRID_COLS) 280 | return vIndices; 281 | 282 | int nMaxCellX = ceil((x - mnMinX[cam] + r)*mfGridElementWidthInv[cam]); 283 | nMaxCellX = std::min(FRAME_GRID_COLS - 1, nMaxCellX); 284 | if (nMaxCellX < 0) 285 | return vIndices; 286 | 287 | int nMinCellY = floor((y - mnMinY[cam] - r)*mfGridElementHeightInv[cam]); 288 | nMinCellY = std::max(0, nMinCellY); 289 | if (nMinCellY >= FRAME_GRID_ROWS) 290 | return vIndices; 291 | 292 | int nMaxCellY = ceil((y - mnMinY[cam] + r)*mfGridElementHeightInv[cam]); 293 | nMaxCellY = std::min(FRAME_GRID_ROWS - 1, nMaxCellY); 294 | if (nMaxCellY < 0) 295 | return vIndices; 296 | 297 | bool bCheckLevels = true; 298 | bool bSameLevel = false; 299 | if (minLevel == -1 && maxLevel == -1) 300 | bCheckLevels = false; 301 | else 302 | if (minLevel == maxLevel) 303 | bSameLevel = true; 304 | 305 | for (int ix = nMinCellX; ix <= nMaxCellX; ++ix) 306 | { 307 | for (int iy = nMinCellY; iy <= nMaxCellY; ++iy) 308 | { 309 | std::vector vCell = mGrids[cam][ix][iy]; 310 | if (vCell.empty()) 311 | continue; 312 | 313 | for (size_t j = 0, jend = vCell.size(); j < jend; ++j) 314 | { 315 | const cv::KeyPoint &kpUn = mvKeys[vCell[j]]; 316 | if (bCheckLevels && !bSameLevel) 317 | { 318 | if (kpUn.octave < minLevel || kpUn.octave > maxLevel) 319 | continue; 320 | } 321 | else if (bSameLevel) 322 | { 323 | if (kpUn.octave != minLevel) 324 | continue; 325 | } 326 | 327 | if (abs(kpUn.pt.x - x) > r || abs(kpUn.pt.y - y) > r) 328 | continue; 329 | 330 | vIndices.push_back(vCell[j]); 331 | } 332 | } 333 | } 334 | 335 | return vIndices; 336 | 337 | } 338 | 339 | bool cMultiFrame::PosInGrid(const int& cam, 340 | cv::KeyPoint &kp, int &posX, int &posY) 341 | { 342 | posX = cvRound((kp.pt.x - mnMinX[cam])*mfGridElementWidthInv[cam]); 343 | posY = cvRound((kp.pt.y - mnMinY[cam])*mfGridElementHeightInv[cam]); 344 | 345 | //Keypoint's coordinates are undistorted, which could cause to go out of the image 346 | if (posX < 0 || posX >= FRAME_GRID_COLS || posY < 0 || posY >= FRAME_GRID_ROWS) 347 | return false; 348 | 349 | return true; 350 | } 351 | 352 | 353 | void cMultiFrame::ComputeBoW() 354 | { 355 | if (mBowVec.empty()) 356 | { 357 | std::vector vCurrentDesc = cConverter::toDescriptorVector(mDescriptors); 358 | mpORBvocabulary->transform(vCurrentDesc, mBowVec, mFeatVec, 4); 359 | } 360 | } 361 | } 362 | -------------------------------------------------------------------------------- /src/cSim3Solver.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * 4 | * Copyright (C) 2015-2016 Steffen Urban 5 | * For more information see 6 | * 7 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * MultiCol-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with MultiCol-SLAM . If not, see . 19 | */ 20 | 21 | /* 22 | * MultiCol-SLAM is based on ORB-SLAM2 which was also released under GPLv3 23 | * For more information see 24 | * Raúl Mur-Artal (University of Zaragoza) 25 | */ 26 | 27 | #include 28 | #include 29 | #include 30 | //#include 31 | 32 | #include "cSim3Solver.h" 33 | #include "cMultiKeyFrame.h" 34 | #include "cORBmatcher.h" 35 | #include "cConverter.h" 36 | 37 | #include 38 | 39 | #include "DBoW2/DUtils/Random.h" 40 | 41 | 42 | namespace MultiColSLAM 43 | { 44 | cSim3Solver::cSim3Solver(cMultiKeyFrame *pKF1, 45 | cMultiKeyFrame *pKF2, 46 | const std::vector &vpMatched12, 47 | cMultiCamSys_* camSys) : 48 | mnIterations(0), 49 | mnBestInliers(0) 50 | { 51 | camSysLocal = camSys; 52 | mpKF1 = pKF1; 53 | mpKF2 = pKF2; 54 | 55 | std::vector vpKeyFrameMP1 = pKF1->GetMapPointMatches(); 56 | 57 | mN1 = vpMatched12.size(); 58 | 59 | mvpMapPoints1.reserve(mN1); 60 | mvpMapPoints2.reserve(mN1); 61 | mvpMatches12 = vpMatched12; 62 | mvnIndices1.reserve(mN1); 63 | mvX3Dc1.reserve(mN1); 64 | mvX3Dc2.reserve(mN1); 65 | 66 | mvAllIndices.reserve(mN1); 67 | 68 | size_t idx = 0; 69 | cv::Matx44d hom1 = cConverter::invMat(pKF1->camSystem.Get_M_t()); 70 | cv::Matx44d hom2 = cConverter::invMat(pKF2->camSystem.Get_M_t()); 71 | for (int i1 = 0; i1 < mN1; ++i1) 72 | { 73 | if (vpMatched12[i1]) 74 | { 75 | cMapPoint* pMP1 = vpKeyFrameMP1[i1]; 76 | 77 | if (!pMP1) 78 | continue; 79 | 80 | cMapPoint* pMP2 = vpMatched12[i1]; 81 | 82 | if (pMP1->isBad() || pMP2->isBad()) 83 | continue; 84 | 85 | std::vector idxs1 = pMP1->GetIndexInKeyFrame(pKF1); 86 | std::vector idxs2 = pMP2->GetIndexInKeyFrame(pKF2); 87 | // TODO 88 | // only if the point was visible in multiple cameras (better scale?) 89 | //if (!(idxs1.size() > 1 || idxs2.size() > 1)) 90 | // continue; 91 | if (idxs1.empty() || idxs2.empty()) 92 | continue; 93 | // TODO 94 | int indexKF1 = idxs1[0]; // naja can be more than 1 95 | int indexKF2 = idxs2[0]; // naja can be more than 1 96 | 97 | if (indexKF1 < 0 || indexKF2 < 0) 98 | continue; 99 | 100 | const cv::KeyPoint &kp1 = pKF1->GetKeyPoint(indexKF1); 101 | const cv::KeyPoint &kp2 = pKF2->GetKeyPoint(indexKF2); 102 | 103 | const double sigmaSquare1 = pKF1->GetSigma2(kp1.octave); 104 | const double sigmaSquare2 = pKF2->GetSigma2(kp2.octave); 105 | 106 | mvnMaxError1.push_back(9.210*sigmaSquare1); 107 | mvnMaxError2.push_back(9.210*sigmaSquare2); 108 | 109 | mvpMapPoints1.push_back(pMP1); 110 | mvpMapPoints2.push_back(pMP2); 111 | mvnIndices1.push_back(i1); 112 | 113 | int cam1 = pKF1->keypoint_to_cam.find(indexKF1)->second; 114 | cv::Vec3d X3D1w = pMP1->GetWorldPos(); 115 | cv::Vec2d proj1(0.0, 0.0); 116 | pKF1->camSystem.WorldToCamHom_fast(cam1, X3D1w, proj1); 117 | // rotate it to the MCS frame! 118 | mvX3Dc1.push_back(cConverter::Hom2R(hom1)*X3D1w + cConverter::Hom2T(hom1)); 119 | mvP1im1.push_back(proj1); 120 | camIdx1.push_back(cam1); 121 | 122 | int cam2 = pKF2->keypoint_to_cam.find(indexKF2)->second; 123 | cv::Vec3d X3D2w = pMP2->GetWorldPos(); 124 | cv::Vec2d proj2(0.0, 0.0); 125 | pKF2->camSystem.WorldToCamHom_fast(cam2, X3D2w, proj2); 126 | 127 | mvX3Dc2.push_back(cConverter::Hom2R(hom2)*X3D2w + cConverter::Hom2T(hom2)); // rotate it to the MCS frame! 128 | mvP2im2.push_back(proj2); 129 | camIdx2.push_back(cam2); 130 | 131 | mvAllIndices.push_back(idx); 132 | ++idx; 133 | } 134 | } 135 | 136 | SetRansacParameters(); 137 | } 138 | 139 | void cSim3Solver::SetRansacParameters(double probability, 140 | int minInliers, 141 | int maxIterations) 142 | { 143 | mRansacProb = probability; 144 | mRansacMinInliers = minInliers; 145 | mRansacMaxIts = maxIterations; 146 | 147 | N = mvpMapPoints1.size(); // number of correspondences 148 | 149 | mvbInliersi.resize(N); 150 | 151 | // Adjust Parameters according to number of correspondences 152 | double epsilon = (double)mRansacMinInliers / N; 153 | 154 | // Set RANSAC iterations according to probability, epsilon, and max iterations 155 | int nIterations; 156 | 157 | if (mRansacMinInliers == N) 158 | nIterations = 1; 159 | else 160 | nIterations = ceil(log(1 - mRansacProb) / log(1 - pow(epsilon, 3))); 161 | 162 | mRansacMaxIts = std::max(1, std::min(nIterations, mRansacMaxIts)); 163 | 164 | mnIterations = 0; 165 | } 166 | 167 | bool cSim3Solver::iterate(int nIterations, 168 | bool &bNoMore, 169 | std::vector &vbInliers, 170 | int &nInliers, 171 | cv::Matx44d& result) 172 | { 173 | bNoMore = false; 174 | vbInliers = std::vector(mN1, false); 175 | nInliers = 0; 176 | 177 | if (N < mRansacMinInliers) 178 | { 179 | 180 | bNoMore = true; 181 | return false; 182 | } 183 | 184 | std::vector vAvailableIndices; 185 | 186 | cv::Matx33d P3Dc1i; 187 | cv::Matx33d P3Dc2i; 188 | std::vector camIdx1Min(3); 189 | std::vector camIdx2Min(3); 190 | int nCurrentIterations = 0; 191 | std::random_device rd; 192 | std::mt19937 gen(rd()); 193 | //cout << " mvAllIndices.size()" << mvAllIndices.size() << endl; 194 | if (mvAllIndices.size() < mRansacMinInliers) 195 | return false; 196 | while (mnIterations < mRansacMaxIts && 197 | nCurrentIterations < nIterations) 198 | { 199 | ++nCurrentIterations; 200 | ++mnIterations; 201 | 202 | vAvailableIndices = mvAllIndices; 203 | std::uniform_int_distribution<> dis(0, vAvailableIndices.size() - 1); 204 | // Get min set of points 205 | for (short i = 0; i < 3; ++i) 206 | { 207 | int randi = dis(gen); 208 | int idx = vAvailableIndices[randi]; 209 | P3Dc1i(0, i) = mvX3Dc1[idx](0); 210 | P3Dc1i(1, i) = mvX3Dc1[idx](1); 211 | P3Dc1i(2, i) = mvX3Dc1[idx](2); 212 | 213 | P3Dc2i(0, i) = mvX3Dc2[idx](0); 214 | P3Dc2i(1, i) = mvX3Dc2[idx](1); 215 | P3Dc2i(2, i) = mvX3Dc2[idx](2); 216 | 217 | camIdx1Min[i] = camIdx1[idx]; 218 | camIdx2Min[i] = camIdx2[idx]; 219 | 220 | vAvailableIndices[idx] = vAvailableIndices.back(); 221 | vAvailableIndices.pop_back(); 222 | } 223 | 224 | computeT(P3Dc1i, P3Dc2i); 225 | 226 | CheckInliers(); 227 | 228 | if (mnInliersi >= mnBestInliers) 229 | { 230 | mvbBestInliers = mvbInliersi; 231 | mnBestInliers = mnInliersi; 232 | mBestT12 = mT12i; 233 | mBestRotation = mR12i; 234 | mBestTranslation = mt12i; 235 | mBestScale = ms12i; 236 | 237 | if (mnInliersi > mRansacMinInliers) 238 | { 239 | nInliers = mnInliersi; 240 | for (int i = 0; i < N; ++i) 241 | if (mvbInliersi[i]) 242 | vbInliers[mvnIndices1[i]] = true; 243 | result = mBestT12; 244 | 245 | return true; 246 | } 247 | } 248 | } 249 | 250 | if (mnIterations >= mRansacMaxIts) 251 | bNoMore = true; 252 | 253 | return false; 254 | } 255 | 256 | bool cSim3Solver::find(std::vector &vbInliers12, 257 | int &nInliers, 258 | cv::Matx44d& result) 259 | { 260 | bool bFlag; 261 | return iterate(mRansacMaxIts, bFlag, vbInliers12, nInliers, result); 262 | } 263 | 264 | void cSim3Solver::centroid(cv::Matx33d &P, 265 | cv::Matx33d &Pr, 266 | cv::Vec3d &C) 267 | { 268 | C = cv::Vec3d(0.0, 0.0, 0.0); 269 | for (int i = 0; i < 3; ++i) 270 | { 271 | C(0) += P(0, i); 272 | C(1) += P(1, i); 273 | C(2) += P(2, i); 274 | } 275 | C /= 3.0; 276 | 277 | Pr = cv::Matx33d::zeros(); 278 | for (int i = 0; i < P.cols; ++i) 279 | { 280 | Pr(0, i) = P(0, i) - C(0); 281 | Pr(1, i) = P(1, i) - C(1); 282 | Pr(2, i) = P(2, i) - C(2); 283 | } 284 | } 285 | 286 | void cSim3Solver::computeT(cv::Matx33d &P1, 287 | cv::Matx33d &P2) 288 | { 289 | // Custom implementation of: 290 | // Horn 1987, Closed-form solution of absolute orientataion using unit quaternions 291 | 292 | // Step 1: Centroid and relative coordinates 293 | 294 | cv::Matx33d Pr1; // Relative coordinates to centroid (set 1) 295 | cv::Matx33d Pr2; // Relative coordinates to centroid (set 2) 296 | cv::Vec3d O1; // Centroid of P1 297 | cv::Vec3d O2; // Centroid of P2 298 | 299 | centroid(P1, Pr1, O1); 300 | centroid(P2, Pr2, O2); 301 | 302 | // Step 2: Compute M matrix 303 | 304 | cv::Matx33d M = Pr2*Pr1.t(); 305 | // Step 3: Compute N matrix 306 | double N11, N12, N13, N14, N22, N23, N24, N33, N34, N44; 307 | 308 | cv::Mat N(4, 4, CV_64FC1); 309 | 310 | N11 = M(0, 0) + M(1, 1) + M(2, 2); 311 | N12 = M(1, 2) - M(2, 1); 312 | N13 = M(2, 0) - M(0, 2); 313 | N14 = M(0, 1) - M(1, 0); 314 | N22 = M(0, 0) - M(1, 1) - M(2, 2); 315 | N23 = M(0, 1) + M(1, 0); 316 | N24 = M(2, 0) + M(0, 2); 317 | N33 = -M(0, 0) + M(1, 1) - M(2, 2); 318 | N34 = M(1, 2) + M(2, 1); 319 | N44 = -M(0, 0) - M(1, 1) + M(2, 2); 320 | 321 | N = (cv::Mat_(4, 4) << N11, N12, N13, N14, 322 | N12, N22, N23, N24, 323 | N13, N23, N33, N34, 324 | N14, N24, N34, N44); 325 | // Step 4: Eigenvector of the highest eigenvalue 326 | cv::Mat eval, evec; 327 | 328 | cv::eigen(N, eval, evec); //evec[0] is the quaternion of the desired rotation 329 | cv::Mat vec(1, 3, evec.type()); 330 | (evec.row(0).colRange(1, 4)).copyTo(vec); //extract imaginary part of the quaternion (sin*axis) 331 | 332 | // Rotation angle. sin is the norm of the imaginary part, cos is the real part 333 | double ang = atan2(norm(vec), evec.ptr(0)[0]); 334 | 335 | vec = 2 * ang*vec / norm(vec); //Angle-axis representation. quaternion angle is the half 336 | 337 | mR12i = cv::Matx33d::eye(); 338 | 339 | cv::Rodrigues(vec, mR12i); // computes the rotation matrix from angle-axis 340 | 341 | // Step 5: Rotate set 2 342 | cv::Matx33d P3 = mR12i*Pr2; 343 | 344 | // Step 6: Scale 345 | 346 | double nom = Pr1.dot(P3); 347 | cv::Matx33d aux_P3; 348 | aux_P3 = P3; 349 | cv::pow(P3, 2, aux_P3); 350 | double den = 0; 351 | 352 | for (int i = 0; i < 3; ++i) 353 | for (int j = 0; j < 3; ++j) 354 | den += aux_P3(i, j); 355 | 356 | ms12i = nom / den; 357 | 358 | // Step 7: Translation 359 | mt12i = O1 - ms12i*mR12i*O2; 360 | 361 | // Step 8: Transformation 362 | 363 | // Step 8.1 T12 364 | cv::Matx33d sR = ms12i*mR12i; 365 | mT12i = cConverter::Rt2Hom(sR, mt12i); 366 | 367 | // Step 8.2 T21 368 | cv::Matx33d sRinv = (1.0 / ms12i) * mR12i.t(); 369 | cv::Vec3d tinv = -sRinv*mt12i; 370 | mT21i = cConverter::Rt2Hom(sRinv, tinv); 371 | } 372 | 373 | 374 | void cSim3Solver::CheckInliers() 375 | { 376 | //std::vector vP1im2(mvP1im1.size()); 377 | //std::vector vP2im1(mvP1im1.size()); 378 | 379 | //cv::Matx44d relOri = mpKF1->GetPoseInverse()*mpKF2->GetPose(); 380 | 381 | mnInliersi = 0; 382 | for (size_t i = 0; i < mvP1im1.size(); ++i) 383 | { 384 | cv::Vec2d vP2im1(0.0, 0.0), vP1im2(0.0, 0.0); 385 | // rotate point from 1 to 2 and vice versa 386 | // transformation sequence: M_c^-1 * mT12i * M_t^-1 * Pt3 387 | // first to MCS frame then from MCS frame 1 to MCS frame 2 and then to the corresponding camera 388 | cv::Matx pt2_in_1 = mT12i * cConverter::toVec4d(mvX3Dc2[i]); 389 | cv::Matx pt1_in_2 = mT21i * cConverter::toVec4d(mvX3Dc1[i]); 390 | 391 | cv::Matx ptRot_in_1 = cConverter::invMat(camSysLocal->Get_M_c(camIdx1[i])) * pt2_in_1; 392 | cv::Matx ptRot_in_2 = cConverter::invMat(camSysLocal->Get_M_c(camIdx2[i])) * pt1_in_2; 393 | 394 | camSysLocal->GetCamModelObj(camIdx1[i]).WorldToImg( 395 | ptRot_in_1(0), ptRot_in_1(1), ptRot_in_1(2), vP2im1(0), vP2im1(1)); 396 | 397 | camSysLocal->GetCamModelObj(camIdx2[i]).WorldToImg( 398 | ptRot_in_2(0), ptRot_in_2(1), ptRot_in_2(2), vP1im2(0), vP1im2(1)); 399 | 400 | cv::Vec2d dist1 = mvP1im1[i] - vP2im1; 401 | cv::Vec2d dist2 = vP1im2 - mvP2im2[i]; 402 | 403 | double err1 = dist1.dot(dist1); 404 | double err2 = dist2.dot(dist2); 405 | 406 | if (err1 < mvnMaxError1[i] && 407 | err2 < mvnMaxError2[i]) 408 | { 409 | mvbInliersi[i] = true; 410 | ++mnInliersi; 411 | } 412 | else 413 | mvbInliersi[i] = false; 414 | } 415 | } 416 | 417 | 418 | cv::Matx33d cSim3Solver::GetEstimatedRotation() 419 | { 420 | return mBestRotation; 421 | } 422 | 423 | cv::Vec3d cSim3Solver::GetEstimatedTranslation() 424 | { 425 | return mBestTranslation; 426 | } 427 | 428 | double cSim3Solver::GetEstimatedScale() 429 | { 430 | return mBestScale; 431 | } 432 | 433 | } -------------------------------------------------------------------------------- /src/cOptimizerLoopStuff.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of MultiCol-SLAM 3 | * 4 | * Copyright (C) 2015-2016 Steffen Urban 5 | * For more information see 6 | * 7 | * MultiCol-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * MultiCol-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with MultiCol-SLAM . If not, see . 19 | */ 20 | 21 | /* 22 | * MultiCol-SLAM is based on ORB-SLAM2 which was also released under GPLv3 23 | * For more information see 24 | * Raúl Mur-Artal (University of Zaragoza) 25 | */ 26 | 27 | #include "cOptimizer.h" 28 | 29 | 30 | #include "g2o/core/block_solver.h" 31 | #include "g2o/core/optimization_algorithm_levenberg.h" 32 | #include "g2o/core/optimization_algorithm_dogleg.h" 33 | #include "g2o/core/optimization_algorithm_factory.h" 34 | #include "g2o/core/sparse_optimizer_terminate_action.h" 35 | 36 | #include "g2o/solvers/linear_solver_eigen.h" 37 | #include "g2o/types/types_six_dof_expmap.h" 38 | #include "g2o/core/robust_kernel_impl.h" 39 | #include "g2o/solvers/linear_solver_dense.h" 40 | #include "g2o/types/types_seven_dof_expmap.h" 41 | 42 | #include "g2o/core/jacobian_workspace.h" 43 | #include "g2o/stuff/macros.h" 44 | 45 | #include 46 | 47 | #include "cConverter.h" 48 | 49 | #include "g2o_MultiCol_vertices_edges.h" 50 | #include "g2o_MultiCol_sim3_expmap.h" 51 | 52 | 53 | namespace MultiColSLAM 54 | { 55 | double cOptimizer::stdSim = 4.0; 56 | 57 | 58 | int cOptimizer::OptimizeSim3(cMultiKeyFrame *pKF1, 59 | cMultiKeyFrame *pKF2, 60 | vector &vpMatches1, 61 | g2o::Sim3 &g2oS12) 62 | { 63 | g2o::SparseOptimizer optimizer; 64 | g2o::BlockSolverX::LinearSolverType * linearSolver; 65 | 66 | linearSolver = new g2o::LinearSolverDense(); 67 | 68 | g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); 69 | 70 | g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); 71 | optimizer.setAlgorithm(solver); 72 | 73 | g2o::SparseOptimizerTerminateAction* terminateAction = 0; 74 | terminateAction = new g2o::SparseOptimizerTerminateAction; 75 | terminateAction->setGainThreshold(1e-6); 76 | terminateAction->setMaxIterations(15); 77 | optimizer.addPostIterationAction(terminateAction); 78 | optimizer.setVerbose(false); 79 | 80 | // Calibration 81 | cMultiCamSys_ camSys1 = pKF1->camSystem; 82 | cMultiCamSys_ camSys2 = pKF2->camSystem; 83 | 84 | // SET SIMILARITY VERTEX 85 | VertexSim3Expmap_Multi * vSim3 = 86 | new VertexSim3Expmap_Multi( 87 | pKF1->keypoint_to_cam, 88 | pKF2->keypoint_to_cam); 89 | 90 | vSim3->setEstimate(g2oS12); 91 | vSim3->setId(0); 92 | vSim3->setFixed(false); 93 | vSim3->camSys1 = &camSys1; 94 | vSim3->camSys2 = &camSys2; 95 | optimizer.addVertex(vSim3); 96 | 97 | // SET MAP POINT VERTICES 98 | const int N = vpMatches1.size(); 99 | vector vpMapPoints1 = pKF1->GetMapPointMatches(); 100 | vector vpEdges12; 101 | vector vpEdges21; 102 | vector vSigmas12, vSigmas21; 103 | vector vnIndexEdge; 104 | 105 | vnIndexEdge.reserve(2 * N); 106 | vpEdges12.reserve(2 * N); 107 | vpEdges21.reserve(2 * N); 108 | 109 | const double deltaHuber = 1.345 * stdSim; 110 | const double deltaHuber2 = deltaHuber*deltaHuber; 111 | int nCorrespondences = 0; 112 | 113 | cv::Matx44d invM_t1 = cConverter::invMat(pKF1->camSystem.Get_M_t()); 114 | cv::Matx44d invM_t2 = cConverter::invMat(pKF2->camSystem.Get_M_t()); 115 | Eigen::Matrix2d I2x2 = Eigen::Matrix2d::Identity(); 116 | 117 | for (int i = 0; i < N; i++) 118 | { 119 | if (!vpMatches1[i]) 120 | continue; 121 | 122 | cMapPoint* pMP1 = vpMapPoints1[i]; 123 | cMapPoint* pMP2 = vpMatches1[i]; 124 | 125 | int id1 = 2 * i + 1; 126 | int id2 = 2 * (i + 1); 127 | 128 | int i2 = pMP2->GetIndexInKeyFrame(pKF2)[0]; 129 | 130 | if (pMP1 && pMP2) 131 | { 132 | if (!pMP1->isBad() && !pMP2->isBad() && i2 >= 0) 133 | { 134 | VertexPointXYZ* vPoint1 = new VertexPointXYZ(); 135 | cv::Vec4d P4D1w = cConverter::toVec4d(pMP1->GetWorldPos()); 136 | cv::Vec4d rot1CamSys = invM_t1*P4D1w; 137 | vPoint1->setEstimate(cv::Vec3d(rot1CamSys(0), rot1CamSys(1), rot1CamSys(2))); 138 | vPoint1->setId(id1); 139 | vPoint1->setFixed(true); 140 | vPoint1->SetID(i); 141 | optimizer.addVertex(vPoint1); 142 | 143 | VertexPointXYZ* vPoint2 = new VertexPointXYZ(); 144 | cv::Vec4d P4D2w = cConverter::toVec4d(pMP2->GetWorldPos()); 145 | cv::Vec4d rot2CamSys = invM_t2*P4D2w; 146 | vPoint2->setEstimate(cv::Vec3d(rot2CamSys(0), rot2CamSys(1), rot2CamSys(2))); 147 | vPoint2->setId(id2); 148 | vPoint2->setFixed(true); 149 | vPoint2->SetID(i2); 150 | optimizer.addVertex(vPoint2); 151 | } 152 | else 153 | continue; 154 | } 155 | else 156 | continue; 157 | 158 | nCorrespondences++; 159 | 160 | // SET EDGE x1 = S12*X2, projection from 2 to 1 161 | Eigen::Matrix obs1; 162 | cv::KeyPoint kpUn1 = pKF1->GetKeyPoint(i); 163 | obs1 << kpUn1.pt.x, kpUn1.pt.y; 164 | 165 | EdgeSim3ProjectXYZ_Multi* e12 = 166 | new EdgeSim3ProjectXYZ_Multi(); 167 | e12->setVertex(0, dynamic_cast(optimizer.vertex(id2))); // point 168 | e12->setVertex(1, dynamic_cast(optimizer.vertex(0))); // Sim3 169 | e12->setMeasurement(obs1); 170 | double invSigmaSquare1 = pKF1->GetInvSigma2(kpUn1.octave); 171 | e12->setInformation(I2x2*invSigmaSquare1); 172 | 173 | g2o::RobustKernelHuber* rk1 = new g2o::RobustKernelHuber; 174 | e12->setRobustKernel(rk1); 175 | rk1->setDelta(deltaHuber); 176 | optimizer.addEdge(e12); 177 | 178 | // SET EDGE x2 = S21*X1, projection from 1 to 2 179 | Eigen::Matrix obs2; 180 | cv::KeyPoint kpUn2 = pKF2->GetKeyPoint(i2); 181 | obs2 << kpUn2.pt.x, kpUn2.pt.y; 182 | 183 | EdgeInverseSim3ProjectXYZ_Multi* e21 = 184 | new EdgeInverseSim3ProjectXYZ_Multi(); 185 | e21->setVertex(0, dynamic_cast(optimizer.vertex(id1))); // point 186 | e21->setVertex(1, dynamic_cast(optimizer.vertex(0))); // Sim3 187 | e21->setMeasurement(obs2); 188 | double invSigmaSquare2 = pKF2->GetSigma2(kpUn2.octave); 189 | e21->setInformation(I2x2*invSigmaSquare2); 190 | 191 | g2o::RobustKernelHuber* rk2 = new g2o::RobustKernelHuber; 192 | e21->setRobustKernel(rk2); 193 | rk2->setDelta(deltaHuber); 194 | optimizer.addEdge(e21); 195 | 196 | vpEdges12.push_back(e12); 197 | vpEdges21.push_back(e21); 198 | vnIndexEdge.push_back(i); 199 | } 200 | 201 | // Optimize 202 | 203 | optimizer.initializeOptimization(); 204 | optimizer.optimize(5); 205 | 206 | // Check inliers 207 | int nBad = 0; 208 | for (size_t i = 0; i < vpEdges12.size(); i++) 209 | { 210 | EdgeSim3ProjectXYZ_Multi* e12 = vpEdges12[i]; 211 | EdgeInverseSim3ProjectXYZ_Multi* e21 = vpEdges21[i]; 212 | if (!e12 || !e21) 213 | continue; 214 | 215 | if (e12->chi2() > deltaHuber2 || 216 | e21->chi2() > deltaHuber2) 217 | { 218 | size_t idx = vnIndexEdge[i]; 219 | vpMatches1[idx] = NULL; 220 | optimizer.removeEdge(e12); 221 | optimizer.removeEdge(e21); 222 | vpEdges12[i] = NULL; 223 | vpEdges21[i] = NULL; 224 | nBad++; 225 | } 226 | } 227 | 228 | int nMoreIterations; 229 | if (nBad > 0) 230 | nMoreIterations = 10; 231 | else 232 | nMoreIterations = 5; 233 | 234 | if (nCorrespondences - nBad < 3) 235 | return 0; 236 | 237 | // Optimize again only with inliers 238 | optimizer.initializeOptimization(); 239 | optimizer.optimize(nMoreIterations); 240 | 241 | int nIn = 0; 242 | for (size_t i = 0; i < vpEdges12.size(); i++) 243 | { 244 | EdgeSim3ProjectXYZ_Multi* e12 = vpEdges12[i]; 245 | EdgeInverseSim3ProjectXYZ_Multi* e21 = vpEdges21[i]; 246 | if (!e12 || !e21) 247 | continue; 248 | 249 | if (e12->chi2() > deltaHuber2 || 250 | e21->chi2()>deltaHuber2) 251 | { 252 | size_t idx = vnIndexEdge[i]; 253 | vpMatches1[idx] = NULL; 254 | } 255 | else 256 | nIn++; 257 | } 258 | 259 | // Recover optimized Sim3 260 | g2o::VertexSim3Expmap* vSim3_recov = static_cast(optimizer.vertex(0)); 261 | g2oS12 = vSim3_recov->estimate(); 262 | 263 | return nIn; 264 | } 265 | 266 | 267 | void cOptimizer::OptimizeEssentialGraph(cMap* pMap, 268 | cMultiKeyFrame* pLoopMKF, 269 | cMultiKeyFrame* pCurMKF, 270 | g2o::Sim3 &Scurw, 271 | const cLoopClosing::KeyFrameAndPose &NonCorrectedSim3, 272 | const cLoopClosing::KeyFrameAndPose &CorrectedSim3, 273 | const std::map > &LoopConnections) 274 | { 275 | g2o::SparseOptimizer optimizer; 276 | optimizer.setVerbose(true); 277 | g2o::BlockSolver_7_3::LinearSolverType* linearSolver = 278 | new g2o::LinearSolverEigen(); 279 | g2o::BlockSolver_7_3* solver_ptr = new g2o::BlockSolver_7_3(linearSolver); 280 | g2o::OptimizationAlgorithmLevenberg* solver = 281 | new g2o::OptimizationAlgorithmLevenberg(solver_ptr); 282 | solver->setUserLambdaInit(1e-6); 283 | optimizer.setAlgorithm(solver); 284 | 285 | g2o::SparseOptimizerTerminateAction* terminateAction = 0; 286 | terminateAction = new g2o::SparseOptimizerTerminateAction; 287 | terminateAction->setGainThreshold(1e-6); 288 | terminateAction->setMaxIterations(15); 289 | optimizer.addPostIterationAction(terminateAction); 290 | 291 | // get all keyframes and map points 292 | unsigned int nMaxMKFid = pMap->GetMaxKFid(); 293 | vector vpMKFs = pMap->GetAllKeyFrames(); 294 | vector vpMPs = pMap->GetAllMapPoints(); 295 | 296 | // create vectors for uncorrected and corrected similarity transformatons 297 | // between MKFs 298 | vector> vScw(nMaxMKFid + 1); 299 | vector> vScw_corrected(nMaxMKFid + 1); 300 | vector vertices(nMaxMKFid + 1); // keeps the parameters to be estimated 301 | 302 | // limit for minimum weight between MKFs 303 | const int minNumFeat = 100; 304 | // now set the MKF vertices 305 | for (auto pMKF : vpMKFs) 306 | { 307 | if (pMKF->isBad()) 308 | continue; 309 | 310 | simpleVertexSim3Expmap* VSim3 = new simpleVertexSim3Expmap(); 311 | 312 | int MKFid = pMKF->mnId; 313 | 314 | // if this MKF was already corrected 315 | cLoopClosing::KeyFrameAndPose::const_iterator it = CorrectedSim3.find(pMKF); 316 | if (it != CorrectedSim3.end()) 317 | { 318 | vScw[MKFid] = it->second; 319 | VSim3->setEstimate(it->second); // set vertex estimate 320 | } 321 | else // convert rigid transformation to similarity 322 | { 323 | // we need the inverse transformation because, we store the world frame M_t, 324 | // instead of M_t^-1 325 | cv::Matx44d poseInv = pMKF->GetPoseInverse(); 326 | Eigen::Matrix R = 327 | cConverter::toMatrix3d(cConverter::Hom2R(poseInv)); 328 | Eigen::Matrix t = 329 | cConverter::toVector3d(cConverter::Hom2T(poseInv)); 330 | g2o::Sim3 sim3(R, t, 1.0); 331 | vScw[MKFid] = sim3; 332 | VSim3->setEstimate(sim3); // set vertex estimate 333 | } 334 | // fix the transformation if the current MKF is the loop MKF 335 | if (pMKF == pLoopMKF) 336 | VSim3->setFixed(true); 337 | VSim3->setId(MKFid); 338 | VSim3->setMarginalized(false); 339 | optimizer.addVertex(VSim3); 340 | 341 | vertices[MKFid] = VSim3; 342 | } 343 | 344 | 345 | set> insertedEdges; 346 | const Eigen::Matrix matLambda = Eigen::Matrix::Identity(); 347 | 348 | // Now set all the loop edges 349 | for (map>::const_iterator iter = LoopConnections.begin(), 350 | iterEnd = LoopConnections.end(); iter != iterEnd; ++iter) 351 | { 352 | cMultiKeyFrame* MKF = iter->first; 353 | const long unsigned int MKFid_i = MKF->mnId; 354 | const set& connections = iter->second; 355 | const g2o::Sim3 Siw = vScw[MKFid_i]; 356 | const g2o::Sim3 Swi = Siw.inverse(); 357 | // now iterate through all connections 358 | for (set::const_iterator connIter = connections.begin(), 359 | connIterEnd = connections.end(); connIter != connIterEnd; ++connIter) 360 | { 361 | const long unsigned int MKFid_j = (*connIter)->mnId; 362 | if ((MKFid_i != pCurMKF->mnId || 363 | MKFid_j != pLoopMKF->mnId) && 364 | MKF->GetWeight(*connIter) < minNumFeat) 365 | continue; 366 | 367 | const g2o::Sim3 Sjw = vScw[MKFid_j]; 368 | edgeSim3* e = new edgeSim3(); 369 | e->setVertex(1, dynamic_cast(optimizer.vertex(MKFid_j))); 370 | e->setVertex(0, dynamic_cast(optimizer.vertex(MKFid_i))); 371 | e->setMeasurement(Sjw * Swi); 372 | e->information() = matLambda; 373 | optimizer.addEdge(e); 374 | insertedEdges.insert(make_pair(min(MKFid_i, MKFid_j), max(MKFid_i, MKFid_j))); 375 | } 376 | } 377 | 378 | // Now set all normal edges 379 | for (size_t i = 0, iend = vpMKFs.size(); i < iend; ++i) 380 | { 381 | cMultiKeyFrame* pMKF = vpMKFs[i]; 382 | const int MKFid_i = pMKF->mnId; 383 | 384 | g2o::Sim3 Swi; 385 | cLoopClosing::KeyFrameAndPose::const_iterator it_i = NonCorrectedSim3.find(pMKF); 386 | if (it_i != NonCorrectedSim3.end()) 387 | Swi = (it_i->second).inverse(); 388 | else 389 | Swi = vScw[MKFid_i].inverse(); 390 | 391 | cMultiKeyFrame* parentMKF = pMKF->GetParent(); 392 | ///////////////////// 393 | // Spanning Tree Edges 394 | ///////////////////// 395 | if (parentMKF) 396 | { 397 | int MKFid_j = parentMKF->mnId; 398 | g2o::Sim3 Sjw; 399 | cLoopClosing::KeyFrameAndPose::const_iterator it_j = NonCorrectedSim3.find(parentMKF); 400 | if (it_j != NonCorrectedSim3.end()) 401 | Sjw = it_j->second; 402 | else 403 | Sjw = vScw[MKFid_j]; 404 | 405 | edgeSim3* e = new edgeSim3(); 406 | e->setVertex(1, dynamic_cast(optimizer.vertex(MKFid_j))); 407 | e->setVertex(0, dynamic_cast(optimizer.vertex(MKFid_i))); 408 | e->setMeasurement(Sjw * Swi); 409 | e->information() = matLambda; 410 | optimizer.addEdge(e); 411 | } 412 | ///////////////////// 413 | // loop edges 414 | ///////////////////// 415 | const set loopEdges = pMKF->GetLoopEdges(); 416 | for (set::const_iterator itLoop = loopEdges.begin(), 417 | itLoopEnd = loopEdges.end(); itLoop != itLoopEnd; ++itLoop) 418 | { 419 | cMultiKeyFrame* loopMKF = *itLoop; 420 | const int MKFid_l = loopMKF->mnId; 421 | if (MKFid_l < pMKF->mnId) 422 | { 423 | g2o::Sim3 Slw; 424 | cLoopClosing::KeyFrameAndPose::const_iterator it_l = NonCorrectedSim3.find(loopMKF); 425 | if (it_l != NonCorrectedSim3.end()) 426 | Slw = it_l->second; 427 | else 428 | Slw = vScw[MKFid_l]; 429 | 430 | edgeSim3* e = new edgeSim3(); 431 | e->setVertex(1, dynamic_cast(optimizer.vertex(MKFid_l))); 432 | e->setVertex(0, dynamic_cast(optimizer.vertex(MKFid_i))); 433 | e->setMeasurement(Slw * Swi); 434 | e->information() = matLambda; 435 | optimizer.addEdge(e); 436 | } 437 | } 438 | ///////////////////// 439 | // covisibility graph edges 440 | ///////////////////// 441 | const vector covConnectedMKFs = pMKF->GetCovisiblesByWeight(minNumFeat); 442 | for (vector::const_iterator itCov = covConnectedMKFs.begin(), 443 | itCovEnd = covConnectedMKFs.end(); itCov != itCovEnd; ++itCov) 444 | { 445 | cMultiKeyFrame* covMKF = *itCov; 446 | const int MKFid_n = covMKF->mnId; 447 | if (MKFid_n < pMKF->mnId) 448 | { 449 | g2o::Sim3 Snw; 450 | cLoopClosing::KeyFrameAndPose::const_iterator it_c = NonCorrectedSim3.find(covMKF); 451 | if (it_c != NonCorrectedSim3.end()) 452 | Snw = it_c->second; 453 | else 454 | Snw = vScw[MKFid_n]; 455 | 456 | edgeSim3* e = new edgeSim3(); 457 | e->setVertex(1, dynamic_cast(optimizer.vertex(MKFid_n))); 458 | e->setVertex(0, dynamic_cast(optimizer.vertex(MKFid_i))); 459 | e->setMeasurement(Snw * Swi); 460 | e->information() = matLambda; 461 | optimizer.addEdge(e); 462 | } 463 | } 464 | } 465 | 466 | // optimize essential graph 467 | optimizer.initializeOptimization(0); 468 | optimizer.optimize(20); 469 | 470 | ///////////////////// 471 | // recover rigid SE3 poses. Sim3: [sR t] -> SE3 [R t/s] 472 | ///////////////////// 473 | for (auto& itMKF : vpMKFs) 474 | { 475 | const int MKFid_i = itMKF->mnId; 476 | simpleVertexSim3Expmap* VSim3 = static_cast(optimizer.vertex(MKFid_i)); 477 | g2o::Sim3 CorrectedSiw = VSim3->estimate(); 478 | vScw_corrected[MKFid_i] = CorrectedSiw.inverse(); 479 | 480 | // set final pose 481 | double scale = CorrectedSiw.scale(); 482 | Eigen::Matrix3d R = CorrectedSiw.rotation().toRotationMatrix(); 483 | Eigen::Vector3d t = CorrectedSiw.translation() / scale; 484 | itMKF->SetPose(cConverter::invMat(cConverter::toCvSE3(R, t))); // inverse!! 485 | } 486 | 487 | ///////////////////// 488 | // correct map points 489 | ///////////////////// 490 | for (auto& itMPs : vpMPs) 491 | { 492 | if (itMPs->isBad()) 493 | continue; 494 | int MP_ref_MKFid = 0; 495 | if (itMPs->mnCorrectedByKF == pCurMKF->mnId) 496 | MP_ref_MKFid = itMPs->mnCorrectedReference; 497 | else 498 | { 499 | cMultiKeyFrame* refMKF = itMPs->GetReferenceKeyFrame(); 500 | MP_ref_MKFid = refMKF->mnId; 501 | } 502 | 503 | 504 | g2o::Sim3 Srw = vScw[MP_ref_MKFid]; 505 | g2o::Sim3 corrected_Swr = vScw_corrected[MP_ref_MKFid]; 506 | 507 | cv::Vec3d mp3D = itMPs->GetWorldPos(); 508 | Eigen::Vector3d eig_mp3D = 509 | corrected_Swr.map(Srw.map(cConverter::toVector3d(mp3D))); 510 | itMPs->SetWorldPos(cConverter::toCvVec3d(eig_mp3D)); 511 | itMPs->UpdateNormalAndDepth(); 512 | } 513 | } 514 | 515 | } --------------------------------------------------------------------------------