diff --git a/ORB-SLAM3-UESTC/.gitignore b/ORB-SLAM3-UESTC/.gitignore new file mode 100644 index 0000000..3704120 --- /dev/null +++ b/ORB-SLAM3-UESTC/.gitignore @@ -0,0 +1,24 @@ +CMakeLists.txt.user + +CMakeLists_modified.txt + +KeyFrameTrajectory.txt +CameraTrajectory.txt +kf_*.txt +f_*.txt + +Workspace/Thirdparty/DBoW2/build/ +Workspace/Thirdparty/DBoW2/lib/ +Workspace/Thirdparty/g2o/build/ +Workspace/Thirdparty/g2o/config.h +Workspace/Thirdparty/g2o/lib/ +Workspace/Vocabulary/ORBvoc.txt +Workspace/build/ +Workspace/lib/ + +cmake_modules/ +cmake-build-debug/ + +*.pyc +*.osa +.vscode/settings.json diff --git a/ORB-SLAM3-UESTC/Docs/Calibration_Tutorial.pdf b/ORB-SLAM3-UESTC/Docs/Calibration_Tutorial.pdf new file mode 100644 index 0000000..4859f5a --- /dev/null +++ b/ORB-SLAM3-UESTC/Docs/Calibration_Tutorial.pdf @@ -0,0 +1,496 @@ + Calibration Tutorial for ORB-SLAM3 v1.0 + + Juan J. Gómez Rodríguez, Carlos Campos, Juan D. Tardós + + December 22, 2021 + +1 Introduction + +This document contains a brief explanation of visual and visual-inertial calibration for +ORB-SLAM3 v1.0. In this version we introduce a new calibration file format whose main +novelties are: + + • Improved readability with consistent naming. + + • Option for internal rectification of stereo images. + + • Option for internal resizing of the input images. + +Some examples of use of the new calibration fromat can be found at directory Examples. +Although we recommend using the new file format, for the user convenience, we main- +tain compatibility with the file format used in previous versions, whose examples are in +directory Examples_old. + +2 Reference systems and extrinsic parameters + +Extrinsic calibration consists of the set of parameters which define how different sensors +are geometrically related. The most complex case is the stereo-inertial configuration, +shown in figure 1, where we define the following reference frames: + + • World (W): Defines a fixed reference system, whose zW axis points in opposite + direction of the gravity vector g. Translation and yaw are freely set by the SLAM + system and remain fixed once initialized. In the pure visual case, the world reference + is set to the first camera pose. + + • Body (B): This is the optimizable reference and is attached to the IMU. We assume + the gyroscope and the accelerometer share the same reference system. Body pose + TWB and velocity vB expressed in W are the optimizable variables. + + • Cameras (C1 and C2): These are coincident with the optical center of the cameras, + with zC pointing forward along the optical axis, yC pointing down and xC pointing + to the right, both aligned with image directions u and v. + + 1 + Figure 1: Reference systems defined for ORB-SLAM3 stereo-inertial. + +Cameras and body poses relate as: + +TWC1 = TWBTBC1 (1) + +TWC2 = TWBTBC1 TC1C2 (2) + +where, for example, TWC1 ∈ SE(3) is the homogeneous transformation that passes points +expressed in camera one reference to the world reference: + +xW = TWC1 xC1 (3) + +The extrinsic parameters that the calibration file needs to provide are: + +• Stereo-inertial: TBC1 and TC1C2 +• Monocular-inertial: TBC1 +• Stereo: only TC1C2, ORB-SLAM3 estimates the pose of the left camera (B = C1) +• Monocular: No parameters needed, ORB-SLAM3 estimates the camera pose. + + If your camera or dataset provides rectified stereo images, the calibration file only +needs to specify the baseline, instead of the full TC1C2 transformation. + + All the extrinsic parameters can be obtained using calibration software such as Kalibr +[3], as explained in section 4. + +3 Intrinsic parameters + +Those are calibration parameters which only depend on each sensor itself. Here, we +distinguish inertial and visual sensors. + + 2 + 3.1 Camera intrinsic parameters + +Depending on camera set-up we will need to provide different calibration parameters. +Those can be calibrated using OpenCV or Kalibr [3]. At ORB-SLAM3 we distinguish +three camera types: + + • Pinhole. The intrinsic parameters to provide are the camera focal length and central + point in pixel (fx, fy, cx, cy), together with the parameters for a radial-tangential + distortion model [4] with two or three radial distortion parameters (k1, k2, k3) and + two tangential distortion coefficients (p1, p2). + + • KannalaBrandt8. The Kannala-Brandt model [2] is appropriate for cameras with + wide-angle and fisheye lenses. The camera parameters to provide are the focal length + and central point in pixels (fx, fy, cx, cy) and four coefficients for the equidistant + distortion model (k1, k2, k3, k4). + + • Rectified. This camera type is to be used for camera or dataset that provide rectified + stereo images. In this case the calibration file only needs to provide (fx, fy, cx, cy) + for the rectified images and the stereo baseline b in meters. + + In the case of stereo pinhole cameras, ORB-SLAM3 internally rectifies the left and +right images using OpenCV’s stereorectify function. To avoid losing resolution and +field-of-view, cameras of type KannalaBrandt8 are not rectified. + + Examples of calibration files for the three camera types can be found at directory +Examples/Stereo-Inertial in files Euroc.yaml (Pinhole), TUM_VI_512.yaml (Kannal- +aBrandt8) and Realsense_D435i.yaml (Rectified). + +3.2 IMU intrinsic parameters + +IMU readings (linear acceleration a and angular velocity ω) are affected by measurement +noise (ηa, ηg) and bias (ba, bg), such as: + + a =a + ηa + ba (4) + + ω =ω + ηg + bg (5) + +where a and ω are the true acceleration (gravity not subtracted) and angular velocity +at the body reference B. Measurement noises are assumed to follow centered normal +distributions, such that: + + ηa ∼ N (0, σa2I3) (6) + + ηg ∼ N (0, σg2I3) (7) + +where σa and σg are both noise densities, which are character√ized in the IMU √data-sheet. +They need to be provided at the calibration file, with m/s2/ Hz and rad/s/ Hz units, +as shown in listing 1. + +1 IMU.NoiseAcc: 0.0028 # m/s^1.5 + +2 IMU.NoiseGyro: 0.00016 # rad/s^0.5 + +3 IMU.Frequency: 200 # s^-1 + +Listing 1: Noise densities for IMU. Values are from TUM-VI dataset + + 3 + When integrating the IMU measurements and estimating their covariances, the used +noise densities σa,f will depend on the IMU sampling frequency f , which must be provided +in the cali√bration file. This is internally managed by ORB-SLAM3, which computes +σa,f = σa/ f + + Regarding biases, they are assumed to evolve according to a Brownian motion. Given +two consecutive instants i and i + 1, this is characterized by: + + bai+1 = bai + ηraw with ηraw ∼ N (0, σa2,rwI3) (8) + + bgi+1 = big + ηrgw with ηrgw ∼ N (0, σg2,rwI3) (9) + +where σa,rw and σg,rw need to be supplied in the calibration file, as shown in listing 2. + +1 IMU.AccWalk: 0.00086 # m/s^2.5 + +2 IMU.GyroWalk: 0.000022 # rad/s^1.5 + + Listing 2: Random walk standard deviations for IMU biases. Values are from TUM-VI + dataset. + + It is common practice to increase the random walk standard deviations provided by +the IMU manufacturer (say multiplying them by 10) to account for unmodelled effects +and improving the IMU initialization convergence. + +4 Calibration example of Realsense D435i with Kalibr + +Here we show an example of visual-inertial calibration for a Realsense D435i device in +monocular-inertial configuration. We will assume the realsense library (https://github. +com/IntelRealSense/librealsense) has been previously installed. For the calibration, +we will use the well-established Kalibr open source software [1, 3], which can be found at +https://github.com/ethz-asl/kalibr. We will follow the next steps: + + • First, for simplicity, we will use Kalibr CDE. Just download it from https:// + github.com/ethz-asl/kalibr/wiki/downloads + + • As calibration pattern, we will adopt the the proposed April tag. You will find + templates for this pattern in the previous download page. You need to print it + and make sure pattern is just scaled and proportions are not modified. Update the + configuration .yaml file for this pattern. You can find a template corresponding to + the A0 size in the same download page. + + • For recording the calibration sequences, you can use the SDK from realsense or our + provided recorder, simply running: + + 1 ./Examples/Calibration/recorder_realsense_D435i ./Examples/ + Calibration/recorder + + Listing 3: Run visual-inertial recorder + + Make sure recorder directory exists and contains empty folders cam0 and IMU. + + • Use python script to process IMU data and interpolate accelerometer measurements + to synchronize it with the gyroscope. + + 4 + 1 python3 ./Examples/Calibration/python_scripts/process_imu.py ./ + Examples/Calibration/recorder/ + + Listing 4: Run visual-inertial recorder + + • Nest, you will need to convert this dataset to a rosbag with rosbag creater from + Kalibr. Run the next command from Kalibr CDE repository: + + 1 ./kalibr_bagcreater --folder /path_to_ORB_SLAM3/Examples/ + Calibration/recorder/. --output -bag /path_to_ORB_SLAM3/Examples/ + Calibration / recorder . bag + + Listing 5: Run visual-inertial recorder + +4.1 Visual calibration + +This step solves for the intrinsic camera parameters as well as relative camera transfor- +mation for stereo sensors. We will go through the next steps: + + • Following previous section steps, record a dataset with slow motion to avoid image + blur, as well as good lighting to have a low exposure time. It could be even possible + to move the pattern instead of the camera, but make sure it is not deformed. An + example calibration sequence can be found at https://youtu.be/R_K9-O4ool8. + + • Use a pattern as big as possible, so you can fill the whole image with the pattern + as far as possible, easing the camera focus. There not exists a minimum size for + this dataset, just try to have different points of view and make sure all pixels see + the pattern multiple times. + + • You could decrease the frame rate for this calibration, to make Kalibr solution + faster, but this is not a requirement. + + • Finally run the calibration from the Kalibr CDE repository: + + 1 ./ kalibr_calibrate_cameras --bag /path_to_ORB_SLAM3/Examples/ + Calibration/recorder.bag --topics /cam0/image_raw --models + pinhole -radtan --target /path_to_ORB_SLAM3/Examples/Calibration/ + april_to_be_updated.yaml + + Listing 6: Run visual calibration + + For our monocular Realsense D435i, we compare Kalibr and factory calibration at + table 1. Typical values of reprojection error for a successful visual calibration are + those whose mean is close to zero pixels (|µ| < 10−4) and its standard deviation + σ < 0.3. + +4.2 Inertial calibration + +Once calibrated visual parameters, we will continue with the inertial calibration: + + • First, record a new dataset with fast motion, trying to excite accelerometer and + gyroscope along all their axes. Try to keep always most of the pattern visible in the + image, so Kalibr can accurately estimate its relative pose. Pattern should remain + + 5 + Table 1: Comparative factory/Kalibr calibration for Realsense D435i. + + Factory Kalibr + + fx 382.613 381.69830045 ± 0.47867208 + + fy 382.613 381.6587096 ± 0.48696699 + cx 320.183 321.58237544 ± 0.40096812 + cy 236.455 236.20193592 ± 0.38600065 + + k1 0 -0.00469988 ± 0.00171615 + + k2 0 0.00110469 ± 0.00196003 + -0.00029279 ± 0.00028587 + r1 0 0.00066225 ± 0.00034486 + + r2 0 + + fixed during this calibration. A suitable sequence for visual-inertial calibration can + be found at hhttps://youtu.be/4XkivVLw5k4 + +• You will need to provide the noise density and bias random walk for the IMU sensor. + You can find these values from IMU datasheet (BMI055 for Realsense D435i) or + estimate them using data acquired while keeping the IMU steady for a long period + of time. + +• Finally run the calibration from the Kalibr CDE repository: + +1 ./ kalibr_calibrate_imu_camera --bag /path_to_ORB_SLAM3/Examples/ + Calibration/recorder.bag --cam /path_to_ORB_SLAM3/Examples/ + Calibration/camera_calibration.yaml --imu /path_to_ORB_SLAM3/ + Examples/Calibration/imu_intrinsics.yaml --target / + path_to_ORB_SLAM3/Examples/Calibration/april_to_be_updated.yaml + + Listing 7: Run visual calibration + +For our monocular Realsense D435i, we compare Kalibr and factory calibration at +table 2. + +Table 2: Comparative factory/Kalibr calibration for Realsense D435i. + + Factory Kalibr + +  1 0 0 −0.005   0.9999 0.0003 −0.0135 −0.0015  + +TBC  0 1 0 −0.005   −0.0002 0.9999 0.0054 0.0004  + 0.9999 +  0 0 1 0.0117   0.0135 −0.0054 0.0201  +     + + 000 1 0 0 0 1 + +4.3 Launch ORB-SLAM3 + +With all these calibration parameters, you can finally create your .yaml file to be used +along with ORB-SLAM3. Finally, run ORB-SLAM launcher as: + + 1 ./Examples/Monocular -Inertial/mono_inertial_realsense_D435i + Vocabulary/ORBvoc.txt Examples/Monocular -Inertial/RealSense_D435i. + yaml + + Listing 8: Run real-time monocular-inertial SLAM + + 6 + 5 Reference for the new calibration files + +This section summarizes all parameters that are required by ORB-SLAM3 in any of its +stages, including intrinsic and extrinsic calibration parameters, ORB extraction param- +eters and visualization settings. All this configuration parameters must be passed to +ORB-SLAM3 in a yaml file. In this type of files, data is stored as a pair, +encoding the parameter name and its value respectively. We now define all parameters +that ORB-SLAM3 accepts, their types and whether they are required or optional. + +5.1 General calibration parameters + +These are general calibration parameters: + + • File.version = "1.0" [REQUIRED]: specifies that the new calibration file format + is being used. + + • Camera.type (string) [REQUIRED]: specifies the camera type beeing used. Must + take one of the following values: + + – PinHole when using pinhole cameras. + – KannalaBrandt8 when using fish-eye cameras with a Kannala-Bradt calibra- + + tion model. + – Rectified when using a stereo rectified pinhole camera. + + • Camera.height, Camera.width (int) [REQUIRED]: input image height and width. + + • Camera.newHeight, Camera.newWidth (int) [OPTIONAL]: If defined, ORB-SLAM3 + resizes the input images to the new resolution specified, recomputing the calibration + parameters as needed. + + • Camera.fps (int) [REQUIRED]: frames per second of the video sequence. + + • Camera.RGB (int) [REQUIRED]: specifies if the images are: BGR (0) or RGB + (1). It is ignored if the images are grayscale. + + • System.thFarPoints (float) [OPTIONAL]: if defined, specifies the maximum depth + in meters allowed for a point. Points farther than this are ignored. + +5.2 Camera intrinsic parameters + +We define the Camera1 as the monocular camera (in monocular SLAM) or the left camera +(in stereo SLAM). Its intrinsic parameters correspond to: + + • Camera1.fx, Camera1.fy, Camera1.cx, Camera1.cy (float) [REQUIRED]: corre- + sponds with the intrinsic calibration parameters of the camera 1. + + If Camera.type is set to PinHole, you should specify: + + • Camera1.k1, Camera1.k2 (float) [REQUIRED]: corresponds to the radial distor- + tion coefficients. + + 7 + • Camera1.p1, Camera1.p2 (float) [REQUIRED]: corresponds to the tangential + distortion coefficients. + + • Camera1.k3 (float) [OPTIONAL]: sometimes a third radial distortion parameter is + used. You can specify it with this parameter. + + If Camera.type is set to KannalaBrandt8, you should specify: + + • Camera1.k1, Camera1.k2, Camera1.k3, Camera1.k4 (float) [REQUIRED]: Kannala- + Brandt distortion coefficients. + + If using a stereo camera, specify also the parameters for Camera2 (right camera), +unless Camera.type is set to Rectified in which case both cameras are assumed to have +the same parameters. + +5.3 Stereo parameters + +The following parameter is required for stereo configurations: + + • Stereo.ThDepth (float) [REQUIRED]: it is the number of the stereo baselines we + use to classify a point as close or far. Close and far points are treated differently in + several parts of the stereo SLAM algorithm. + + If Camera.type is set to Rectified, you need to add the following parameter: + + • Stereo.b (float) [REQUIRED]: stereo baseline in meters. + + If you are using a non-rectified stereo, you need to provide: + + • Stereo.T_c1_c2 (cv::Mat) [REQUIRED]: relative pose between stereo cameras. + + If you are using stereo fish-eye cameras (i.e. Camera.type is set to KannalaBrandt8 ), +you also need to specify the overlapping area between both images: + + • Camera1.overlappingBegin, Camera2.overlappingBegin (int) [REQUIRED]: start + column of the overlapping area. + + • Camera1.overlappingEnd, Camera2.overlappingEnd (int) [REQUIRED]: end col- + umn of the overlapping area. + +5.4 Inertial parameters + +When using a inertial sensor, the user must define the following parameters: + + • IMU.NoiseGyro (float) [REQUIRED]: measurement noise density of the gyro- + scope. + + • IMU.NoiseAcc (float) [REQUIRED]: measurement noise density of the accelerom- + eter. + + • IMU.GyroWalk (float) [REQUIRED]: Random walk variance of the gyroscope. + + 8 + • IMU.AccWalk (float) [REQUIRED]: Random walk variance of the accelerometer. + + • IMU.Frequency (float) [REQUIRED]: IMU frequency. + + • IMU.T_b_c1 (cv::Mat) [REQUIRED]: Relative pose between IMU and Camera1 + (i.e. the transformation that takes a point from Camera1 to IMU ). + + • IMU.InsertKFsWhenLost (int) [OPTIONAL]: Specifies if the system inserts KeyFrames + when the visual tracking is lost but the inertial tracking is alive. + +5.5 RGB-D parameters + +The following parameter is required for RGB-D cameras: + + • RGBD.DepthMapFactor (float) [REQUIRED]: factor to transform the depth map + to real units. + +5.6 ORB parameters + +The following parameters are related with the ORB feature extraction: + + • ORBextractor.nFeatures (int) [REQUIRED]: number of features to extract per + image. + + • ORBextractor.scaleFactor (float) [REQUIRED]: scale factor between levels in the + image pyramid. + + • ORBextractor.nLevels (int) [REQUIRED]: number of levels in the image pyramid. + + • ORBextractor.iniThFAST (int) [REQUIRED]: initial threshold to detect FAST + corners. + + • ORBextractor.minThFAST (int) [REQUIRED]: if no features are found with the + initial threshold, the system tries a second time with this threshold value. + +5.7 Atlas parameters + +These parameters define if we load/save a map from/to a file: + + • System.LoadAtlasFromFile (string) [OPTIONAL]: file path where the map to load + is located. + + • System.SaveAtlasToFile (string) [OPTIONAL]: destination file to save the map + generated. + 9 + 5.8 Viewer parameters + +These are some parameters related to the ORB-SLAM3 user interface: + • Viewer.KeyFrameSize (float) [REQUIRED]: size in which the KeyFrames are + drawn in the map viewer. + • Viewer.KeyFrameLineWidth (float) [REQUIRED]: line width of the KeyFrame + drawing. + • Viewer.GraphLineWidth (float) [REQUIRED]: line width of the covisibility graph. + • Viewer.PointSize (float) [REQUIRED]: size of the MapPoint drawing. + • Viewer.CameraSize (float) [REQUIRED]: size in which the current camera is + drawn in the map viewer. + • Viewer.CameraLineWidth (float) [REQUIRED]: line width of the current camera + drawing. + • Viewer.ViewpointX, Viewer.ViewpointY, Viewer.ViewpointZ, Viewer.ViewpointF + (float) [REQUIRED]: starting view point of the map viewer. + • Viewer.imageViewScale (float) OPTIONAL: resize factor for the image visualiza- + tion (only for visualization, not used in the SLAM pipeline). + +References + +[1] Paul Furgale, Joern Rehder, and Roland Siegwart. Unified temporal and spatial + calibration for multi-sensor systems. In IROS, pages 1280–1286. IEEE, 2013. + +[2] Juho Kannala and Sami S Brandt. A generic camera model and calibration method + for conventional, wide-angle, and fish-eye lenses. IEEE Trans. Pattern Analysis and + Machine Intelligence, 28(8):1335–1340, 2006. + +[3] Joern Rehder, Janosch Nikolic, Thomas Schneider, Timo Hinzmann, and Roland Sieg- + wart. Extending kalibr: Calibrating the extrinsics of multiple IMUs and of individual + axes. In Proc. IEEE Int. Conf. Robotics and Automation (ICRA), pages 4304–4311, + 2016. + +[4] Richard Szeliski. Computer vision: algorithms and applications. Springer Verlag, + London, 2011. + + 10 + diff --git a/ORB-SLAM3-UESTC/Docs/ORB-SLAM3-Image/Untitled.png b/ORB-SLAM3-UESTC/Docs/ORB-SLAM3-Image/Untitled.png new file mode 100644 index 0000000..10825d1 Binary files /dev/null and b/ORB-SLAM3-UESTC/Docs/ORB-SLAM3-Image/Untitled.png differ diff --git a/ORB-SLAM3-UESTC/Docs/ORB-SLAM3环境配置.md b/ORB-SLAM3-UESTC/Docs/ORB-SLAM3环境配置.md new file mode 100644 index 0000000..8f6f750 --- /dev/null +++ b/ORB-SLAM3-UESTC/Docs/ORB-SLAM3环境配置.md @@ -0,0 +1,69 @@ +# ORB-SLAM3环境配置 + +> [ORB-SLAM3配置安装及运行---Ubuntu20.04（2021年）_1900_的博客-CSDN博客](https://blog.csdn.net/holly_Z_P_F/article/details/118031317) +> +> [GitHub - electech6/ORB_SLAM3_detailed_comments: Detailed comments for ORB-SLAM3](https://github.com/electech6/ORB_SLAM3_detailed_comments/blob/master/euroc_examples.sh) + +## 一、Linux环境下常见问题解决方案 + +### 1.1 error: ‘slots_reference’ was not declared in this scope + +**解决方法：** + +[https://blog.csdn.net/BigHandsome2020/article/details/123458612](https://blog.csdn.net/BigHandsome2020/article/details/123458612) + +**不是pangolin的问题！！！** + + **sed -i 's/++11/++14/g' CMakeLists.txt** + +**问题2: realsense2** + +![Untitled](ORB-SLAM3-Image/Untitled.png) + +**解决方法：** + +[https://blog.csdn.net/weixin_43901418/article/details/121475001](https://blog.csdn.net/weixin_43901418/article/details/121475001) + +```shell +sudo apt-key adv --keyserver [keyserver.ubuntu.com](http://keyserver.ubuntu.com/) --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE +sudo add-apt-repository "deb [https://librealsense.intel.com/Debian/apt-repo](https://librealsense.intel.com/Debian/apt-repo) $(lsb_release -cs) main" -u +sudo apt-get install librealsense2-dkms +sudo apt-get install librealsense2-utils +sudo apt-get install librealsense2-dev +``` + +**问题3：在运行build.sh 时最后一步make -j4 构建panglin失败** + +```shell +cd Pangolin +mkdir build +cd build +cmake .. +cmake --build . +``` + +​ [https://github.com/raulmur/ORB_SLAM2/issues/22](https://github.com/raulmur/ORB_SLAM2/issues/22) + +​ 需要重新安装panglin，版本如果比较新，则需要切换到C++14 + +### 1.4 没有可视化界面只有轨迹结果 + +​ Monocular程序运行Euroc数据集或者其他官方数据集的时候，运行结果**只有轨迹结果，但没有可视化界面**。 + +```shell +./Examples/Monocular/mono_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular/EuRoC.yaml +``` + +​ 这是因为原始灰度图像的输出是需要通过代码设置开启的，mono_euroc.cc修改： + +```c++ +// 这里的第三个参数为bool类型，用于控制是否显示摄像头的灰度图像 +ORB_SLAM3::System SLAM(argv[1], argv[2], ORB_SLAM3::System::MONOCULAR, false); +// 如果需要显示摄像头的内容，设置为true即可 +ORB_SLAM3::System SLAM(argv[1], argv[2], ORB_SLAM3::System::MONOCULAR, true); +``` + +### 1.5 c++: fatal error: Killed signal terminated program cc1plus + +​ 内存爆了，用 make -j 不要给太多任务 + diff --git a/ORB-SLAM3-UESTC/Docs/更新日志.md b/ORB-SLAM3-UESTC/Docs/更新日志.md new file mode 100644 index 0000000..9bb9889 --- /dev/null +++ b/ORB-SLAM3-UESTC/Docs/更新日志.md @@ -0,0 +1,49 @@ +# ORB-SLAM3 +Details of changes between the different versions. + +### V1.0, 22th December 2021 + +- OpenCV static matrices changed to Eigen matrices. The average code speed-up is 16% in tracking and 19% in mapping, w.r.t. times reported in the ORB-SLAM3 paper. + +- New calibration file format, see file Calibration_Tutorial. Added options for stereo rectification and image resizing. + +- Added load/save map functionalities. + +- Added examples of live SLAM using Intel Realsense cameras. + +- Fixed several bugs. + +### V0.4: Beta version, 21st April 2021 + +- Changed OpenCV dynamic matrices to static matrices to speed up the code. + +- Capability to measure running time of the system threads. + +- Compatibility with OpenCV 4.0 (Requires at least OpenCV 3.0). + +- Fixed minor bugs. + + +### V0.3: Beta version, 4th Sep 2020 + +- RGB-D compatibility: the RGB-D examples have been adapted to the new version. + +- Kitti and TUM dataset compatibility: these examples have been adapted to the new version. + +- ROS compatibility: updated the old references in the code to work with this version. + +- Config file parser: the YAML file contains the session configuration, a wrong parametrization may break the execution without any information to solve it. This version parses the file to read all the fields and give a proper answer if one of the fields have been wrongly deffined or does not exist. + +- Fixed minor bugs. + + +### V0.2: Beta version, 7th Aug 2020 +Initial release. It has these capabilities: + +- Multiple-Map capabilities: it is able to handle multiple maps in the same session and merge them when a common area is detected with a seamless fussion. + +- Inertial sensor: the IMU initialization takes 2 seconds to achieve a scale error less than 5\% and it is reffined in the next 10 seconds until it is around 1\%. Inertial measures are integrated at frame rate to estimate the scale, gravity and velocity in order to improve the visual features detection and make the system robust to temporal occlusions. + +- Fisheye cameras: cameras with wide-angle and fisheye lenses are now fully supported in monocular and stereo. + + diff --git a/ORB-SLAM3-UESTC/README.md b/ORB-SLAM3-UESTC/README.md new file mode 100644 index 0000000..bdf69c9 --- /dev/null +++ b/ORB-SLAM3-UESTC/README.md @@ -0,0 +1,60 @@ + + +# ORB-SLAM3 -UESTC + +概述：基于ORB-SLAM3调整的专用版本。`Workspace`目录下的ORB-SLAM3的核心部分源自 + +## 一、基本操作 + +### 1.1 代码的编译和运行 + +​ 代码的编译和运行参考文档`Workspace/README.md` + +## 二、数据的接入与部署 + +### 2.1 单目摄像头加入IMU数据实时接入 + +​ 由于ORB-SLAM3采用的是读取图片和csv文件的方式运行，不适用于小车的实际实时接入数据。所以，我们需要对原本的读取代码进行改进，并且对我们使用的IMU实现一个读取器。伪代码如下： + +```c++ +// 单目摄像头+IMU接入ORB-SLAM3伪代码 + +int main(){ + // 创建IMU数据读取器 + IMUReader imuReader("/dev/ttyUSB0"); + // 创建ORB_SLAM3定位系统 + ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true); + // 默认30fps帧率 + int fps = 30; + // 初始化摄像头 + cv::VideoCapture cap = cv::VideoCapture(0); + cap.set(cv::CAP_PROP_FRAME_WIDTH, 640); + cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480); + // 清空IMU内部缓存，等待1fps的时间用于填充数据 + imuReader.clear(); + delay_ms(1000 / fps); + while(true){ + Mat frame; + // 通过Opencv从摄像头读取视频帧 + cap >> frame; + // 获取当前系统的时间戳 + tframe = get_systemTick(); + // imuReader从串口读取数据 + IMUData vImuMeas; + int statsu = imuReader(vImuMeas); + // 清空IMUReader内部缓存, imuReader内部线程继续读取新的数据 + imuReader.clear(); + // 如果读取IMU状态正确 + if(status){ + // 视觉+IMU VSLAM + SLAM.TrackMonocular(frame,tframe,vImuMeas); + }else{ + // 纯视觉 VSLAM + SLAM.TrackMonocular(frame,tframe); + } + } + return 0; +} + +``` + diff --git a/ORB-SLAM3-UESTC/Workspace/.vscode/settings.json b/ORB-SLAM3-UESTC/Workspace/.vscode/settings.json new file mode 100644 index 0000000..5d7d730 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "ros.distro": "noetic" +} \ No newline at end of file diff --git a/ORB-SLAM3-UESTC/Workspace/Application/IMUDriver/CMakeLists.txt b/ORB-SLAM3-UESTC/Workspace/Application/IMUDriver/CMakeLists.txt new file mode 100644 index 0000000..1b09c21 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Application/IMUDriver/CMakeLists.txt @@ -0,0 +1,22 @@ +# 声明要求的 cmake 最低版本 +cmake_minimum_required(VERSION 2.8) + +# 声明一个 cmake 工程 +project(imu) + +# 设置编译模式 +set(CMAKE_BUILD_TYPE "Debug") + +# 查找 Boost 库 +find_package(Boost REQUIRED) + +# 语法：add_executable( 程序名 源代码文件 ） + +add_executable(imu main.cpp IMUReader.cpp) + +target_include_directories(imu PRIVATE ${CMAKE_SOURCE_DIR}) + +# 添加线程库 +find_package(Threads REQUIRED) +target_link_libraries(imu PRIVATE Threads::Threads) +target_link_libraries(imu ${Boost_LIBRARIES} pthread) \ No newline at end of file diff --git a/ORB-SLAM3-UESTC/Workspace/Application/IMUDriver/main.cpp b/ORB-SLAM3-UESTC/Workspace/Application/IMUDriver/main.cpp new file mode 100644 index 0000000..72ebaca --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Application/IMUDriver/main.cpp @@ -0,0 +1,18 @@ +#include +#include +int main() { + // 创建 IMUReader 实例 + IMUReader imuReader("/dev/ttyUSB0"); + + // orb-slam read cam + while(1){ + IMUData data; + int status = imuReader.ReadData(data); + + printf("Acc=%f,%f,%f, Gyro=%f,%f,%f\n",data.accelX,data.accelY,data.accelZ, + data.gyroX,data.gyroY,data.gyroZ); + // 30FPS + std::this_thread::sleep_for(std::chrono::milliseconds(33)); + } + return 0; +} \ No newline at end of file diff --git a/ORB-SLAM3-UESTC/Workspace/Application/cam2.yaml b/ORB-SLAM3-UESTC/Workspace/Application/cam2.yaml new file mode 100755 index 0000000..932cbfe --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Application/cam2.yaml @@ -0,0 +1,69 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- +Camera.type: "KannalaBrandt8" + +# Camera calibration and distortion parameters (OpenCV) +Camera.fx: 199.842179 # 190.97847715128717 + +Camera.fy: 199.453469 # 190.9733070521226 +Camera.cx: 342.844938 # 254.93170605935475 +Camera.cy: 265.535897 # 256.8974428996504 + +# Equidistant distortion 0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182 +# Camera.bFishEye: 1 +Camera.k1: -0.221323 # 0.0034823894022493434 +Camera.k2: 0.173058 # 0.0007150348452162257 +Camera.k3: -0.197220 # -0.0020532361418706202 +Camera.k4: 0.069912 # 0.00020293673591811182 + +# Camera resolution +Camera.width: 640 +Camera.height: 480 + +# Camera frames per second +# Camera.fps: 20.0 + +Camera.fps: 30 + + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1500 # Tested with 1250 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +# ORBextractor.iniThFAST: 20 +# ORBextractor.minThFAST: 7 +ORBextractor.iniThFAST: 20 # 20 +ORBextractor.minThFAST: 7 # 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize: 2 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3 +Viewer.ViewpointX: 0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -3.5 # -1.8 +Viewer.ViewpointF: 500 diff --git a/ORB-SLAM3-UESTC/Workspace/Application/mono_euroc_cap b/ORB-SLAM3-UESTC/Workspace/Application/mono_euroc_cap new file mode 100755 index 0000000..91e5d11 Binary files /dev/null and b/ORB-SLAM3-UESTC/Workspace/Application/mono_euroc_cap differ diff --git a/ORB-SLAM3-UESTC/Workspace/Application/mono_euroc_cap.cc b/ORB-SLAM3-UESTC/Workspace/Application/mono_euroc_cap.cc new file mode 100755 index 0000000..4503d02 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Application/mono_euroc_cap.cc @@ -0,0 +1,203 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#include +#include +#include +#include + +#include + +#include + +using namespace std; + +void LoadImages(const string &strImagePath, const string &strPathTimes, + vector &vstrImages, vector &vTimeStamps); + +int main(int argc, char **argv) +{ + if(argc < 3) + { + cerr << endl << "Usage: ./mono_euroc path_to_vocabulary path_to_settings path_to_sequence_folder_1 path_to_times_file_1 (path_to_image_folder_2 path_to_times_file_2 ... path_to_image_folder_N path_to_times_file_N) (trajectory_file_name)" << endl; + return 1; + } + + const int num_seq = 1; + cout << "num_seq = " << num_seq << endl; + + // Load all sequences: + int seq; + vector< vector > vstrImageFilenames; + vector< vector > vTimestampsCam; + vector nImages; + + vstrImageFilenames.resize(num_seq); + vTimestampsCam.resize(num_seq); + nImages.resize(num_seq); + + + + // Vector for tracking time statistics + vector vTimesTrack; + // vTimesTrack.resize(tot_images); + + cout << endl << "-------" << endl; + cout.precision(19); + + + // int fps = 30; + // float dT = 1.f/fps; + // Create SLAM system. It initializes all system threads and gets ready to process frames. + ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::MONOCULAR, true); + float imageScale = SLAM.GetImageScale(); + + double t_resize = 0.f; + double t_track = 0.f; + + for (seq = 0; seq> im; + if(im.empty()) + { + cerr << endl << "Error: Couldn't capture a frame" + << endl; + return 1; + } + // Show the origin image. + // cv::imshow("img",im); + + auto currentTime = std::chrono::high_resolution_clock::now(); + auto timestamp = std::chrono::duration_cast(currentTime.time_since_epoch()); + double tframe = static_cast(timestamp.count()); + + if(imageScale != 1.f) + { +#ifdef REGISTER_TIMES + #ifdef COMPILEDWITHC11 + std::chrono::steady_clock::time_point t_Start_Resize = std::chrono::steady_clock::now(); + #else + std::chrono::monotonic_clock::time_point t_Start_Resize = std::chrono::monotonic_clock::now(); + #endif +#endif + int width = im.cols * imageScale; + int height = im.rows * imageScale; + cv::resize(im, im, cv::Size(width, height)); +#ifdef REGISTER_TIMES + #ifdef COMPILEDWITHC11 + std::chrono::steady_clock::time_point t_End_Resize = std::chrono::steady_clock::now(); + #else + std::chrono::monotonic_clock::time_point t_End_Resize = std::chrono::monotonic_clock::now(); + #endif + t_resize = std::chrono::duration_cast >(t_End_Resize - t_Start_Resize).count(); + SLAM.InsertResizeTime(t_resize); +#endif + } + + #ifdef COMPILEDWITHC11 + std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); + #else + std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now(); + #endif + + // Pass the image to the SLAM system + // cout << "tframe = " << tframe << endl; + SLAM.TrackMonocular(im, tframe); // TODO change to monocular_inertial + + #ifdef COMPILEDWITHC11 + std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now(); + #else + std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now(); + #endif + +#ifdef REGISTER_TIMES + t_track = t_resize + std::chrono::duration_cast >(t2 - t1).count(); + SLAM.InsertTrackTime(t_track); +#endif + + double ttrack= std::chrono::duration_cast >(t2 - t1).count(); + // std::cout<<"ttrack:"<0) + // T = tframe-vTimestampsCam[seq][ni-1]; + + // std::cout << "T: " << T << std::endl; + // std::cout << "ttrack: " << ttrack << std::endl; + + // if(ttrack 3.2 not found.") + endif() + +MESSAGE("OPENCV VERSION:") +MESSAGE(${OpenCV_VERSION}) + +find_package(Eigen3 3.1.0 REQUIRED) +find_package(Pangolin REQUIRED) +find_package(realsense2) + +include_directories( +${PROJECT_SOURCE_DIR} +${PROJECT_SOURCE_DIR}/include +${PROJECT_SOURCE_DIR}/include/CameraModels +${PROJECT_SOURCE_DIR}/Thirdparty/Sophus +${EIGEN3_INCLUDE_DIR} +${Pangolin_INCLUDE_DIRS} +) + +set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) + +add_library(${PROJECT_NAME} SHARED +src/System.cc +src/Tracking.cc +src/LocalMapping.cc +src/LoopClosing.cc +src/ORBextractor.cc +src/ORBmatcher.cc +src/FrameDrawer.cc +src/Converter.cc +src/MapPoint.cc +src/KeyFrame.cc +src/Atlas.cc +src/Map.cc +src/MapDrawer.cc +src/Optimizer.cc +src/Frame.cc +src/KeyFrameDatabase.cc +src/Sim3Solver.cc +src/Viewer.cc +src/ImuTypes.cc +src/G2oTypes.cc +src/CameraModels/Pinhole.cpp +src/CameraModels/KannalaBrandt8.cpp +src/OptimizableTypes.cpp +src/MLPnPsolver.cpp +src/GeometricTools.cc +src/TwoViewReconstruction.cc +src/Config.cc +src/Settings.cc +include/System.h +include/Tracking.h +include/LocalMapping.h +include/LoopClosing.h +include/ORBextractor.h +include/ORBmatcher.h +include/FrameDrawer.h +include/Converter.h +include/MapPoint.h +include/KeyFrame.h +include/Atlas.h +include/Map.h +include/MapDrawer.h +include/Optimizer.h +include/Frame.h +include/KeyFrameDatabase.h +include/Sim3Solver.h +include/Viewer.h +include/ImuTypes.h +include/G2oTypes.h +include/CameraModels/GeometricCamera.h +include/CameraModels/Pinhole.h +include/CameraModels/KannalaBrandt8.h +include/OptimizableTypes.h +include/MLPnPsolver.h +include/GeometricTools.h +include/TwoViewReconstruction.h +include/SerializationUtils.h +include/Config.h +include/Settings.h) + +add_subdirectory(Thirdparty/g2o) + +target_link_libraries(${PROJECT_NAME} +${OpenCV_LIBS} +${EIGEN3_LIBS} +${Pangolin_LIBRARIES} +${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so +${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so +-lboost_serialization +-lcrypto +) + +# If RealSense SDK is found the library is added and its examples compiled +if(realsense2_FOUND) + include_directories(${PROJECT_NAME} + ${realsense_INCLUDE_DIR} + ) + target_link_libraries(${PROJECT_NAME} + ${realsense2_LIBRARY} + ) +endif() + + +# Build examples + +# # RGB-D examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D) + +# add_executable(rgbd_tum +# Examples/RGB-D/rgbd_tum.cc) +# target_link_libraries(rgbd_tum ${PROJECT_NAME}) + +# if(realsense2_FOUND) +# add_executable(rgbd_realsense_D435i +# Examples/RGB-D/rgbd_realsense_D435i.cc) +# target_link_libraries(rgbd_realsense_D435i ${PROJECT_NAME}) +# endif() + + +# # RGB-D inertial examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D-Inertial) + +# if(realsense2_FOUND) +# add_executable(rgbd_inertial_realsense_D435i +# Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc) +# target_link_libraries(rgbd_inertial_realsense_D435i ${PROJECT_NAME}) +# endif() + +# #Stereo examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo) + +# add_executable(stereo_kitti +# Examples/Stereo/stereo_kitti.cc) +# target_link_libraries(stereo_kitti ${PROJECT_NAME}) + +# add_executable(stereo_euroc +# Examples/Stereo/stereo_euroc.cc) +# target_link_libraries(stereo_euroc ${PROJECT_NAME}) + +# add_executable(stereo_tum_vi +# Examples/Stereo/stereo_tum_vi.cc) +# target_link_libraries(stereo_tum_vi ${PROJECT_NAME}) + +# if(realsense2_FOUND) +# add_executable(stereo_realsense_t265 +# Examples/Stereo/stereo_realsense_t265.cc) +# target_link_libraries(stereo_realsense_t265 ${PROJECT_NAME}) + +# add_executable(stereo_realsense_D435i +# Examples/Stereo/stereo_realsense_D435i.cc) +# target_link_libraries(stereo_realsense_D435i ${PROJECT_NAME}) +# endif() + +# Monocular examples +# Setting binary ouput directory +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Application) + +# add_executable(mono_tum +# Examples/Monocular/mono_tum.cc) +# target_link_libraries(mono_tum ${PROJECT_NAME}) + +add_executable(mono_euroc_cap + Application/mono_euroc_cap.cc) +target_link_libraries(mono_euroc_cap ${PROJECT_NAME}) + +# add_executable(mono_kitti +# Examples/Monocular/mono_kitti.cc) +# target_link_libraries(mono_kitti ${PROJECT_NAME}) + +# add_executable(mono_euroc +# Examples/Monocular/mono_euroc.cc) +# target_link_libraries(mono_euroc ${PROJECT_NAME}) + +# add_executable(mono_tum_vi +# Examples/Monocular/mono_tum_vi.cc) +# target_link_libraries(mono_tum_vi ${PROJECT_NAME}) + +# if(realsense2_FOUND) +# add_executable(mono_realsense_t265 +# Examples/Monocular/mono_realsense_t265.cc) +# target_link_libraries(mono_realsense_t265 ${PROJECT_NAME}) + +# add_executable(mono_realsense_D435i +# Examples/Monocular/mono_realsense_D435i.cc) +# target_link_libraries(mono_realsense_D435i ${PROJECT_NAME}) +# endif() + +# #Monocular inertial examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular-Inertial) + +# add_executable(mono_inertial_euroc +# Examples/Monocular-Inertial/mono_inertial_euroc.cc) +# target_link_libraries(mono_inertial_euroc ${PROJECT_NAME}) + +# add_executable(mono_inertial_tum_vi +# Examples/Monocular-Inertial/mono_inertial_tum_vi.cc) +# target_link_libraries(mono_inertial_tum_vi ${PROJECT_NAME}) + +# if(realsense2_FOUND) +# add_executable(mono_inertial_realsense_t265 +# Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc) +# target_link_libraries(mono_inertial_realsense_t265 ${PROJECT_NAME}) + +# add_executable(mono_inertial_realsense_D435i +# Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc) +# target_link_libraries(mono_inertial_realsense_D435i ${PROJECT_NAME}) +# endif() + +# #Stereo Inertial examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo-Inertial) + +# add_executable(stereo_inertial_euroc +# Examples/Stereo-Inertial/stereo_inertial_euroc.cc) +# target_link_libraries(stereo_inertial_euroc ${PROJECT_NAME}) + +# add_executable(stereo_inertial_tum_vi +# Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc) +# target_link_libraries(stereo_inertial_tum_vi ${PROJECT_NAME}) + +# if(realsense2_FOUND) +# add_executable(stereo_inertial_realsense_t265 +# Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc) +# target_link_libraries(stereo_inertial_realsense_t265 ${PROJECT_NAME}) + +# add_executable(stereo_inertial_realsense_D435i +# Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc) +# target_link_libraries(stereo_inertial_realsense_D435i ${PROJECT_NAME}) +# endif() + +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Calibration) +# if(realsense2_FOUND) +# add_executable(recorder_realsense_D435i +# Examples/Calibration/recorder_realsense_D435i.cc) +# target_link_libraries(recorder_realsense_D435i ${PROJECT_NAME}) + +# add_executable(recorder_realsense_T265 +# Examples/Calibration/recorder_realsense_T265.cc) +# target_link_libraries(recorder_realsense_T265 ${PROJECT_NAME}) +# endif() + +# #Old examples + +# # RGB-D examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/RGB-D) + +# add_executable(rgbd_tum_old +# Examples_old/RGB-D/rgbd_tum.cc) +# target_link_libraries(rgbd_tum_old ${PROJECT_NAME}) + +# if(realsense2_FOUND) +# add_executable(rgbd_realsense_D435i_old +# Examples_old/RGB-D/rgbd_realsense_D435i.cc) +# target_link_libraries(rgbd_realsense_D435i_old ${PROJECT_NAME}) +# endif() + + +# # RGB-D inertial examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/RGB-D-Inertial) + +# if(realsense2_FOUND) +# add_executable(rgbd_inertial_realsense_D435i_old +# Examples_old/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc) +# target_link_libraries(rgbd_inertial_realsense_D435i_old ${PROJECT_NAME}) +# endif() + +# #Stereo examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/Stereo) + +# add_executable(stereo_kitti_old +# Examples_old/Stereo/stereo_kitti.cc) +# target_link_libraries(stereo_kitti_old ${PROJECT_NAME}) + +# add_executable(stereo_euroc_old +# Examples_old/Stereo/stereo_euroc.cc) +# target_link_libraries(stereo_euroc_old ${PROJECT_NAME}) + +# add_executable(stereo_tum_vi_old +# Examples_old/Stereo/stereo_tum_vi.cc) +# target_link_libraries(stereo_tum_vi_old ${PROJECT_NAME}) + +# if(realsense2_FOUND) +# add_executable(stereo_realsense_t265_old +# Examples_old/Stereo/stereo_realsense_t265.cc) +# target_link_libraries(stereo_realsense_t265_old ${PROJECT_NAME}) + +# add_executable(stereo_realsense_D435i_old +# Examples_old/Stereo/stereo_realsense_D435i.cc) +# target_link_libraries(stereo_realsense_D435i_old ${PROJECT_NAME}) +# endif() + +# #Monocular examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/Monocular) + +# add_executable(mono_tum_old +# Examples_old/Monocular/mono_tum.cc) +# target_link_libraries(mono_tum_old ${PROJECT_NAME}) + +# add_executable(mono_kitti_old +# Examples_old/Monocular/mono_kitti.cc) +# target_link_libraries(mono_kitti_old ${PROJECT_NAME}) + +# add_executable(mono_euroc_old +# Examples_old/Monocular/mono_euroc.cc) +# target_link_libraries(mono_euroc_old ${PROJECT_NAME}) + +# add_executable(mono_tum_vi_old +# Examples_old/Monocular/mono_tum_vi.cc) +# target_link_libraries(mono_tum_vi_old ${PROJECT_NAME}) + +# if(realsense2_FOUND) +# add_executable(mono_realsense_t265_old +# Examples_old/Monocular/mono_realsense_t265.cc) +# target_link_libraries(mono_realsense_t265_old ${PROJECT_NAME}) + +# add_executable(mono_realsense_D435i_old +# Examples_old/Monocular/mono_realsense_D435i.cc) +# target_link_libraries(mono_realsense_D435i_old ${PROJECT_NAME}) +# endif() + +#Monocular inertial examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/Monocular-Inertial) + +# add_executable(mono_inertial_euroc_old +# Examples_old/Monocular-Inertial/mono_inertial_euroc.cc) +# target_link_libraries(mono_inertial_euroc_old ${PROJECT_NAME}) + +# add_executable(mono_inertial_tum_vi_old +# Examples_old/Monocular-Inertial/mono_inertial_tum_vi.cc) +# target_link_libraries(mono_inertial_tum_vi_old ${PROJECT_NAME}) + +# if(realsense2_FOUND) +# add_executable(mono_inertial_realsense_t265_old +# Examples_old/Monocular-Inertial/mono_inertial_realsense_t265.cc) +# target_link_libraries(mono_inertial_realsense_t265_old ${PROJECT_NAME}) + +# add_executable(mono_inertial_realsense_D435i_old +# Examples_old/Monocular-Inertial/mono_inertial_realsense_D435i.cc) +# target_link_libraries(mono_inertial_realsense_D435i_old ${PROJECT_NAME}) +# endif() + +# #Stereo Inertial examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/Stereo-Inertial) + +# add_executable(stereo_inertial_euroc_old +# Examples_old/Stereo-Inertial/stereo_inertial_euroc.cc) +# target_link_libraries(stereo_inertial_euroc_old ${PROJECT_NAME}) + +# add_executable(stereo_inertial_tum_vi_old +# Examples_old/Stereo-Inertial/stereo_inertial_tum_vi.cc) +# target_link_libraries(stereo_inertial_tum_vi_old ${PROJECT_NAME}) + +# if(realsense2_FOUND) +# add_executable(stereo_inertial_realsense_t265_old +# Examples_old/Stereo-Inertial/stereo_inertial_realsense_t265.cc) +# target_link_libraries(stereo_inertial_realsense_t265_old ${PROJECT_NAME}) + +# add_executable(stereo_inertial_realsense_D435i_old +# Examples_old/Stereo-Inertial/stereo_inertial_realsense_D435i.cc) +# target_link_libraries(stereo_inertial_realsense_D435i_old ${PROJECT_NAME}) +# endif() diff --git a/ORB-SLAM3-UESTC/Workspace/README.md b/ORB-SLAM3-UESTC/Workspace/README.md new file mode 100644 index 0000000..f4d0996 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/README.md @@ -0,0 +1,36 @@ + + +# ORB-SLAM3 -UESTC使用说明 + +## 一、基本操作 + +### 1.1 代码的编译 + +```shell +# 如果没有build文件夹则创建build文件夹 +mkdir build +cd build +# 该命令会在当前目录的上级目录中查找CMakeLists.txt文件，并使用CMake生成Makefile。CMAKE_BUILD_TYPE指定编译的模式，这里指定为# Release，表示编译优化版本，以提高代码的性能 +cmake .. -DCMAKE_BUILD_TYPE=Release +# 该命令会使用Makefile编译和构建代码。Makefile是由CMake生成的，它包含了编译和构建代码所需的所有信息。在运行make命令后，它会# # 编译和构建代码，并在当前目录生成可执行文件或库文件 + +# make -j4 命令用于在Linux或Unix系统上使用多个线程同时编译代码 +make +``` + +### 1.2 代码的运行 + +​ 实际上是执行Examples目录下的mono_euroc_cap程序并提供两个参数信息，分别为ORBvoc.txt和cam2.yaml。 + +​ ORBvoc是ORB-SLA-M2中使用的词汇表（vocabulary），也称为字典。ORB-SLAM2是一种基于特征点的实时单目SLAM算法，用于在机器人或无人机等设备上进行实时定位和建图。ORBvoc词汇表包含了一组视觉特征点的描述符，用于对图像进行特征点匹配和建图。在ORB-SLAM2中，ORB-voc词汇表用于实现回环检测和重定位，以提高定位和建图的准确性和鲁棒性。ORBvoc词汇表是通过使用一些大规模的数据集（如TUM、KITTI等）进行训练得到的，可以通过下载预训练的ORBvoc词汇表来加速ORB-SLAM2的使用。 + +​ cam.yaml是摄像头的标定信息。 + +```shell +# 原版 +# 运行可执行程序 mono_euroc_cap 参数0为ORBvoc.txt 参数1为./Examples/Monocular/cam2.yaml +./Examples/Monocular/mono_euroc_cap ./Vocabulary/ORBvoc.txt ./Examples/Monocular/cam2.yaml +# zhanli整理版 +./Application/mono_euroc_cap ./Vocabulary/ORBvoc.txt ./Application/cam2.yaml +``` + diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/CMakeLists.txt b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/CMakeLists.txt new file mode 100644 index 0000000..9517aca --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 2.8) +project(DBoW2) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") + +set(HDRS_DBOW2 + DBoW2/BowVector.h + DBoW2/FORB.h + DBoW2/FClass.h + DBoW2/FeatureVector.h + DBoW2/ScoringObject.h + DBoW2/TemplatedVocabulary.h) +set(SRCS_DBOW2 + DBoW2/BowVector.cpp + DBoW2/FORB.cpp + DBoW2/FeatureVector.cpp + DBoW2/ScoringObject.cpp) + +set(HDRS_DUTILS + DUtils/Random.h + DUtils/Timestamp.h) +set(SRCS_DUTILS + DUtils/Random.cpp + DUtils/Timestamp.cpp) + +# 要保证整个工程的opencv版本一致，包括dbow，源码以及ros相关的 +# 3 4 都可以正常运行 +find_package(OpenCV 3.2 QUIET) +if(NOT OpenCV_FOUND) + find_package(OpenCV 3.0 QUIET) + if(NOT OpenCV_FOUND) + message(FATAL_ERROR "OpenCV > 3.0 not found.") + endif() +endif() + +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +include_directories(${OpenCV_INCLUDE_DIRS}) +add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS}) +target_link_libraries(DBoW2 ${OpenCV_LIBS}) + diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/BowVector.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/BowVector.cpp new file mode 100644 index 0000000..1337fa3 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/BowVector.cpp @@ -0,0 +1,130 @@ +/** + * File: BowVector.cpp + * Date: March 2011 + * Author: Dorian Galvez-Lopez + * Description: bag of words vector + * License: see the LICENSE.txt file + * + */ + +#include +#include +#include +#include +#include + +#include "BowVector.h" + +namespace DBoW2 { + +// -------------------------------------------------------------------------- + +BowVector::BowVector(void) +{ +} + +// -------------------------------------------------------------------------- + +BowVector::~BowVector(void) +{ +} + +// -------------------------------------------------------------------------- + +void BowVector::addWeight(WordId id, WordValue v) +{ + BowVector::iterator vit = this->lower_bound(id); + + if(vit != this->end() && !(this->key_comp()(id, vit->first))) + { + vit->second += v; + } + else + { + this->insert(vit, BowVector::value_type(id, v)); + } +} + +// -------------------------------------------------------------------------- + +void BowVector::addIfNotExist(WordId id, WordValue v) +{ + BowVector::iterator vit = this->lower_bound(id); + + if(vit == this->end() || (this->key_comp()(id, vit->first))) + { + this->insert(vit, BowVector::value_type(id, v)); + } +} + +// -------------------------------------------------------------------------- + +void BowVector::normalize(LNorm norm_type) +{ + double norm = 0.0; + BowVector::iterator it; + + if(norm_type == DBoW2::L1) + { + for(it = begin(); it != end(); ++it) + norm += fabs(it->second); + } + else + { + for(it = begin(); it != end(); ++it) + norm += it->second * it->second; + norm = sqrt(norm); + } + + if(norm > 0.0) + { + for(it = begin(); it != end(); ++it) + it->second /= norm; + } +} + +// -------------------------------------------------------------------------- + +std::ostream& operator<< (std::ostream &out, const BowVector &v) +{ + BowVector::const_iterator vit; + std::vector::const_iterator iit; + unsigned int i = 0; + const unsigned int N = v.size(); + for(vit = v.begin(); vit != v.end(); ++vit, ++i) + { + out << "<" << vit->first << ", " << vit->second << ">"; + + if(i < N-1) out << ", "; + } + return out; +} + +// -------------------------------------------------------------------------- + +void BowVector::saveM(const std::string &filename, size_t W) const +{ + std::fstream f(filename.c_str(), std::ios::out); + + WordId last = 0; + BowVector::const_iterator bit; + for(bit = this->begin(); bit != this->end(); ++bit) + { + for(; last < bit->first; ++last) + { + f << "0 "; + } + f << bit->second << " "; + + last = bit->first + 1; + } + for(; last < (WordId)W; ++last) + f << "0 "; + + f.close(); +} + +// -------------------------------------------------------------------------- + +} // namespace DBoW2 + diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/BowVector.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/BowVector.h new file mode 100644 index 0000000..a7a12a9 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/BowVector.h @@ -0,0 +1,119 @@ +/** + * File: BowVector.h + * Date: March 2011 + * Author: Dorian Galvez-Lopez + * Description: bag of words vector + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T_BOW_VECTOR__ +#define __D_T_BOW_VECTOR__ + +#include +#include +#include + +#include +#include + +namespace DBoW2 { + +/// Id of words +typedef unsigned int WordId; + +/// Value of a word +typedef double WordValue; + +/// Id of nodes in the vocabulary treee +typedef unsigned int NodeId; + +/// L-norms for normalization +enum LNorm +{ + L1, + L2 +}; + +/// Weighting type +enum WeightingType +{ + TF_IDF, + TF, + IDF, + BINARY +}; + +/// Scoring type +enum ScoringType +{ + L1_NORM, + L2_NORM, + CHI_SQUARE, + KL, + BHATTACHARYYA, + DOT_PRODUCT, +}; + +/// Vector of words to represent images +class BowVector: + public std::map +{ + friend class boost::serialization::access; + template + void serialize(Archive& ar, const int version) + { + ar & boost::serialization::base_object >(*this); + } + +public: + + /** + * Constructor + */ + BowVector(void); + + /** + * Destructor + */ + ~BowVector(void); + + /** + * Adds a value to a word value existing in the vector, or creates a new + * word with the given value + * @param id word id to look for + * @param v value to create the word with, or to add to existing word + */ + void addWeight(WordId id, WordValue v); + + /** + * Adds a word with a value to the vector only if this does not exist yet + * @param id word id to look for + * @param v value to give to the word if this does not exist + */ + void addIfNotExist(WordId id, WordValue v); + + /** + * L1-Normalizes the values in the vector + * @param norm_type norm used + */ + void normalize(LNorm norm_type); + + /** + * Prints the content of the bow vector + * @param out stream + * @param v + */ + friend std::ostream& operator<<(std::ostream &out, const BowVector &v); + + /** + * Saves the bow vector as a vector in a matlab file + * @param filename + * @param W number of words in the vocabulary + */ + void saveM(const std::string &filename, size_t W) const; +}; + +} // namespace DBoW2 + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/FClass.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/FClass.h new file mode 100644 index 0000000..13be53d --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/FClass.h @@ -0,0 +1,71 @@ +/** + * File: FClass.h + * Date: November 2011 + * Author: Dorian Galvez-Lopez + * Description: generic FClass to instantiate templated classes + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T_FCLASS__ +#define __D_T_FCLASS__ + +#include +#include +#include + +namespace DBoW2 { + +/// Generic class to encapsulate functions to manage descriptors. +/** + * This class must be inherited. Derived classes can be used as the + * parameter F when creating Templated structures + * (TemplatedVocabulary, TemplatedDatabase, ...) + */ +class FClass +{ + class TDescriptor; + typedef const TDescriptor *pDescriptor; + + /** + * Calculates the mean value of a set of descriptors + * @param descriptors + * @param mean mean descriptor + */ + virtual void meanValue(const std::vector &descriptors, + TDescriptor &mean) = 0; + + /** + * Calculates the distance between two descriptors + * @param a + * @param b + * @return distance + */ + static double distance(const TDescriptor &a, const TDescriptor &b); + + /** + * Returns a string version of the descriptor + * @param a descriptor + * @return string version + */ + static std::string toString(const TDescriptor &a); + + /** + * Returns a descriptor from a string + * @param a descriptor + * @param s string version + */ + static void fromString(TDescriptor &a, const std::string &s); + + /** + * Returns a mat with the descriptors in float format + * @param descriptors + * @param mat (out) NxL 32F matrix + */ + static void toMat32F(const std::vector &descriptors, + cv::Mat &mat); +}; + +} // namespace DBoW2 + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/FORB.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/FORB.cpp new file mode 100644 index 0000000..1f1990c --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/FORB.cpp @@ -0,0 +1,193 @@ +/** + * File: FORB.cpp + * Date: June 2012 + * Author: Dorian Galvez-Lopez + * Description: functions for ORB descriptors + * License: see the LICENSE.txt file + * + * Distance function has been modified + * + */ + + +#include +#include +#include +#include + +#include "FORB.h" + +using namespace std; + +namespace DBoW2 { + +// -------------------------------------------------------------------------- + +const int FORB::L=32; + +void FORB::meanValue(const std::vector &descriptors, + FORB::TDescriptor &mean) +{ + if(descriptors.empty()) + { + mean.release(); + return; + } + else if(descriptors.size() == 1) + { + mean = descriptors[0]->clone(); + } + else + { + vector sum(FORB::L * 8, 0); + + for(size_t i = 0; i < descriptors.size(); ++i) + { + const cv::Mat &d = *descriptors[i]; + const unsigned char *p = d.ptr(); + + for(int j = 0; j < d.cols; ++j, ++p) + { + if(*p & (1 << 7)) ++sum[ j*8 ]; + if(*p & (1 << 6)) ++sum[ j*8 + 1 ]; + if(*p & (1 << 5)) ++sum[ j*8 + 2 ]; + if(*p & (1 << 4)) ++sum[ j*8 + 3 ]; + if(*p & (1 << 3)) ++sum[ j*8 + 4 ]; + if(*p & (1 << 2)) ++sum[ j*8 + 5 ]; + if(*p & (1 << 1)) ++sum[ j*8 + 6 ]; + if(*p & (1)) ++sum[ j*8 + 7 ]; + } + } + + mean = cv::Mat::zeros(1, FORB::L, CV_8U); + unsigned char *p = mean.ptr(); + + const int N2 = (int)descriptors.size() / 2 + descriptors.size() % 2; + for(size_t i = 0; i < sum.size(); ++i) + { + if(sum[i] >= N2) + { + // set bit + *p |= 1 << (7 - (i % 8)); + } + + if(i % 8 == 7) ++p; + } + } +} + +// -------------------------------------------------------------------------- + +int FORB::distance(const FORB::TDescriptor &a, + const FORB::TDescriptor &b) +{ + // Bit set count operation from + // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel + + const int *pa = a.ptr(); + const int *pb = b.ptr(); + + int dist=0; + + for(int i=0; i<8; i++, pa++, pb++) + { + unsigned int v = *pa ^ *pb; + v = v - ((v >> 1) & 0x55555555); + v = (v & 0x33333333) + ((v >> 2) & 0x33333333); + dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24; + } + + return dist; +} + +// -------------------------------------------------------------------------- + +std::string FORB::toString(const FORB::TDescriptor &a) +{ + stringstream ss; + const unsigned char *p = a.ptr(); + + for(int i = 0; i < a.cols; ++i, ++p) + { + ss << (int)*p << " "; + } + + return ss.str(); +} + +// -------------------------------------------------------------------------- + +void FORB::fromString(FORB::TDescriptor &a, const std::string &s) +{ + a.create(1, FORB::L, CV_8U); + unsigned char *p = a.ptr(); + + stringstream ss(s); + for(int i = 0; i < FORB::L; ++i, ++p) + { + int n; + ss >> n; + + if(!ss.fail()) + *p = (unsigned char)n; + } + +} + +// -------------------------------------------------------------------------- + +void FORB::toMat32F(const std::vector &descriptors, + cv::Mat &mat) +{ + if(descriptors.empty()) + { + mat.release(); + return; + } + + const size_t N = descriptors.size(); + + mat.create(N, FORB::L*8, CV_32F); + float *p = mat.ptr(); + + for(size_t i = 0; i < N; ++i) + { + const int C = descriptors[i].cols; + const unsigned char *desc = descriptors[i].ptr(); + + for(int j = 0; j < C; ++j, p += 8) + { + p[0] = (desc[j] & (1 << 7) ? 1 : 0); + p[1] = (desc[j] & (1 << 6) ? 1 : 0); + p[2] = (desc[j] & (1 << 5) ? 1 : 0); + p[3] = (desc[j] & (1 << 4) ? 1 : 0); + p[4] = (desc[j] & (1 << 3) ? 1 : 0); + p[5] = (desc[j] & (1 << 2) ? 1 : 0); + p[6] = (desc[j] & (1 << 1) ? 1 : 0); + p[7] = desc[j] & (1); + } + } +} + +// -------------------------------------------------------------------------- + +void FORB::toMat8U(const std::vector &descriptors, + cv::Mat &mat) +{ + mat.create(descriptors.size(), 32, CV_8U); + + unsigned char *p = mat.ptr(); + + for(size_t i = 0; i < descriptors.size(); ++i, p += 32) + { + const unsigned char *d = descriptors[i].ptr(); + std::copy(d, d+32, p); + } + +} + +// -------------------------------------------------------------------------- + +} // namespace DBoW2 + + diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/FORB.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/FORB.h new file mode 100644 index 0000000..a39599f --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/FORB.h @@ -0,0 +1,79 @@ +/** + * File: FORB.h + * Date: June 2012 + * Author: Dorian Galvez-Lopez + * Description: functions for ORB descriptors + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T_F_ORB__ +#define __D_T_F_ORB__ + +#include +#include +#include + +#include "FClass.h" + +namespace DBoW2 { + +/// Functions to manipulate ORB descriptors +class FORB: protected FClass +{ +public: + + /// Descriptor type + typedef cv::Mat TDescriptor; // CV_8U + /// Pointer to a single descriptor + typedef const TDescriptor *pDescriptor; + /// Descriptor length (in bytes) + static const int L; + + /** + * Calculates the mean value of a set of descriptors + * @param descriptors + * @param mean mean descriptor + */ + static void meanValue(const std::vector &descriptors, + TDescriptor &mean); + + /** + * Calculates the distance between two descriptors + * @param a + * @param b + * @return distance + */ + static int distance(const TDescriptor &a, const TDescriptor &b); + + /** + * Returns a string version of the descriptor + * @param a descriptor + * @return string version + */ + static std::string toString(const TDescriptor &a); + + /** + * Returns a descriptor from a string + * @param a descriptor + * @param s string version + */ + static void fromString(TDescriptor &a, const std::string &s); + + /** + * Returns a mat with the descriptors in float format + * @param descriptors + * @param mat (out) NxL 32F matrix + */ + static void toMat32F(const std::vector &descriptors, + cv::Mat &mat); + + static void toMat8U(const std::vector &descriptors, + cv::Mat &mat); + +}; + +} // namespace DBoW2 + +#endif + diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/FeatureVector.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/FeatureVector.cpp new file mode 100644 index 0000000..c055a15 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/FeatureVector.cpp @@ -0,0 +1,85 @@ +/** + * File: FeatureVector.cpp + * Date: November 2011 + * Author: Dorian Galvez-Lopez + * Description: feature vector + * License: see the LICENSE.txt file + * + */ + +#include "FeatureVector.h" +#include +#include +#include + +namespace DBoW2 { + +// --------------------------------------------------------------------------- + +FeatureVector::FeatureVector(void) +{ +} + +// --------------------------------------------------------------------------- + +FeatureVector::~FeatureVector(void) +{ +} + +// --------------------------------------------------------------------------- + +void FeatureVector::addFeature(NodeId id, unsigned int i_feature) +{ + FeatureVector::iterator vit = this->lower_bound(id); + + if(vit != this->end() && vit->first == id) + { + vit->second.push_back(i_feature); + } + else + { + vit = this->insert(vit, FeatureVector::value_type(id, + std::vector() )); + vit->second.push_back(i_feature); + } +} + +// --------------------------------------------------------------------------- + +std::ostream& operator<<(std::ostream &out, + const FeatureVector &v) +{ + if(!v.empty()) + { + FeatureVector::const_iterator vit = v.begin(); + + const std::vector* f = &vit->second; + + out << "<" << vit->first << ": ["; + if(!f->empty()) out << (*f)[0]; + for(unsigned int i = 1; i < f->size(); ++i) + { + out << ", " << (*f)[i]; + } + out << "]>"; + + for(++vit; vit != v.end(); ++vit) + { + f = &vit->second; + + out << ", <" << vit->first << ": ["; + if(!f->empty()) out << (*f)[0]; + for(unsigned int i = 1; i < f->size(); ++i) + { + out << ", " << (*f)[i]; + } + out << "]>"; + } + } + + return out; +} + +// --------------------------------------------------------------------------- + +} // namespace DBoW2 diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/FeatureVector.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/FeatureVector.h new file mode 100644 index 0000000..426f36d --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/FeatureVector.h @@ -0,0 +1,66 @@ +/** + * File: FeatureVector.h + * Date: November 2011 + * Author: Dorian Galvez-Lopez + * Description: feature vector + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T_FEATURE_VECTOR__ +#define __D_T_FEATURE_VECTOR__ + +#include "BowVector.h" +#include +#include +#include + +#include +#include + +namespace DBoW2 { + +/// Vector of nodes with indexes of local features +class FeatureVector: + public std::map > +{ + friend class boost::serialization::access; + template + void serialize(Archive& ar, const int version) + { + ar & boost::serialization::base_object > >(*this); + } + +public: + + /** + * Constructor + */ + FeatureVector(void); + + /** + * Destructor + */ + ~FeatureVector(void); + + /** + * Adds a feature to an existing node, or adds a new node with an initial + * feature + * @param id node id to add or to modify + * @param i_feature index of feature to add to the given node + */ + void addFeature(NodeId id, unsigned int i_feature); + + /** + * Sends a string versions of the feature vector through the stream + * @param out stream + * @param v feature vector + */ + friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); + +}; + +} // namespace DBoW2 + +#endif + diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/ScoringObject.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/ScoringObject.cpp new file mode 100644 index 0000000..063a96e --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/ScoringObject.cpp @@ -0,0 +1,315 @@ +/** + * File: ScoringObject.cpp + * Date: November 2011 + * Author: Dorian Galvez-Lopez + * Description: functions to compute bow scores + * License: see the LICENSE.txt file + * + */ + +#include +#include "TemplatedVocabulary.h" +#include "BowVector.h" + +using namespace DBoW2; + +// If you change the type of WordValue, make sure you change also the +// epsilon value (this is needed by the KL method) +const double GeneralScoring::LOG_EPS = log(DBL_EPSILON); // FLT_EPSILON + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +double L1Scoring::score(const BowVector &v1, const BowVector &v2) const +{ + BowVector::const_iterator v1_it, v2_it; + const BowVector::const_iterator v1_end = v1.end(); + const BowVector::const_iterator v2_end = v2.end(); + + v1_it = v1.begin(); + v2_it = v2.begin(); + + double score = 0; + + while(v1_it != v1_end && v2_it != v2_end) + { + const WordValue& vi = v1_it->second; + const WordValue& wi = v2_it->second; + + if(v1_it->first == v2_it->first) + { + score += fabs(vi - wi) - fabs(vi) - fabs(wi); + + // move v1 and v2 forward + ++v1_it; + ++v2_it; + } + else if(v1_it->first < v2_it->first) + { + // move v1 forward + v1_it = v1.lower_bound(v2_it->first); + // v1_it = (first element >= v2_it.id) + } + else + { + // move v2 forward + v2_it = v2.lower_bound(v1_it->first); + // v2_it = (first element >= v1_it.id) + } + } + + // ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|) + // for all i | v_i != 0 and w_i != 0 + // (Nister, 2006) + // scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1} + score = -score/2.0; + + return score; // [0..1] +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +double L2Scoring::score(const BowVector &v1, const BowVector &v2) const +{ + BowVector::const_iterator v1_it, v2_it; + const BowVector::const_iterator v1_end = v1.end(); + const BowVector::const_iterator v2_end = v2.end(); + + v1_it = v1.begin(); + v2_it = v2.begin(); + + double score = 0; + + while(v1_it != v1_end && v2_it != v2_end) + { + const WordValue& vi = v1_it->second; + const WordValue& wi = v2_it->second; + + if(v1_it->first == v2_it->first) + { + score += vi * wi; + + // move v1 and v2 forward + ++v1_it; + ++v2_it; + } + else if(v1_it->first < v2_it->first) + { + // move v1 forward + v1_it = v1.lower_bound(v2_it->first); + // v1_it = (first element >= v2_it.id) + } + else + { + // move v2 forward + v2_it = v2.lower_bound(v1_it->first); + // v2_it = (first element >= v1_it.id) + } + } + + // ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) ) + // for all i | v_i != 0 and w_i != 0 ) + // (Nister, 2006) + if(score >= 1) // rounding errors + score = 1.0; + else + score = 1.0 - sqrt(1.0 - score); // [0..1] + + return score; +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +double ChiSquareScoring::score(const BowVector &v1, const BowVector &v2) + const +{ + BowVector::const_iterator v1_it, v2_it; + const BowVector::const_iterator v1_end = v1.end(); + const BowVector::const_iterator v2_end = v2.end(); + + v1_it = v1.begin(); + v2_it = v2.begin(); + + double score = 0; + + // all the items are taken into account + + while(v1_it != v1_end && v2_it != v2_end) + { + const WordValue& vi = v1_it->second; + const WordValue& wi = v2_it->second; + + if(v1_it->first == v2_it->first) + { + // (v-w)^2/(v+w) - v - w = -4 vw/(v+w) + // we move the -4 out + if(vi + wi != 0.0) score += vi * wi / (vi + wi); + + // move v1 and v2 forward + ++v1_it; + ++v2_it; + } + else if(v1_it->first < v2_it->first) + { + // move v1 forward + v1_it = v1.lower_bound(v2_it->first); + } + else + { + // move v2 forward + v2_it = v2.lower_bound(v1_it->first); + } + } + + // this takes the -4 into account + score = 2. * score; // [0..1] + + return score; +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +double KLScoring::score(const BowVector &v1, const BowVector &v2) const +{ + BowVector::const_iterator v1_it, v2_it; + const BowVector::const_iterator v1_end = v1.end(); + const BowVector::const_iterator v2_end = v2.end(); + + v1_it = v1.begin(); + v2_it = v2.begin(); + + double score = 0; + + // all the items or v are taken into account + + while(v1_it != v1_end && v2_it != v2_end) + { + const WordValue& vi = v1_it->second; + const WordValue& wi = v2_it->second; + + if(v1_it->first == v2_it->first) + { + if(vi != 0 && wi != 0) score += vi * log(vi/wi); + + // move v1 and v2 forward + ++v1_it; + ++v2_it; + } + else if(v1_it->first < v2_it->first) + { + // move v1 forward + score += vi * (log(vi) - LOG_EPS); + ++v1_it; + } + else + { + // move v2_it forward, do not add any score + v2_it = v2.lower_bound(v1_it->first); + // v2_it = (first element >= v1_it.id) + } + } + + // sum rest of items of v + for(; v1_it != v1_end; ++v1_it) + if(v1_it->second != 0) + score += v1_it->second * (log(v1_it->second) - LOG_EPS); + + return score; // cannot be scaled +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +double BhattacharyyaScoring::score(const BowVector &v1, + const BowVector &v2) const +{ + BowVector::const_iterator v1_it, v2_it; + const BowVector::const_iterator v1_end = v1.end(); + const BowVector::const_iterator v2_end = v2.end(); + + v1_it = v1.begin(); + v2_it = v2.begin(); + + double score = 0; + + while(v1_it != v1_end && v2_it != v2_end) + { + const WordValue& vi = v1_it->second; + const WordValue& wi = v2_it->second; + + if(v1_it->first == v2_it->first) + { + score += sqrt(vi * wi); + + // move v1 and v2 forward + ++v1_it; + ++v2_it; + } + else if(v1_it->first < v2_it->first) + { + // move v1 forward + v1_it = v1.lower_bound(v2_it->first); + // v1_it = (first element >= v2_it.id) + } + else + { + // move v2 forward + v2_it = v2.lower_bound(v1_it->first); + // v2_it = (first element >= v1_it.id) + } + } + + return score; // already scaled +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +double DotProductScoring::score(const BowVector &v1, + const BowVector &v2) const +{ + BowVector::const_iterator v1_it, v2_it; + const BowVector::const_iterator v1_end = v1.end(); + const BowVector::const_iterator v2_end = v2.end(); + + v1_it = v1.begin(); + v2_it = v2.begin(); + + double score = 0; + + while(v1_it != v1_end && v2_it != v2_end) + { + const WordValue& vi = v1_it->second; + const WordValue& wi = v2_it->second; + + if(v1_it->first == v2_it->first) + { + score += vi * wi; + + // move v1 and v2 forward + ++v1_it; + ++v2_it; + } + else if(v1_it->first < v2_it->first) + { + // move v1 forward + v1_it = v1.lower_bound(v2_it->first); + // v1_it = (first element >= v2_it.id) + } + else + { + // move v2 forward + v2_it = v2.lower_bound(v1_it->first); + // v2_it = (first element >= v1_it.id) + } + } + + return score; // cannot scale +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/ScoringObject.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/ScoringObject.h new file mode 100644 index 0000000..8d5b821 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/ScoringObject.h @@ -0,0 +1,96 @@ +/** + * File: ScoringObject.h + * Date: November 2011 + * Author: Dorian Galvez-Lopez + * Description: functions to compute bow scores + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T_SCORING_OBJECT__ +#define __D_T_SCORING_OBJECT__ + +#include "BowVector.h" + +namespace DBoW2 { + +/// Base class of scoring functions +class GeneralScoring +{ +public: + /** + * Computes the score between two vectors. Vectors must be sorted and + * normalized if necessary + * @param v (in/out) + * @param w (in/out) + * @return score + */ + virtual double score(const BowVector &v, const BowVector &w) const = 0; + + /** + * Returns whether a vector must be normalized before scoring according + * to the scoring scheme + * @param norm norm to use + * @return true iff must normalize + */ + virtual bool mustNormalize(LNorm &norm) const = 0; + + /// Log of epsilon + static const double LOG_EPS; + // If you change the type of WordValue, make sure you change also the + // epsilon value (this is needed by the KL method) + + virtual ~GeneralScoring() {} //!< Required for virtual base classes + +}; + +/** + * Macro for defining Scoring classes + * @param NAME name of class + * @param MUSTNORMALIZE if vectors must be normalized to compute the score + * @param NORM type of norm to use when MUSTNORMALIZE + */ +#define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ + NAME: public GeneralScoring \ + { public: \ + /** \ + * Computes score between two vectors \ + * @param v \ + * @param w \ + * @return score between v and w \ + */ \ + virtual double score(const BowVector &v, const BowVector &w) const; \ + \ + /** \ + * Says if a vector must be normalized according to the scoring function \ + * @param norm (out) if true, norm to use + * @return true iff vectors must be normalized \ + */ \ + virtual inline bool mustNormalize(LNorm &norm) const \ + { norm = NORM; return MUSTNORMALIZE; } \ + } + +/// L1 Scoring object +class __SCORING_CLASS(L1Scoring, true, L1); + +/// L2 Scoring object +class __SCORING_CLASS(L2Scoring, true, L2); + +/// Chi square Scoring object +class __SCORING_CLASS(ChiSquareScoring, true, L1); + +/// KL divergence Scoring object +class __SCORING_CLASS(KLScoring, true, L1); + +/// Bhattacharyya Scoring object +class __SCORING_CLASS(BhattacharyyaScoring, true, L1); + +/// Dot product Scoring object +class __SCORING_CLASS(DotProductScoring, false, L1); + +#undef __SCORING_CLASS + +} // namespace DBoW2 + +#endif + diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h new file mode 100644 index 0000000..0195934 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h @@ -0,0 +1,1665 @@ +/** + * This is a modified version of TemplatedVocabulary.h from DBoW2 (see below). + * Added functions: Save and Load from text files without using cv::FileStorage. + * Date: August 2015 + * Raúl Mur-Artal + */ + +/** + * File: TemplatedVocabulary.h + * Date: February 2011 + * Author: Dorian Galvez-Lopez + * Description: templated vocabulary + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T_TEMPLATED_VOCABULARY__ +#define __D_T_TEMPLATED_VOCABULARY__ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "FeatureVector.h" +#include "BowVector.h" +#include "ScoringObject.h" + +#include "../DUtils/Random.h" + +using namespace std; + +namespace DBoW2 { + +/// @param TDescriptor class of descriptor +/// @param F class of descriptor functions +template +/// Generic Vocabulary +class TemplatedVocabulary +{ +public: + + /** + * Initiates an empty vocabulary + * @param k branching factor + * @param L depth levels + * @param weighting weighting type + * @param scoring scoring type + */ + TemplatedVocabulary(int k = 10, int L = 5, + WeightingType weighting = TF_IDF, ScoringType scoring = L1_NORM); + + /** + * Creates the vocabulary by loading a file + * @param filename + */ + TemplatedVocabulary(const std::string &filename); + + /** + * Creates the vocabulary by loading a file + * @param filename + */ + TemplatedVocabulary(const char *filename); + + /** + * Copy constructor + * @param voc + */ + TemplatedVocabulary(const TemplatedVocabulary &voc); + + /** + * Destructor + */ + virtual ~TemplatedVocabulary(); + + /** + * Assigns the given vocabulary to this by copying its data and removing + * all the data contained by this vocabulary before + * @param voc + * @return reference to this vocabulary + */ + TemplatedVocabulary& operator=( + const TemplatedVocabulary &voc); + + /** + * Creates a vocabulary from the training features with the already + * defined parameters + * @param training_features + */ + virtual void create + (const std::vector > &training_features); + + /** + * Creates a vocabulary from the training features, setting the branching + * factor and the depth levels of the tree + * @param training_features + * @param k branching factor + * @param L depth levels + */ + virtual void create + (const std::vector > &training_features, + int k, int L); + + /** + * Creates a vocabulary from the training features, setting the branching + * factor nad the depth levels of the tree, and the weighting and scoring + * schemes + */ + virtual void create + (const std::vector > &training_features, + int k, int L, WeightingType weighting, ScoringType scoring); + + /** + * Returns the number of words in the vocabulary + * @return number of words + */ + virtual inline unsigned int size() const; + + /** + * Returns whether the vocabulary is empty (i.e. it has not been trained) + * @return true iff the vocabulary is empty + */ + virtual inline bool empty() const; + + /** + * Transforms a set of descriptores into a bow vector + * @param features + * @param v (out) bow vector of weighted words + */ + virtual void transform(const std::vector& features, BowVector &v) + const; + + /** + * Transform a set of descriptors into a bow vector and a feature vector + * @param features + * @param v (out) bow vector + * @param fv (out) feature vector of nodes and feature indexes + * @param levelsup levels to go up the vocabulary tree to get the node index + */ + virtual void transform(const std::vector& features, + BowVector &v, FeatureVector &fv, int levelsup) const; + + /** + * Transforms a single feature into a word (without weight) + * @param feature + * @return word id + */ + virtual WordId transform(const TDescriptor& feature) const; + + /** + * Returns the score of two vectors + * @param a vector + * @param b vector + * @return score between vectors + * @note the vectors must be already sorted and normalized if necessary + */ + inline double score(const BowVector &a, const BowVector &b) const; + + /** + * Returns the id of the node that is "levelsup" levels from the word given + * @param wid word id + * @param levelsup 0..L + * @return node id. if levelsup is 0, returns the node id associated to the + * word id + */ + virtual NodeId getParentNode(WordId wid, int levelsup) const; + + /** + * Returns the ids of all the words that are under the given node id, + * by traversing any of the branches that goes down from the node + * @param nid starting node id + * @param words ids of words + */ + void getWordsFromNode(NodeId nid, std::vector &words) const; + + /** + * Returns the branching factor of the tree (k) + * @return k + */ + inline int getBranchingFactor() const { return m_k; } + + /** + * Returns the depth levels of the tree (L) + * @return L + */ + inline int getDepthLevels() const { return m_L; } + + /** + * Returns the real depth levels of the tree on average + * @return average of depth levels of leaves + */ + float getEffectiveLevels() const; + + /** + * Returns the descriptor of a word + * @param wid word id + * @return descriptor + */ + virtual inline TDescriptor getWord(WordId wid) const; + + /** + * Returns the weight of a word + * @param wid word id + * @return weight + */ + virtual inline WordValue getWordWeight(WordId wid) const; + + /** + * Returns the weighting method + * @return weighting method + */ + inline WeightingType getWeightingType() const { return m_weighting; } + + /** + * Returns the scoring method + * @return scoring method + */ + inline ScoringType getScoringType() const { return m_scoring; } + + /** + * Changes the weighting method + * @param type new weighting type + */ + inline void setWeightingType(WeightingType type); + + /** + * Changes the scoring method + * @param type new scoring type + */ + void setScoringType(ScoringType type); + + /** + * Loads the vocabulary from a text file + * @param filename + */ + bool loadFromTextFile(const std::string &filename); + + /** + * Saves the vocabulary into a text file + * @param filename + */ + void saveToTextFile(const std::string &filename) const; + + /** + * Saves the vocabulary into a file + * @param filename + */ + void save(const std::string &filename) const; + + /** + * Loads the vocabulary from a file + * @param filename + */ + void load(const std::string &filename); + + /** + * Saves the vocabulary to a file storage structure + * @param fn node in file storage + */ + virtual void save(cv::FileStorage &fs, + const std::string &name = "vocabulary") const; + + /** + * Loads the vocabulary from a file storage node + * @param fn first node + * @param subname name of the child node of fn where the tree is stored. + * If not given, the fn node is used instead + */ + virtual void load(const cv::FileStorage &fs, + const std::string &name = "vocabulary"); + + /** + * Stops those words whose weight is below minWeight. + * Words are stopped by setting their weight to 0. There are not returned + * later when transforming image features into vectors. + * Note that when using IDF or TF_IDF, the weight is the idf part, which + * is equivalent to -log(f), where f is the frequency of the word + * (f = Ni/N, Ni: number of training images where the word is present, + * N: number of training images). + * Note that the old weight is forgotten, and subsequent calls to this + * function with a lower minWeight have no effect. + * @return number of words stopped now + */ + virtual int stopWords(double minWeight); + +protected: + + /// Pointer to descriptor + typedef const TDescriptor *pDescriptor; + + /// Tree node + struct Node + { + /// Node id + NodeId id; + /// Weight if the node is a word + WordValue weight; + /// Children + vector children; + /// Parent node (undefined in case of root) + NodeId parent; + /// Node descriptor + TDescriptor descriptor; + + /// Word id if the node is a word + WordId word_id; + + /** + * Empty constructor + */ + Node(): id(0), weight(0), parent(0), word_id(0){} + + /** + * Constructor + * @param _id node id + */ + Node(NodeId _id): id(_id), weight(0), parent(0), word_id(0){} + + /** + * Returns whether the node is a leaf node + * @return true iff the node is a leaf + */ + inline bool isLeaf() const { return children.empty(); } + }; + +protected: + + /** + * Creates an instance of the scoring object accoring to m_scoring + */ + void createScoringObject(); + + /** + * Returns a set of pointers to descriptores + * @param training_features all the features + * @param features (out) pointers to the training features + */ + void getFeatures( + const vector > &training_features, + vector &features) const; + + /** + * Returns the word id associated to a feature + * @param feature + * @param id (out) word id + * @param weight (out) word weight + * @param nid (out) if given, id of the node "levelsup" levels up + * @param levelsup + */ + virtual void transform(const TDescriptor &feature, + WordId &id, WordValue &weight, NodeId* nid = NULL, int levelsup = 0) const; + + /** + * Returns the word id associated to a feature + * @param feature + * @param id (out) word id + */ + virtual void transform(const TDescriptor &feature, WordId &id) const; + + /** + * Creates a level in the tree, under the parent, by running kmeans with + * a descriptor set, and recursively creates the subsequent levels too + * @param parent_id id of parent node + * @param descriptors descriptors to run the kmeans on + * @param current_level current level in the tree + */ + void HKmeansStep(NodeId parent_id, const vector &descriptors, + int current_level); + + /** + * Creates k clusters from the given descriptors with some seeding algorithm. + * @note In this class, kmeans++ is used, but this function should be + * overriden by inherited classes. + */ + virtual void initiateClusters(const vector &descriptors, + vector &clusters) const; + + /** + * Creates k clusters from the given descriptor sets by running the + * initial step of kmeans++ + * @param descriptors + * @param clusters resulting clusters + */ + void initiateClustersKMpp(const vector &descriptors, + vector &clusters) const; + + /** + * Create the words of the vocabulary once the tree has been built + */ + void createWords(); + + /** + * Sets the weights of the nodes of tree according to the given features. + * Before calling this function, the nodes and the words must be already + * created (by calling HKmeansStep and createWords) + * @param features + */ + void setNodeWeights(const vector > &features); + +protected: + + /// Branching factor + int m_k; + + /// Depth levels + int m_L; + + /// Weighting method + WeightingType m_weighting; + + /// Scoring method + ScoringType m_scoring; + + /// Object for computing scores + GeneralScoring* m_scoring_object; + + /// Tree nodes + std::vector m_nodes; + + /// Words of the vocabulary (tree leaves) + /// this condition holds: m_words[wid]->word_id == wid + std::vector m_words; + +}; + +// -------------------------------------------------------------------------- + +template +TemplatedVocabulary::TemplatedVocabulary + (int k, int L, WeightingType weighting, ScoringType scoring) + : m_k(k), m_L(L), m_weighting(weighting), m_scoring(scoring), + m_scoring_object(NULL) +{ + createScoringObject(); +} + +// -------------------------------------------------------------------------- + +template +TemplatedVocabulary::TemplatedVocabulary + (const std::string &filename): m_scoring_object(NULL) +{ + load(filename); +} + +// -------------------------------------------------------------------------- + +template +TemplatedVocabulary::TemplatedVocabulary + (const char *filename): m_scoring_object(NULL) +{ + load(filename); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::createScoringObject() +{ + delete m_scoring_object; + m_scoring_object = NULL; + + switch(m_scoring) + { + case L1_NORM: + m_scoring_object = new L1Scoring; + break; + + case L2_NORM: + m_scoring_object = new L2Scoring; + break; + + case CHI_SQUARE: + m_scoring_object = new ChiSquareScoring; + break; + + case KL: + m_scoring_object = new KLScoring; + break; + + case BHATTACHARYYA: + m_scoring_object = new BhattacharyyaScoring; + break; + + case DOT_PRODUCT: + m_scoring_object = new DotProductScoring; + break; + + } +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::setScoringType(ScoringType type) +{ + m_scoring = type; + createScoringObject(); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::setWeightingType(WeightingType type) +{ + this->m_weighting = type; +} + +// -------------------------------------------------------------------------- + +template +TemplatedVocabulary::TemplatedVocabulary( + const TemplatedVocabulary &voc) + : m_scoring_object(NULL) +{ + *this = voc; +} + +// -------------------------------------------------------------------------- + +template +TemplatedVocabulary::~TemplatedVocabulary() +{ + delete m_scoring_object; +} + +// -------------------------------------------------------------------------- + +template +TemplatedVocabulary& +TemplatedVocabulary::operator= + (const TemplatedVocabulary &voc) +{ + this->m_k = voc.m_k; + this->m_L = voc.m_L; + this->m_scoring = voc.m_scoring; + this->m_weighting = voc.m_weighting; + + this->createScoringObject(); + + this->m_nodes.clear(); + this->m_words.clear(); + + this->m_nodes = voc.m_nodes; + this->createWords(); + + return *this; +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::create( + const std::vector > &training_features) +{ + m_nodes.clear(); + m_words.clear(); + + // expected_nodes = Sum_{i=0..L} ( k^i ) + int expected_nodes = + (int)((pow((double)m_k, (double)m_L + 1) - 1)/(m_k - 1)); + + m_nodes.reserve(expected_nodes); // avoid allocations when creating the tree + + + vector features; + getFeatures(training_features, features); + + + // create root + m_nodes.push_back(Node(0)); // root + + // create the tree + HKmeansStep(0, features, 1); + + // create the words + createWords(); + + // and set the weight of each node of the tree + setNodeWeights(training_features); + +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::create( + const std::vector > &training_features, + int k, int L) +{ + m_k = k; + m_L = L; + + create(training_features); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::create( + const std::vector > &training_features, + int k, int L, WeightingType weighting, ScoringType scoring) +{ + m_k = k; + m_L = L; + m_weighting = weighting; + m_scoring = scoring; + createScoringObject(); + + create(training_features); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::getFeatures( + const vector > &training_features, + vector &features) const +{ + features.resize(0); + + typename vector >::const_iterator vvit; + typename vector::const_iterator vit; + for(vvit = training_features.begin(); vvit != training_features.end(); ++vvit) + { + features.reserve(features.size() + vvit->size()); + for(vit = vvit->begin(); vit != vvit->end(); ++vit) + { + features.push_back(&(*vit)); + } + } +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::HKmeansStep(NodeId parent_id, + const vector &descriptors, int current_level) +{ + if(descriptors.empty()) return; + + // features associated to each cluster + vector clusters; + vector > groups; // groups[i] = [j1, j2, ...] + // j1, j2, ... indices of descriptors associated to cluster i + + clusters.reserve(m_k); + groups.reserve(m_k); + + //const int msizes[] = { m_k, descriptors.size() }; + //cv::SparseMat assoc(2, msizes, CV_8U); + //cv::SparseMat last_assoc(2, msizes, CV_8U); + //// assoc.row(cluster_idx).col(descriptor_idx) = 1 iif associated + + if((int)descriptors.size() <= m_k) + { + // trivial case: one cluster per feature + groups.resize(descriptors.size()); + + for(unsigned int i = 0; i < descriptors.size(); i++) + { + groups[i].push_back(i); + clusters.push_back(*descriptors[i]); + } + } + else + { + // select clusters and groups with kmeans + + bool first_time = true; + bool goon = true; + + // to check if clusters move after iterations + vector last_association, current_association; + + while(goon) + { + // 1. Calculate clusters + + if(first_time) + { + // random sample + initiateClusters(descriptors, clusters); + } + else + { + // calculate cluster centres + + for(unsigned int c = 0; c < clusters.size(); ++c) + { + vector cluster_descriptors; + cluster_descriptors.reserve(groups[c].size()); + + /* + for(unsigned int d = 0; d < descriptors.size(); ++d) + { + if( assoc.find(c, d) ) + { + cluster_descriptors.push_back(descriptors[d]); + } + } + */ + + vector::const_iterator vit; + for(vit = groups[c].begin(); vit != groups[c].end(); ++vit) + { + cluster_descriptors.push_back(descriptors[*vit]); + } + + + F::meanValue(cluster_descriptors, clusters[c]); + } + + } // if(!first_time) + + // 2. Associate features with clusters + + // calculate distances to cluster centers + groups.clear(); + groups.resize(clusters.size(), vector()); + current_association.resize(descriptors.size()); + + //assoc.clear(); + + typename vector::const_iterator fit; + //unsigned int d = 0; + for(fit = descriptors.begin(); fit != descriptors.end(); ++fit)//, ++d) + { + double best_dist = F::distance(*(*fit), clusters[0]); + unsigned int icluster = 0; + + for(unsigned int c = 1; c < clusters.size(); ++c) + { + double dist = F::distance(*(*fit), clusters[c]); + if(dist < best_dist) + { + best_dist = dist; + icluster = c; + } + } + + //assoc.ref(icluster, d) = 1; + + groups[icluster].push_back(fit - descriptors.begin()); + current_association[ fit - descriptors.begin() ] = icluster; + } + + // kmeans++ ensures all the clusters has any feature associated with them + + // 3. check convergence + if(first_time) + { + first_time = false; + } + else + { + //goon = !eqUChar(last_assoc, assoc); + + goon = false; + for(unsigned int i = 0; i < current_association.size(); i++) + { + if(current_association[i] != last_association[i]){ + goon = true; + break; + } + } + } + + if(goon) + { + // copy last feature-cluster association + last_association = current_association; + //last_assoc = assoc.clone(); + } + + } // while(goon) + + } // if must run kmeans + + // create nodes + for(unsigned int i = 0; i < clusters.size(); ++i) + { + NodeId id = m_nodes.size(); + m_nodes.push_back(Node(id)); + m_nodes.back().descriptor = clusters[i]; + m_nodes.back().parent = parent_id; + m_nodes[parent_id].children.push_back(id); + } + + // go on with the next level + if(current_level < m_L) + { + // iterate again with the resulting clusters + const vector &children_ids = m_nodes[parent_id].children; + for(unsigned int i = 0; i < clusters.size(); ++i) + { + NodeId id = children_ids[i]; + + vector child_features; + child_features.reserve(groups[i].size()); + + vector::const_iterator vit; + for(vit = groups[i].begin(); vit != groups[i].end(); ++vit) + { + child_features.push_back(descriptors[*vit]); + } + + if(child_features.size() > 1) + { + HKmeansStep(id, child_features, current_level + 1); + } + } + } +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::initiateClusters + (const vector &descriptors, vector &clusters) const +{ + initiateClustersKMpp(descriptors, clusters); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::initiateClustersKMpp( + const vector &pfeatures, vector &clusters) const +{ + // Implements kmeans++ seeding algorithm + // Algorithm: + // 1. Choose one center uniformly at random from among the data points. + // 2. For each data point x, compute D(x), the distance between x and the nearest + // center that has already been chosen. + // 3. Add one new data point as a center. Each point x is chosen with probability + // proportional to D(x)^2. + // 4. Repeat Steps 2 and 3 until k centers have been chosen. + // 5. Now that the initial centers have been chosen, proceed using standard k-means + // clustering. + + DUtils::Random::SeedRandOnce(); + + clusters.resize(0); + clusters.reserve(m_k); + vector min_dists(pfeatures.size(), std::numeric_limits::max()); + + // 1. + + int ifeature = DUtils::Random::RandomInt(0, pfeatures.size()-1); + + // create first cluster + clusters.push_back(*pfeatures[ifeature]); + + // compute the initial distances + typename vector::const_iterator fit; + vector::iterator dit; + dit = min_dists.begin(); + for(fit = pfeatures.begin(); fit != pfeatures.end(); ++fit, ++dit) + { + *dit = F::distance(*(*fit), clusters.back()); + } + + while((int)clusters.size() < m_k) + { + // 2. + dit = min_dists.begin(); + for(fit = pfeatures.begin(); fit != pfeatures.end(); ++fit, ++dit) + { + if(*dit > 0) + { + double dist = F::distance(*(*fit), clusters.back()); + if(dist < *dit) *dit = dist; + } + } + + // 3. + double dist_sum = std::accumulate(min_dists.begin(), min_dists.end(), 0.0); + + if(dist_sum > 0) + { + double cut_d; + do + { + cut_d = DUtils::Random::RandomValue(0, dist_sum); + } while(cut_d == 0.0); + + double d_up_now = 0; + for(dit = min_dists.begin(); dit != min_dists.end(); ++dit) + { + d_up_now += *dit; + if(d_up_now >= cut_d) break; + } + + if(dit == min_dists.end()) + ifeature = pfeatures.size()-1; + else + ifeature = dit - min_dists.begin(); + + clusters.push_back(*pfeatures[ifeature]); + + } // if dist_sum > 0 + else + break; + + } // while(used_clusters < m_k) + +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::createWords() +{ + m_words.resize(0); + + if(!m_nodes.empty()) + { + m_words.reserve( (int)pow((double)m_k, (double)m_L) ); + + typename vector::iterator nit; + + nit = m_nodes.begin(); // ignore root + for(++nit; nit != m_nodes.end(); ++nit) + { + if(nit->isLeaf()) + { + nit->word_id = m_words.size(); + m_words.push_back( &(*nit) ); + } + } + } +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::setNodeWeights + (const vector > &training_features) +{ + const unsigned int NWords = m_words.size(); + const unsigned int NDocs = training_features.size(); + + if(m_weighting == TF || m_weighting == BINARY) + { + // idf part must be 1 always + for(unsigned int i = 0; i < NWords; i++) + m_words[i]->weight = 1; + } + else if(m_weighting == IDF || m_weighting == TF_IDF) + { + // IDF and TF-IDF: we calculte the idf path now + + // Note: this actually calculates the idf part of the tf-idf score. + // The complete tf-idf score is calculated in ::transform + + vector Ni(NWords, 0); + vector counted(NWords, false); + + typename vector >::const_iterator mit; + typename vector::const_iterator fit; + + for(mit = training_features.begin(); mit != training_features.end(); ++mit) + { + fill(counted.begin(), counted.end(), false); + + for(fit = mit->begin(); fit < mit->end(); ++fit) + { + WordId word_id; + transform(*fit, word_id); + + if(!counted[word_id]) + { + Ni[word_id]++; + counted[word_id] = true; + } + } + } + + // set ln(N/Ni) + for(unsigned int i = 0; i < NWords; i++) + { + if(Ni[i] > 0) + { + m_words[i]->weight = log((double)NDocs / (double)Ni[i]); + }// else // This cannot occur if using kmeans++ + } + + } + +} + +// -------------------------------------------------------------------------- + +template +inline unsigned int TemplatedVocabulary::size() const +{ + return m_words.size(); +} + +// -------------------------------------------------------------------------- + +template +inline bool TemplatedVocabulary::empty() const +{ + return m_words.empty(); +} + +// -------------------------------------------------------------------------- + +template +float TemplatedVocabulary::getEffectiveLevels() const +{ + long sum = 0; + typename std::vector::const_iterator wit; + for(wit = m_words.begin(); wit != m_words.end(); ++wit) + { + const Node *p = *wit; + + for(; p->id != 0; sum++) p = &m_nodes[p->parent]; + } + + return (float)((double)sum / (double)m_words.size()); +} + +// -------------------------------------------------------------------------- + +template +TDescriptor TemplatedVocabulary::getWord(WordId wid) const +{ + return m_words[wid]->descriptor; +} + +// -------------------------------------------------------------------------- + +template +WordValue TemplatedVocabulary::getWordWeight(WordId wid) const +{ + return m_words[wid]->weight; +} + +// -------------------------------------------------------------------------- + +template +WordId TemplatedVocabulary::transform + (const TDescriptor& feature) const +{ + if(empty()) + { + return 0; + } + + WordId wid; + transform(feature, wid); + return wid; +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::transform( + const std::vector& features, BowVector &v) const +{ + v.clear(); + + if(empty()) + { + return; + } + + // normalize + LNorm norm; + bool must = m_scoring_object->mustNormalize(norm); + + typename vector::const_iterator fit; + + if(m_weighting == TF || m_weighting == TF_IDF) + { + for(fit = features.begin(); fit < features.end(); ++fit) + { + WordId id; + WordValue w; + // w is the idf value if TF_IDF, 1 if TF + + transform(*fit, id, w); + + // not stopped + if(w > 0) v.addWeight(id, w); + } + + if(!v.empty() && !must) + { + // unnecessary when normalizing + const double nd = v.size(); + for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++) + vit->second /= nd; + } + + } + else // IDF || BINARY + { + for(fit = features.begin(); fit < features.end(); ++fit) + { + WordId id; + WordValue w; + // w is idf if IDF, or 1 if BINARY + + transform(*fit, id, w); + + // not stopped + if(w > 0) v.addIfNotExist(id, w); + + } // if add_features + } // if m_weighting == ... + + if(must) v.normalize(norm); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::transform( + const std::vector& features, + BowVector &v, FeatureVector &fv, int levelsup) const +{ + v.clear(); + fv.clear(); + + if(empty()) // safe for subclasses + { + return; + } + + // normalize + LNorm norm; + bool must = m_scoring_object->mustNormalize(norm); + + typename vector::const_iterator fit; + + if(m_weighting == TF || m_weighting == TF_IDF) + { + unsigned int i_feature = 0; + for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature) + { + WordId id; + NodeId nid; + WordValue w; + // w is the idf value if TF_IDF, 1 if TF + + transform(*fit, id, w, &nid, levelsup); + + if(w > 0) // not stopped + { + v.addWeight(id, w); + fv.addFeature(nid, i_feature); + } + } + + if(!v.empty() && !must) + { + // unnecessary when normalizing + const double nd = v.size(); + for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++) + vit->second /= nd; + } + + } + else // IDF || BINARY + { + unsigned int i_feature = 0; + for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature) + { + WordId id; + NodeId nid; + WordValue w; + // w is idf if IDF, or 1 if BINARY + + transform(*fit, id, w, &nid, levelsup); + + if(w > 0) // not stopped + { + v.addIfNotExist(id, w); + fv.addFeature(nid, i_feature); + } + } + } // if m_weighting == ... + + if(must) v.normalize(norm); +} + +// -------------------------------------------------------------------------- + +template +inline double TemplatedVocabulary::score + (const BowVector &v1, const BowVector &v2) const +{ + return m_scoring_object->score(v1, v2); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::transform + (const TDescriptor &feature, WordId &id) const +{ + WordValue weight; + transform(feature, id, weight); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::transform(const TDescriptor &feature, + WordId &word_id, WordValue &weight, NodeId *nid, int levelsup) const +{ + // propagate the feature down the tree + vector nodes; + typename vector::const_iterator nit; + + // level at which the node must be stored in nid, if given + const int nid_level = m_L - levelsup; + if(nid_level <= 0 && nid != NULL) *nid = 0; // root + + NodeId final_id = 0; // root + int current_level = 0; + + do + { + ++current_level; + nodes = m_nodes[final_id].children; + final_id = nodes[0]; + + double best_d = F::distance(feature, m_nodes[final_id].descriptor); + + for(nit = nodes.begin() + 1; nit != nodes.end(); ++nit) + { + NodeId id = *nit; + double d = F::distance(feature, m_nodes[id].descriptor); + if(d < best_d) + { + best_d = d; + final_id = id; + } + } + + if(nid != NULL && current_level == nid_level) + *nid = final_id; + + } while( !m_nodes[final_id].isLeaf() ); + + // turn node id into word id + word_id = m_nodes[final_id].word_id; + weight = m_nodes[final_id].weight; +} + +// -------------------------------------------------------------------------- + +template +NodeId TemplatedVocabulary::getParentNode + (WordId wid, int levelsup) const +{ + NodeId ret = m_words[wid]->id; // node id + while(levelsup > 0 && ret != 0) // ret == 0 --> root + { + --levelsup; + ret = m_nodes[ret].parent; + } + return ret; +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::getWordsFromNode + (NodeId nid, std::vector &words) const +{ + words.clear(); + + if(m_nodes[nid].isLeaf()) + { + words.push_back(m_nodes[nid].word_id); + } + else + { + words.reserve(m_k); // ^1, ^2, ... + + vector parents; + parents.push_back(nid); + + while(!parents.empty()) + { + NodeId parentid = parents.back(); + parents.pop_back(); + + const vector &child_ids = m_nodes[parentid].children; + vector::const_iterator cit; + + for(cit = child_ids.begin(); cit != child_ids.end(); ++cit) + { + const Node &child_node = m_nodes[*cit]; + + if(child_node.isLeaf()) + words.push_back(child_node.word_id); + else + parents.push_back(*cit); + + } // for each child + } // while !parents.empty + } +} + +// -------------------------------------------------------------------------- + +template +int TemplatedVocabulary::stopWords(double minWeight) +{ + int c = 0; + typename vector::iterator wit; + for(wit = m_words.begin(); wit != m_words.end(); ++wit) + { + if((*wit)->weight < minWeight) + { + ++c; + (*wit)->weight = 0; + } + } + return c; +} + +// -------------------------------------------------------------------------- + +template +bool TemplatedVocabulary::loadFromTextFile(const std::string &filename) +{ + ifstream f; + f.open(filename.c_str()); + + if(f.eof()) + return false; + + m_words.clear(); + m_nodes.clear(); + + string s; + getline(f,s); + stringstream ss; + ss << s; + ss >> m_k; + ss >> m_L; + int n1, n2; + ss >> n1; + ss >> n2; + + if(m_k<0 || m_k>20 || m_L<1 || m_L>10 || n1<0 || n1>5 || n2<0 || n2>3) + { + std::cerr << "Vocabulary loading failure: This is not a correct text file!" << endl; + return false; + } + + m_scoring = (ScoringType)n1; + m_weighting = (WeightingType)n2; + createScoringObject(); + + // nodes + int expected_nodes = + (int)((pow((double)m_k, (double)m_L + 1) - 1)/(m_k - 1)); + m_nodes.reserve(expected_nodes); + + m_words.reserve(pow((double)m_k, (double)m_L + 1)); + + m_nodes.resize(1); + m_nodes[0].id = 0; + while(!f.eof()) + { + string snode; + getline(f,snode); + stringstream ssnode; + ssnode << snode; + + int nid = m_nodes.size(); + m_nodes.resize(m_nodes.size()+1); + m_nodes[nid].id = nid; + + int pid ; + ssnode >> pid; + m_nodes[nid].parent = pid; + m_nodes[pid].children.push_back(nid); + + int nIsLeaf; + ssnode >> nIsLeaf; + + stringstream ssd; + for(int iD=0;iD> sElement; + ssd << sElement << " "; + } + F::fromString(m_nodes[nid].descriptor, ssd.str()); + + ssnode >> m_nodes[nid].weight; + + if(nIsLeaf>0) + { + int wid = m_words.size(); + m_words.resize(wid+1); + + m_nodes[nid].word_id = wid; + m_words[wid] = &m_nodes[nid]; + } + else + { + m_nodes[nid].children.reserve(m_k); + } + } + + return true; + +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::saveToTextFile(const std::string &filename) const +{ + fstream f; + f.open(filename.c_str(),ios_base::out); + f << m_k << " " << m_L << " " << " " << m_scoring << " " << m_weighting << endl; + + for(size_t i=1; i +void TemplatedVocabulary::save(const std::string &filename) const +{ + cv::FileStorage fs(filename.c_str(), cv::FileStorage::WRITE); + if(!fs.isOpened()) throw string("Could not open file ") + filename; + + save(fs); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::load(const std::string &filename) +{ + cv::FileStorage fs(filename.c_str(), cv::FileStorage::READ); + if(!fs.isOpened()) throw string("Could not open file ") + filename; + + this->load(fs); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::save(cv::FileStorage &f, + const std::string &name) const +{ + // Format YAML: + // vocabulary + // { + // k: + // L: + // scoringType: + // weightingType: + // nodes + // [ + // { + // nodeId: + // parentId: + // weight: + // descriptor: + // } + // ] + // words + // [ + // { + // wordId: + // nodeId: + // } + // ] + // } + // + // The root node (index 0) is not included in the node vector + // + + f << name << "{"; + + f << "k" << m_k; + f << "L" << m_L; + f << "scoringType" << m_scoring; + f << "weightingType" << m_weighting; + + // tree + f << "nodes" << "["; + vector parents, children; + vector::const_iterator pit; + + parents.push_back(0); // root + + while(!parents.empty()) + { + NodeId pid = parents.back(); + parents.pop_back(); + + const Node& parent = m_nodes[pid]; + children = parent.children; + + for(pit = children.begin(); pit != children.end(); pit++) + { + const Node& child = m_nodes[*pit]; + + // save node data + f << "{:"; + f << "nodeId" << (int)child.id; + f << "parentId" << (int)pid; + f << "weight" << (double)child.weight; + f << "descriptor" << F::toString(child.descriptor); + f << "}"; + + // add to parent list + if(!child.isLeaf()) + { + parents.push_back(*pit); + } + } + } + + f << "]"; // nodes + + // words + f << "words" << "["; + + typename vector::const_iterator wit; + for(wit = m_words.begin(); wit != m_words.end(); wit++) + { + WordId id = wit - m_words.begin(); + f << "{:"; + f << "wordId" << (int)id; + f << "nodeId" << (int)(*wit)->id; + f << "}"; + } + + f << "]"; // words + + f << "}"; + +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::load(const cv::FileStorage &fs, + const std::string &name) +{ + m_words.clear(); + m_nodes.clear(); + + cv::FileNode fvoc = fs[name]; + + m_k = (int)fvoc["k"]; + m_L = (int)fvoc["L"]; + m_scoring = (ScoringType)((int)fvoc["scoringType"]); + m_weighting = (WeightingType)((int)fvoc["weightingType"]); + + createScoringObject(); + + // nodes + cv::FileNode fn = fvoc["nodes"]; + + m_nodes.resize(fn.size() + 1); // +1 to include root + m_nodes[0].id = 0; + + for(unsigned int i = 0; i < fn.size(); ++i) + { + NodeId nid = (int)fn[i]["nodeId"]; + NodeId pid = (int)fn[i]["parentId"]; + WordValue weight = (WordValue)fn[i]["weight"]; + string d = (string)fn[i]["descriptor"]; + + m_nodes[nid].id = nid; + m_nodes[nid].parent = pid; + m_nodes[nid].weight = weight; + m_nodes[pid].children.push_back(nid); + + F::fromString(m_nodes[nid].descriptor, d); + } + + // words + fn = fvoc["words"]; + + m_words.resize(fn.size()); + + for(unsigned int i = 0; i < fn.size(); ++i) + { + NodeId wid = (int)fn[i]["wordId"]; + NodeId nid = (int)fn[i]["nodeId"]; + + m_nodes[nid].word_id = wid; + m_words[wid] = &m_nodes[nid]; + } +} + +// -------------------------------------------------------------------------- + +/** + * Writes printable information of the vocabulary + * @param os stream to write to + * @param voc + */ +template +std::ostream& operator<<(std::ostream &os, + const TemplatedVocabulary &voc) +{ + os << "Vocabulary: k = " << voc.getBranchingFactor() + << ", L = " << voc.getDepthLevels() + << ", Weighting = "; + + switch(voc.getWeightingType()) + { + case TF_IDF: os << "tf-idf"; break; + case TF: os << "tf"; break; + case IDF: os << "idf"; break; + case BINARY: os << "binary"; break; + } + + os << ", Scoring = "; + switch(voc.getScoringType()) + { + case L1_NORM: os << "L1-norm"; break; + case L2_NORM: os << "L2-norm"; break; + case CHI_SQUARE: os << "Chi square distance"; break; + case KL: os << "KL-divergence"; break; + case BHATTACHARYYA: os << "Bhattacharyya coefficient"; break; + case DOT_PRODUCT: os << "Dot product"; break; + } + + os << ", Number of words = " << voc.size(); + + return os; +} + +} // namespace DBoW2 + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DUtils/Random.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DUtils/Random.cpp new file mode 100644 index 0000000..79df2a6 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DUtils/Random.cpp @@ -0,0 +1,129 @@ +/* + * File: Random.cpp + * Project: DUtils library + * Author: Dorian Galvez-Lopez + * Date: April 2010 + * Description: manages pseudo-random numbers + * License: see the LICENSE.txt file + * + */ + +#include "Random.h" +#include "Timestamp.h" +#include +using namespace std; + +bool DUtils::Random::m_already_seeded = false; + +void DUtils::Random::SeedRand(){ + Timestamp time; + time.setToCurrentTime(); + srand((unsigned)time.getFloatTime()); +} + +void DUtils::Random::SeedRandOnce() +{ + if(!m_already_seeded) + { + DUtils::Random::SeedRand(); + m_already_seeded = true; + } +} + +void DUtils::Random::SeedRand(int seed) +{ + srand(seed); +} + +void DUtils::Random::SeedRandOnce(int seed) +{ + if(!m_already_seeded) + { + DUtils::Random::SeedRand(seed); + m_already_seeded = true; + } +} + +int DUtils::Random::RandomInt(int min, int max){ + int d = max - min + 1; + return int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min; +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max) +{ + if(min <= max) + { + m_min = min; + m_max = max; + } + else + { + m_min = max; + m_max = min; + } + + createValues(); +} + +// --------------------------------------------------------------------------- + +DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer + (const DUtils::Random::UnrepeatedRandomizer& rnd) +{ + *this = rnd; +} + +// --------------------------------------------------------------------------- + +int DUtils::Random::UnrepeatedRandomizer::get() +{ + if(empty()) createValues(); + + DUtils::Random::SeedRandOnce(); + + int k = DUtils::Random::RandomInt(0, m_values.size()-1); + int ret = m_values[k]; + m_values[k] = m_values.back(); + m_values.pop_back(); + + return ret; +} + +// --------------------------------------------------------------------------- + +void DUtils::Random::UnrepeatedRandomizer::createValues() +{ + int n = m_max - m_min + 1; + + m_values.resize(n); + for(int i = 0; i < n; ++i) m_values[i] = m_min + i; +} + +// --------------------------------------------------------------------------- + +void DUtils::Random::UnrepeatedRandomizer::reset() +{ + if((int)m_values.size() != m_max - m_min + 1) createValues(); +} + +// --------------------------------------------------------------------------- + +DUtils::Random::UnrepeatedRandomizer& +DUtils::Random::UnrepeatedRandomizer::operator= + (const DUtils::Random::UnrepeatedRandomizer& rnd) +{ + if(this != &rnd) + { + this->m_min = rnd.m_min; + this->m_max = rnd.m_max; + this->m_values = rnd.m_values; + } + return *this; +} + +// --------------------------------------------------------------------------- + + diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DUtils/Random.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DUtils/Random.h new file mode 100644 index 0000000..5115dc4 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DUtils/Random.h @@ -0,0 +1,184 @@ +/* + * File: Random.h + * Project: DUtils library + * Author: Dorian Galvez-Lopez + * Date: April 2010, November 2011 + * Description: manages pseudo-random numbers + * License: see the LICENSE.txt file + * + */ + +#pragma once +#ifndef __D_RANDOM__ +#define __D_RANDOM__ + +#include +#include + +namespace DUtils { + +/// Functions to generate pseudo-random numbers +class Random +{ +public: + class UnrepeatedRandomizer; + +public: + /** + * Sets the random number seed to the current time + */ + static void SeedRand(); + + /** + * Sets the random number seed to the current time only the first + * time this function is called + */ + static void SeedRandOnce(); + + /** + * Sets the given random number seed + * @param seed + */ + static void SeedRand(int seed); + + /** + * Sets the given random number seed only the first time this function + * is called + * @param seed + */ + static void SeedRandOnce(int seed); + + /** + * Returns a random number in the range [0..1] + * @return random T number in [0..1] + */ + template + static T RandomValue(){ + return (T)rand()/(T)RAND_MAX; + } + + /** + * Returns a random number in the range [min..max] + * @param min + * @param max + * @return random T number in [min..max] + */ + template + static T RandomValue(T min, T max){ + return Random::RandomValue() * (max - min) + min; + } + + /** + * Returns a random int in the range [min..max] + * @param min + * @param max + * @return random int in [min..max] + */ + static int RandomInt(int min, int max); + + /** + * Returns a random number from a gaussian distribution + * @param mean + * @param sigma standard deviation + */ + template + static T RandomGaussianValue(T mean, T sigma) + { + // Box-Muller transformation + T x1, x2, w, y1; + + do { + x1 = (T)2. * RandomValue() - (T)1.; + x2 = (T)2. * RandomValue() - (T)1.; + w = x1 * x1 + x2 * x2; + } while ( w >= (T)1. || w == (T)0. ); + + w = sqrt( ((T)-2.0 * log( w ) ) / w ); + y1 = x1 * w; + + return( mean + y1 * sigma ); + } + +private: + + /// If SeedRandOnce() or SeedRandOnce(int) have already been called + static bool m_already_seeded; + +}; + +// --------------------------------------------------------------------------- + +/// Provides pseudo-random numbers with no repetitions +class Random::UnrepeatedRandomizer +{ +public: + + /** + * Creates a randomizer that returns numbers in the range [min, max] + * @param min + * @param max + */ + UnrepeatedRandomizer(int min, int max); + ~UnrepeatedRandomizer(){} + + /** + * Copies a randomizer + * @param rnd + */ + UnrepeatedRandomizer(const UnrepeatedRandomizer& rnd); + + /** + * Copies a randomizer + * @param rnd + */ + UnrepeatedRandomizer& operator=(const UnrepeatedRandomizer& rnd); + + /** + * Returns a random number not given before. If all the possible values + * were already given, the process starts again + * @return unrepeated random number + */ + int get(); + + /** + * Returns whether all the possible values between min and max were + * already given. If get() is called when empty() is true, the behaviour + * is the same than after creating the randomizer + * @return true iff all the values were returned + */ + inline bool empty() const { return m_values.empty(); } + + /** + * Returns the number of values still to be returned + * @return amount of values to return + */ + inline unsigned int left() const { return m_values.size(); } + + /** + * Resets the randomizer as it were just created + */ + void reset(); + +protected: + + /** + * Creates the vector with available values + */ + void createValues(); + +protected: + + /// Min of range of values + int m_min; + /// Max of range of values + int m_max; + + /// Available values + std::vector m_values; + +}; + +} + +#endif + diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DUtils/Timestamp.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DUtils/Timestamp.cpp new file mode 100644 index 0000000..4551839 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DUtils/Timestamp.cpp @@ -0,0 +1,246 @@ +/* + * File: Timestamp.cpp + * Author: Dorian Galvez-Lopez + * Date: March 2009 + * Description: timestamping functions + * + * Note: in windows, this class has a 1ms resolution + * + * License: see the LICENSE.txt file + * + */ + +#include +#include +#include +#include +#include +#include + +#ifdef _WIN32 +#ifndef WIN32 +#define WIN32 +#endif +#endif + +#ifdef WIN32 +#include +#define sprintf sprintf_s +#else +#include +#endif + +#include "Timestamp.h" + +using namespace std; + +using namespace DUtils; + +Timestamp::Timestamp(Timestamp::tOptions option) +{ + if(option & CURRENT_TIME) + setToCurrentTime(); + else if(option & ZERO) + setTime(0.); +} + +Timestamp::~Timestamp(void) +{ +} + +bool Timestamp::empty() const +{ + return m_secs == 0 && m_usecs == 0; +} + +void Timestamp::setToCurrentTime(){ + +#ifdef WIN32 + struct __timeb32 timebuffer; + _ftime32_s( &timebuffer ); // C4996 + // Note: _ftime is deprecated; consider using _ftime_s instead + m_secs = timebuffer.time; + m_usecs = timebuffer.millitm * 1000; +#else + struct timeval now; + gettimeofday(&now, NULL); + m_secs = now.tv_sec; + m_usecs = now.tv_usec; +#endif + +} + +void Timestamp::setTime(const string &stime){ + string::size_type p = stime.find('.'); + if(p == string::npos){ + m_secs = atol(stime.c_str()); + m_usecs = 0; + }else{ + m_secs = atol(stime.substr(0, p).c_str()); + + string s_usecs = stime.substr(p+1, 6); + m_usecs = atol(stime.substr(p+1).c_str()); + m_usecs *= (unsigned long)pow(10.0, double(6 - s_usecs.length())); + } +} + +void Timestamp::setTime(double s) +{ + m_secs = (unsigned long)s; + m_usecs = (s - (double)m_secs) * 1e6; +} + +double Timestamp::getFloatTime() const { + return double(m_secs) + double(m_usecs)/1000000.0; +} + +string Timestamp::getStringTime() const { + char buf[32]; + sprintf(buf, "%.6lf", this->getFloatTime()); + return string(buf); +} + +double Timestamp::operator- (const Timestamp &t) const { + return this->getFloatTime() - t.getFloatTime(); +} + +Timestamp& Timestamp::operator+= (double s) +{ + *this = *this + s; + return *this; +} + +Timestamp& Timestamp::operator-= (double s) +{ + *this = *this - s; + return *this; +} + +Timestamp Timestamp::operator+ (double s) const +{ + unsigned long secs = (long)floor(s); + unsigned long usecs = (long)((s - (double)secs) * 1e6); + + return this->plus(secs, usecs); +} + +Timestamp Timestamp::plus(unsigned long secs, unsigned long usecs) const +{ + Timestamp t; + + const unsigned long max = 1000000ul; + + if(m_usecs + usecs >= max) + t.setTime(m_secs + secs + 1, m_usecs + usecs - max); + else + t.setTime(m_secs + secs, m_usecs + usecs); + + return t; +} + +Timestamp Timestamp::operator- (double s) const +{ + unsigned long secs = (long)floor(s); + unsigned long usecs = (long)((s - (double)secs) * 1e6); + + return this->minus(secs, usecs); +} + +Timestamp Timestamp::minus(unsigned long secs, unsigned long usecs) const +{ + Timestamp t; + + const unsigned long max = 1000000ul; + + if(m_usecs < usecs) + t.setTime(m_secs - secs - 1, max - (usecs - m_usecs)); + else + t.setTime(m_secs - secs, m_usecs - usecs); + + return t; +} + +bool Timestamp::operator> (const Timestamp &t) const +{ + if(m_secs > t.m_secs) return true; + else if(m_secs == t.m_secs) return m_usecs > t.m_usecs; + else return false; +} + +bool Timestamp::operator>= (const Timestamp &t) const +{ + if(m_secs > t.m_secs) return true; + else if(m_secs == t.m_secs) return m_usecs >= t.m_usecs; + else return false; +} + +bool Timestamp::operator< (const Timestamp &t) const +{ + if(m_secs < t.m_secs) return true; + else if(m_secs == t.m_secs) return m_usecs < t.m_usecs; + else return false; +} + +bool Timestamp::operator<= (const Timestamp &t) const +{ + if(m_secs < t.m_secs) return true; + else if(m_secs == t.m_secs) return m_usecs <= t.m_usecs; + else return false; +} + +bool Timestamp::operator== (const Timestamp &t) const +{ + return(m_secs == t.m_secs && m_usecs == t.m_usecs); +} + + +string Timestamp::Format(bool machine_friendly) const +{ + struct tm tm_time; + + time_t t = (time_t)getFloatTime(); + +#ifdef WIN32 + localtime_s(&tm_time, &t); +#else + localtime_r(&t, &tm_time); +#endif + + char buffer[128]; + + if(machine_friendly) + { + strftime(buffer, 128, "%Y%m%d_%H%M%S", &tm_time); + } + else + { + strftime(buffer, 128, "%c", &tm_time); // Thu Aug 23 14:55:02 2001 + } + + return string(buffer); +} + +string Timestamp::Format(double s) { + int days = int(s / (24. * 3600.0)); + s -= days * (24. * 3600.0); + int hours = int(s / 3600.0); + s -= hours * 3600; + int minutes = int(s / 60.0); + s -= minutes * 60; + int seconds = int(s); + int ms = int((s - seconds)*1e6); + + stringstream ss; + ss.fill('0'); + bool b; + if((b = (days > 0))) ss << days << "d "; + if((b = (b || hours > 0))) ss << setw(2) << hours << ":"; + if((b = (b || minutes > 0))) ss << setw(2) << minutes << ":"; + if(b) ss << setw(2); + ss << seconds; + if(!b) ss << "." << setw(6) << ms; + + return ss.str(); +} + + diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DUtils/Timestamp.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DUtils/Timestamp.h new file mode 100644 index 0000000..b92f89f --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/DUtils/Timestamp.h @@ -0,0 +1,204 @@ +/* + * File: Timestamp.h + * Author: Dorian Galvez-Lopez + * Date: March 2009 + * Description: timestamping functions + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_TIMESTAMP__ +#define __D_TIMESTAMP__ + +#include +using namespace std; + +namespace DUtils { + +/// Timestamp +class Timestamp +{ +public: + + /// Options to initiate a timestamp + enum tOptions + { + NONE = 0, + CURRENT_TIME = 0x1, + ZERO = 0x2 + }; + +public: + + /** + * Creates a timestamp + * @param option option to set the initial time stamp + */ + Timestamp(Timestamp::tOptions option = NONE); + + /** + * Destructor + */ + virtual ~Timestamp(void); + + /** + * Says if the timestamp is "empty": seconds and usecs are both 0, as + * when initiated with the ZERO flag + * @return true iif secs == usecs == 0 + */ + bool empty() const; + + /** + * Sets this instance to the current time + */ + void setToCurrentTime(); + + /** + * Sets the timestamp from seconds and microseconds + * @param secs: seconds + * @param usecs: microseconds + */ + inline void setTime(unsigned long secs, unsigned long usecs){ + m_secs = secs; + m_usecs = usecs; + } + + /** + * Returns the timestamp in seconds and microseconds + * @param secs seconds + * @param usecs microseconds + */ + inline void getTime(unsigned long &secs, unsigned long &usecs) const + { + secs = m_secs; + usecs = m_usecs; + } + + /** + * Sets the timestamp from a string with the time in seconds + * @param stime: string such as "1235603336.036609" + */ + void setTime(const string &stime); + + /** + * Sets the timestamp from a number of seconds from the epoch + * @param s seconds from the epoch + */ + void setTime(double s); + + /** + * Returns this timestamp as the number of seconds in (long) float format + */ + double getFloatTime() const; + + /** + * Returns this timestamp as the number of seconds in fixed length string format + */ + string getStringTime() const; + + /** + * Returns the difference in seconds between this timestamp (greater) and t (smaller) + * If the order is swapped, a negative number is returned + * @param t: timestamp to subtract from this timestamp + * @return difference in seconds + */ + double operator- (const Timestamp &t) const; + + /** + * Returns a copy of this timestamp + s seconds + us microseconds + * @param s seconds + * @param us microseconds + */ + Timestamp plus(unsigned long s, unsigned long us) const; + + /** + * Returns a copy of this timestamp - s seconds - us microseconds + * @param s seconds + * @param us microseconds + */ + Timestamp minus(unsigned long s, unsigned long us) const; + + /** + * Adds s seconds to this timestamp and returns a reference to itself + * @param s seconds + * @return reference to this timestamp + */ + Timestamp& operator+= (double s); + + /** + * Substracts s seconds to this timestamp and returns a reference to itself + * @param s seconds + * @return reference to this timestamp + */ + Timestamp& operator-= (double s); + + /** + * Returns a copy of this timestamp + s seconds + * @param s: seconds + */ + Timestamp operator+ (double s) const; + + /** + * Returns a copy of this timestamp - s seconds + * @param s: seconds + */ + Timestamp operator- (double s) const; + + /** + * Returns whether this timestamp is at the future of t + * @param t + */ + bool operator> (const Timestamp &t) const; + + /** + * Returns whether this timestamp is at the future of (or is the same as) t + * @param t + */ + bool operator>= (const Timestamp &t) const; + + /** + * Returns whether this timestamp and t represent the same instant + * @param t + */ + bool operator== (const Timestamp &t) const; + + /** + * Returns whether this timestamp is at the past of t + * @param t + */ + bool operator< (const Timestamp &t) const; + + /** + * Returns whether this timestamp is at the past of (or is the same as) t + * @param t + */ + bool operator<= (const Timestamp &t) const; + + /** + * Returns the timestamp in a human-readable string + * @param machine_friendly if true, the returned string is formatted + * to yyyymmdd_hhmmss, without weekday or spaces + * @note This has not been tested under Windows + * @note The timestamp is truncated to seconds + */ + string Format(bool machine_friendly = false) const; + + /** + * Returns a string version of the elapsed time in seconds, with the format + * xd hh:mm:ss, hh:mm:ss, mm:ss or s.us + * @param s: elapsed seconds (given by getFloatTime) to format + */ + static string Format(double s); + + +protected: + /// Seconds + unsigned long m_secs; // seconds + /// Microseconds + unsigned long m_usecs; // microseconds +}; + +} + +#endif + diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/README.txt b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/README.txt new file mode 100644 index 0000000..71827f0 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/DBoW2/README.txt @@ -0,0 +1,7 @@ +You should have received this DBoW2 version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). +See the original DBoW2 library at: https://github.com/dorian3d/DBoW2 +All files included in this version are BSD, see LICENSE.txt + +We also use Random.h, Random.cpp, Timestamp.pp and Timestamp.h from DLib/DUtils. +See the original DLib library at: https://github.com/dorian3d/DLib +All files included in this version are BSD, see LICENSE.txt diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/.clang-format b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/.clang-format new file mode 100644 index 0000000..6beec50 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/.clang-format @@ -0,0 +1,87 @@ +Language: Cpp +# BasedOnStyle: Google +AccessModifierOffset: -1 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlinesLeft: true +AlignOperands: true +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: true +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: true +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +ColumnLimit: 80 +CommentPragmas: '^ IWYU pragma:' +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: true +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] +IncludeCategories: + - Regex: '^<.*\.h>' + Priority: 1 + - Regex: '^<.*' + Priority: 2 + - Regex: '.*' + Priority: 3 +IndentCaseLabels: true +IndentWidth: 2 +IndentWrappedFunctionNames: false +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: false +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +ReflowComments: true +SortIncludes: true +SpaceAfterCStyleCast: false +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Auto +TabWidth: 8 +UseTab: Never diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/.gitignore b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/.gitignore new file mode 100644 index 0000000..0aed9a8 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/.gitignore @@ -0,0 +1,4 @@ +build +CMakeLists.txt.user +*.pyc +.vscode/settings.json \ No newline at end of file diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/.travis.yml b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/.travis.yml new file mode 100644 index 0000000..bb9e463 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/.travis.yml @@ -0,0 +1,83 @@ +sudo: required +dist: xenial + +before_install: + - if [[ "$TRAVIS_OS_NAME" == "linux" ]]; then if ! [[ "$BUILD_TYPE" == "Python" ]]; then if ! [[ "$BUILD_TYPE" == "Docs" ]]; then scripts/install_linux_deps.sh ; fi ; fi ; fi + - if [[ "$TRAVIS_OS_NAME" == "osx" ]]; then scripts/install_osx_deps.sh ; fi + +install: +- if [[ "$BUILD_TYPE" == "Coverage" ]]; then wget http://ftp.de.debian.org/debian/pool/main/l/lcov/lcov_1.11.orig.tar.gz ; fi +- if [[ "$BUILD_TYPE" == "Coverage" ]]; then tar xf lcov_1.11.orig.tar.gz ; fi +- if [[ "$BUILD_TYPE" == "Coverage" ]]; then sudo make -C lcov-1.11/ install ; fi +- if [[ "$BUILD_TYPE" == "Coverage" ]]; then gem install coveralls-lcov ; fi +- if [[ "$BUILD_TYPE" == "Python" ]]; then pip3 install sympy ; fi +- if [[ "$BUILD_TYPE" == "Docs" ]]; then scripts/install_docs_deps.sh ; fi + +language: cpp + +cache: ccache + +matrix: + include: + - language: cpp + compiler: gcc + os: linux + env: BUILD_TYPE=Coverage ROW_MAJOR_DEFAULT=OFF + - language: cpp + compiler: gcc + os: linux + env: BUILD_TYPE=Release ROW_MAJOR_DEFAULT=OFF + - language: cpp + compiler: clang + os: linux + env: BUILD_TYPE=Debug ROW_MAJOR_DEFAULT=OFF + - language: cpp + compiler: gcc + os: linux + env: BUILD_TYPE=Debug ROW_MAJOR_DEFAULT=ON + - language: cpp + compiler: clang + os: linux + env: BUILD_TYPE=Release ROW_MAJOR_DEFAULT=OFF + - language: cpp + compiler: clang + os: osx + env: BUILD_TYPE=Debug + - language: python + env: BUILD_TYPE=Python + os: linux + python: "3.7" + - language: python + env: BUILD_TYPE=Docs + os: linux + python: "3.7" + + +script: + - if ! [[ "$BUILD_TYPE" == "Python" ]]; then if ! [[ "$BUILD_TYPE" == "Docs" ]]; then scripts/run_cpp_tests.sh ; fi ; fi + - if [[ "$BUILD_TYPE" == "Python" ]]; then cd py; ./run_tests.sh ; fi + - if [[ "$BUILD_TYPE" == "Docs" ]]; then ./make_docs.sh ; fi + - if [[ "$BUILD_TYPE" == "Docs" ]]; then cd html-dir ; fi + - if [[ "$BUILD_TYPE" == "Docs" ]]; then touch .nojekyll ; fi + - if [[ "$BUILD_TYPE" == "Docs" ]]; then cd ../.. ; fi + +after_success: + - if [[ "$BUILD_TYPE" == "Coverage" ]]; then lcov --directory . --capture --output-file coverage.info ; fi + - if [[ "$BUILD_TYPE" == "Coverage" ]]; then lcov --remove coverage.info 'test/*' '/usr/*' --output-file coverage.info ; fi + - if [[ "$BUILD_TYPE" == "Coverage" ]]; then lcov --list coverage.info ; fi + - if [[ "$BUILD_TYPE" == "Coverage" ]]; then coveralls-lcov --repo-token ${COVERALLS_TOKEN} coverage.info ; fi + +deploy: + provider: pages + skip_cleanup: true + github_token: $GITHUB_TOKEN # Set in the settings page of your repository, as a secure variable + keep_history: false + local_dir: html-dir/ + verbose: true + on: + branch: master + condition: $BUILD_TYPE = Docs + +branches: + only: + - master diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/CMakeLists.txt b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/CMakeLists.txt new file mode 100644 index 0000000..933445b --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/CMakeLists.txt @@ -0,0 +1,158 @@ +cmake_minimum_required(VERSION 3.4) +project(Sophus VERSION 1.1.0) + +include(CMakePackageConfigHelpers) +include(GNUInstallDirs) + +# Release by default +# Turn on Debug with "-DCMAKE_BUILD_TYPE=Debug" +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +set(CMAKE_CXX_STANDARD 11) + +# Set compiler specific settings (FixMe: Should not cmake do this for us automatically?) +IF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") + SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g") + SET(CMAKE_CXX_FLAGS_RELEASE "-O3") + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra -Wno-deprecated-register -Qunused-arguments -fcolor-diagnostics") +ELSEIF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") + SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g") + SET(CMAKE_CXX_FLAGS_RELEASE "-O3") + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra -std=c++11 -Wno-deprecated-declarations -ftemplate-backtrace-limit=0") + SET(CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_CXX_FLAGS_DEBUG} --coverage -fno-inline -fno-inline-small-functions -fno-default-inline") + SET(CMAKE_EXE_LINKER_FLAGS_COVERAGE "${CMAKE_EXE_LINKER_FLAGS_DEBUG} --coverage") + SET(CMAKE_SHARED_LINKER_FLAGS_COVERAGE "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} --coverage") +ELSEIF(CMAKE_CXX_COMPILER_ID MATCHES "^MSVC$") + ADD_DEFINITIONS("-D _USE_MATH_DEFINES /bigobj /wd4305 /wd4244 /MP") +ENDIF() + +# Add local path for finding packages, set the local version first +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules") + +# Find Eigen 3 (dependency) +find_package(Eigen3 3.3.0 REQUIRED) + +# Define interface library target +add_library(sophus INTERFACE) + +set(SOPHUS_HEADER_FILES + sophus/average.hpp + sophus/common.hpp + sophus/geometry.hpp + sophus/interpolate.hpp + sophus/interpolate_details.hpp + sophus/num_diff.hpp + sophus/rotation_matrix.hpp + sophus/rxso2.hpp + sophus/rxso3.hpp + sophus/se2.hpp + sophus/se3.hpp + sophus/sim2.hpp + sophus/sim3.hpp + sophus/sim_details.hpp + sophus/so2.hpp + sophus/so3.hpp + sophus/types.hpp + sophus/velocities.hpp + sophus/formatstring.hpp +) + +set(SOPHUS_OTHER_FILES + sophus/test_macros.hpp + sophus/example_ensure_handler.cpp +) + +if(MSVC) + # Define common math constants if we compile with MSVC + target_compile_definitions (sophus INTERFACE _USE_MATH_DEFINES) +endif (MSVC) + +# Add Eigen interface dependency, depending on available cmake info +if(TARGET Eigen3::Eigen) + target_link_libraries(sophus INTERFACE Eigen3::Eigen) + set(Eigen3_DEPENDENCY "find_dependency (Eigen3 ${Eigen3_VERSION})") +else(TARGET Eigen3::Eigen) + target_include_directories (sophus SYSTEM INTERFACE ${EIGEN3_INCLUDE_DIR}) +endif(TARGET Eigen3::Eigen) + +# Associate target with include directory +target_include_directories(sophus INTERFACE + "$" + "$" +) + +# Declare all used C++11 features +target_compile_features (sophus INTERFACE + cxx_auto_type + cxx_decltype + cxx_nullptr + cxx_right_angle_brackets + cxx_variadic_macros + cxx_variadic_templates +) + +# Add sources as custom target so that they are shown in IDE's +add_custom_target(other SOURCES ${SOPHUS_OTHER_FILES}) + +# Create 'test' make target using ctest +option(BUILD_TESTS "Build tests." ON) +if(BUILD_TESTS) + enable_testing() + add_subdirectory(test) +endif() + +# Create examples make targets using ctest +option(BUILD_EXAMPLES "Build examples." ON) +if(BUILD_EXAMPLES) + add_subdirectory(examples) +endif() + +# Export package for use from the build tree +set(SOPHUS_CMAKE_EXPORT_DIR ${CMAKE_INSTALL_DATADIR}/sophus/cmake) + +set_target_properties(sophus PROPERTIES EXPORT_NAME Sophus) + +install(TARGETS sophus EXPORT SophusTargets) +install(EXPORT SophusTargets + NAMESPACE Sophus:: + DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR} +) + +export(TARGETS sophus NAMESPACE Sophus:: FILE SophusTargets.cmake) +export(PACKAGE Sophus) + +configure_package_config_file( + SophusConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake + INSTALL_DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR} + NO_CHECK_REQUIRED_COMPONENTS_MACRO +) + +# Remove architecture dependence. Sophus is a header-only library. +set(TEMP_SIZEOF_VOID_P ${CMAKE_SIZEOF_VOID_P}) +unset(CMAKE_SIZEOF_VOID_P) + +# Write version to file +write_basic_package_version_file ( + SophusConfigVersion.cmake + VERSION ${PROJECT_VERSION} + COMPATIBILITY SameMajorVersion +) + +# Recover architecture dependence +set(CMAKE_SIZEOF_VOID_P ${TEMP_SIZEOF_VOID_P}) + +# Install cmake targets +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake + ${CMAKE_CURRENT_BINARY_DIR}/SophusConfigVersion.cmake + DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR} +) + +# Install header files +install( + FILES ${SOPHUS_HEADER_FILES} + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sophus +) diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/LICENSE.txt b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/LICENSE.txt new file mode 100644 index 0000000..fb67702 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/LICENSE.txt @@ -0,0 +1,20 @@ +Copyright 2011-2017 Hauke Strasdat + 2012-2017 Steven Lovegrove + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to +deal in the Software without restriction, including without limitation the +rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +sell copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +IN THE SOFTWARE. \ No newline at end of file diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/README.rst b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/README.rst new file mode 100644 index 0000000..ba99e5a --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/README.rst @@ -0,0 +1,37 @@ +linux, os x: |TravisCI|_ windows: |AppVeyor|_ code coverage: |ci_cov|_ + + +Sophus +====== + +Overview +-------- + +This is a c++ implementation of Lie groups commonly used for 2d and 3d +geometric problems (i.e. for Computer Vision or Robotics applications). +Among others, this package includes the special orthogonal groups SO(2) and +SO(3) to present rotations in 2d and 3d as well as the special Euclidean group +SE(2) and SE(3) to represent rigid body transformations (i.e. rotations and +translations) in 2d and 3d. + +API documentation: https://strasdat.github.io/Sophus/ + +Cross platform support +---------------------- + +Sophus compiles with clang and gcc on Linux and OS X as well as msvc on Windows. +The specific compiler and operating system versions which are supported are +the ones which are used in the Continuous Integration (CI): See TravisCI_ and +AppVeyor_ for details. + +However, it should work (with no to minor modification) on many other +modern configurations as long they support c++11, CMake, and Eigen 3.X. + +.. |TravisCI| image:: https://travis-ci.org/strasdat/Sophus.svg?branch=master +.. _TravisCI: https://travis-ci.org/strasdat/Sophus + +.. |AppVeyor| image:: https://ci.appveyor.com/api/projects/status/um4285lwhs8ci7pt/branch/master?svg=true +.. _AppVeyor: https://ci.appveyor.com/project/strasdat/sophus/branch/master + +.. |ci_cov| image:: https://coveralls.io/repos/github/strasdat/Sophus/badge.svg?branch=master +.. _ci_cov: https://coveralls.io/github/strasdat/Sophus?branch=master diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/SophusConfig.cmake.in b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/SophusConfig.cmake.in new file mode 100644 index 0000000..1e9a117 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/SophusConfig.cmake.in @@ -0,0 +1,7 @@ +@PACKAGE_INIT@ + +include (CMakeFindDependencyMacro) + +@Eigen3_DEPENDENCY@ + +include ("${CMAKE_CURRENT_LIST_DIR}/SophusTargets.cmake") diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/appveyor.yml b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/appveyor.yml new file mode 100644 index 0000000..c5720e2 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/appveyor.yml @@ -0,0 +1,26 @@ +branches: + only: + - master + +os: Visual Studio 2015 + +clone_folder: c:\projects\sophus + +platform: x64 +configuration: Release + +build: + project: c:\projects\sophus\build\Sophus.sln + +install: + - ps: wget https://gitlab.com/libeigen/eigen/-/archive/3.3.4/eigen-3.3.4.zip -outfile eigen3.zip + - cmd: 7z x eigen3.zip -o"C:\projects" -y > nul + +before_build: + - cd c:\projects\sophus + - mkdir build + - cd build + - cmake -G "Visual Studio 14 2015 Win64" -D EIGEN3_INCLUDE_DIR=C:\projects\eigen-3.3.4 .. + +after_build: + - ctest diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/doxyrest-config.lua b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/doxyrest-config.lua new file mode 100644 index 0000000..c4e8bc8 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/doxyrest-config.lua @@ -0,0 +1,20 @@ +FRAME_DIR_LIST = { "doxyrest_b/doxyrest/frame/cfamily", "doxyrest_b/doxyrest/frame/common" } +FRAME_FILE = "index.rst.in" +INPUT_FILE = "xml-dir/index.xml" +OUTPUT_FILE = "rst-dir/index.rst" +INTRO_FILE = "page_index.rst" +SORT_GROUPS_BY = "title" +GLOBAL_AUX_COMPOUND_ID = "group_global" +LANGUAGE = cpp +VERBATIM_TO_CODE_BLOCK = "cpp" +ESCAPE_ASTERISKS = true +ESCAPE_PIPES = true +ESCAPE_TRAILING_UNDERSCORES = true +PROTECTION_FILTER = "protected" +EXCLUDE_EMPTY_DEFINES = true +EXCLUDE_DEFAULT_CONSTRUCTORS = false +EXCLUDE_DESTRUCTORS = false +EXCLUDE_PRIMITIVE_TYPEDEFS = true +SHOW_DIRECT_DESCENDANTS = true +TYPEDEF_TO_USING = true +ML_PARAM_LIST_LENGTH_THRESHOLD = 80 \ No newline at end of file diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/examples/CMakeLists.txt b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/examples/CMakeLists.txt new file mode 100644 index 0000000..4132e36 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/examples/CMakeLists.txt @@ -0,0 +1,8 @@ +# Tests to run +SET( EXAMPLE_SOURCES HelloSO3) +find_package( Ceres 1.6.0 QUIET ) + +FOREACH(example_src ${EXAMPLE_SOURCES}) + ADD_EXECUTABLE( ${example_src} ${example_src}.cpp) + TARGET_LINK_LIBRARIES( ${example_src} sophus ) +ENDFOREACH(example_src) diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/examples/HelloSO3.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/examples/HelloSO3.cpp new file mode 100644 index 0000000..0d12be1 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/examples/HelloSO3.cpp @@ -0,0 +1,38 @@ +#include +#include "sophus/geometry.hpp" + +int main() { + // The following demonstrates the group multiplication of rotation matrices + + // Create rotation matrices from rotations around the x and y and z axes: + const double kPi = Sophus::Constants::pi(); + Sophus::SO3d R1 = Sophus::SO3d::rotX(kPi / 4); + Sophus::SO3d R2 = Sophus::SO3d::rotY(kPi / 6); + Sophus::SO3d R3 = Sophus::SO3d::rotZ(-kPi / 3); + + std::cout << "The rotation matrices are" << std::endl; + std::cout << "R1:\n" << R1.matrix() << std::endl; + std::cout << "R2:\n" << R2.matrix() << std::endl; + std::cout << "R3:\n" << R3.matrix() << std::endl; + std::cout << "Their product R1*R2*R3:\n" + << (R1 * R2 * R3).matrix() << std::endl; + std::cout << std::endl; + + // Rotation matrices can act on vectors + Eigen::Vector3d x; + x << 0.0, 0.0, 1.0; + std::cout << "Rotation matrices can act on vectors" << std::endl; + std::cout << "x\n" << x << std::endl; + std::cout << "R2*x\n" << R2 * x << std::endl; + std::cout << "R1*(R2*x)\n" << R1 * (R2 * x) << std::endl; + std::cout << "(R1*R2)*x\n" << (R1 * R2) * x << std::endl; + std::cout << std::endl; + + // SO(3) are internally represented as unit quaternions. + std::cout << "R1 in matrix form:\n" << R1.matrix() << std::endl; + std::cout << "R1 in unit quaternion form:\n" + << R1.unit_quaternion().coeffs() << std::endl; + // Note that the order of coefficiences of Eigen's quaternion class is + // (imag0, imag1, imag2, real) + std::cout << std::endl; +} \ No newline at end of file diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/make_docs.sh b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/make_docs.sh new file mode 100755 index 0000000..6393cef --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/make_docs.sh @@ -0,0 +1,3 @@ +doxygen doxyfile +doxyrest_b/build/doxyrest/bin/Release/doxyrest -c doxyrest-config.lua +sphinx-build -b html rst-dir html-dir \ No newline at end of file diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/package.xml b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/package.xml new file mode 100644 index 0000000..78d99bf --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/package.xml @@ -0,0 +1,22 @@ + + + sophus + 1.1.0 + + C++ implementation of Lie Groups using Eigen. + + https://github.com/strasdat/sophus + https://github.com/strasdat/sophus/issues + Daniel Stonier + Hauke Strasdat + MIT + + cmake + + eigen + eigen + + + cmake + + diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/Se2_Dx_exp_x.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/Se2_Dx_exp_x.cpp new file mode 100644 index 0000000..06d0fff --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/Se2_Dx_exp_x.cpp @@ -0,0 +1,22 @@ +Scalar const c0 = sin(theta); +Scalar const c1 = cos(theta); +Scalar const c2 = 1.0/theta; +Scalar const c3 = c0*c2; +Scalar const c4 = 1 - c1; +Scalar const c5 = c2*c4; +Scalar const c6 = c1*c2; +Scalar const c7 = pow(theta, -2); +Scalar const c8 = c0*c7; +Scalar const c9 = c4*c7; +result[0] = 0; +result[1] = 0; +result[2] = -c0; +result[3] = 0; +result[4] = 0; +result[5] = c1; +result[6] = c3; +result[7] = -c5; +result[8] = -c3*upsilon[1] + c6*upsilon[0] - c8*upsilon[0] + c9*upsilon[1]; +result[9] = c5; +result[10] = c3; +result[11] = c3*upsilon[0] + c6*upsilon[1] - c8*upsilon[1] - c9*upsilon[0]; diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/Se2_Dx_this_mul_exp_x_at_0.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/Se2_Dx_this_mul_exp_x_at_0.cpp new file mode 100644 index 0000000..d4280fc --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/Se2_Dx_this_mul_exp_x_at_0.cpp @@ -0,0 +1,13 @@ +Scalar const c0 = -c[1]; +result[0] = 0; +result[1] = 0; +result[2] = c0; +result[3] = 0; +result[4] = 0; +result[5] = c[0]; +result[6] = c[0]; +result[7] = c0; +result[8] = 0; +result[9] = c[1]; +result[10] = c[0]; +result[11] = 0; diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/Se3_Dx_exp_x.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/Se3_Dx_exp_x.cpp new file mode 100644 index 0000000..6ab4884 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/Se3_Dx_exp_x.cpp @@ -0,0 +1,138 @@ +Scalar const c0 = pow(omega[0], 2); +Scalar const c1 = pow(omega[1], 2); +Scalar const c2 = pow(omega[2], 2); +Scalar const c3 = c0 + c1 + c2; +Scalar const c4 = sqrt(c3); +Scalar const c5 = 1.0/c4; +Scalar const c6 = 0.5*c4; +Scalar const c7 = sin(c6); +Scalar const c8 = c5*c7; +Scalar const c9 = pow(c3, -3.0/2.0); +Scalar const c10 = c7*c9; +Scalar const c11 = 1.0/c3; +Scalar const c12 = 0.5*c11*cos(c6); +Scalar const c13 = c10*omega[0]; +Scalar const c14 = omega[0]*omega[1]; +Scalar const c15 = c12*c14 - c13*omega[1]; +Scalar const c16 = omega[0]*omega[2]; +Scalar const c17 = c12*c16 - c13*omega[2]; +Scalar const c18 = omega[1]*omega[2]; +Scalar const c19 = -c10*c18 + c12*c18; +Scalar const c20 = 0.5*c8; +Scalar const c21 = sin(c4); +Scalar const c22 = -c21 + c4; +Scalar const c23 = -c1; +Scalar const c24 = -c2; +Scalar const c25 = c23 + c24; +Scalar const c26 = c25*c9; +Scalar const c27 = cos(c4); +Scalar const c28 = 1 - c27; +Scalar const c29 = c11*c28; +Scalar const c30 = c29*omega[2]; +Scalar const c31 = c22*c9; +Scalar const c32 = c31*omega[1]; +Scalar const c33 = c32*omega[0]; +Scalar const c34 = c29*omega[1]; +Scalar const c35 = c31*omega[2]; +Scalar const c36 = c35*omega[0]; +Scalar const c37 = 3*c22/pow(c3, 5.0/2.0); +Scalar const c38 = c25*c37; +Scalar const c39 = c5*omega[0]; +Scalar const c40 = -c27*c39 + c39; +Scalar const c41 = c40*c9; +Scalar const c42 = c0*c37; +Scalar const c43 = c16*c41 + c35 - c42*omega[2]; +Scalar const c44 = c21*c9; +Scalar const c45 = c14*c44; +Scalar const c46 = 2*c28/pow(c3, 2); +Scalar const c47 = c14*c46; +Scalar const c48 = c45 - c47; +Scalar const c49 = c14*c41 + c32 - c42*omega[1]; +Scalar const c50 = c16*c44; +Scalar const c51 = c16*c46; +Scalar const c52 = -c50 + c51; +Scalar const c53 = -2*c32; +Scalar const c54 = c5*omega[1]; +Scalar const c55 = -c27*c54 + c54; +Scalar const c56 = c1*c44; +Scalar const c57 = c1*c46; +Scalar const c58 = c55*c9; +Scalar const c59 = c16*c58; +Scalar const c60 = c37*omega[0]; +Scalar const c61 = -c18*c60; +Scalar const c62 = c29 + c61; +Scalar const c63 = c31*omega[0]; +Scalar const c64 = -c1*c60 + c14*c58 + c63; +Scalar const c65 = c18*c44; +Scalar const c66 = c18*c46; +Scalar const c67 = -c65 + c66; +Scalar const c68 = -2*c35; +Scalar const c69 = c5*omega[2]; +Scalar const c70 = -c27*c69 + c69; +Scalar const c71 = c2*c44; +Scalar const c72 = c2*c46; +Scalar const c73 = c70*c9; +Scalar const c74 = c14*c73; +Scalar const c75 = -c29 + c61; +Scalar const c76 = c65 - c66; +Scalar const c77 = c16*c73 - c2*c60 + c63; +Scalar const c78 = -c0; +Scalar const c79 = c24 + c78; +Scalar const c80 = c29*omega[0]; +Scalar const c81 = c32*omega[2]; +Scalar const c82 = -2*c63; +Scalar const c83 = c79*c9; +Scalar const c84 = c0*c44; +Scalar const c85 = c0*c46; +Scalar const c86 = c18*c41; +Scalar const c87 = c50 - c51; +Scalar const c88 = c37*c79; +Scalar const c89 = -c45 + c47; +Scalar const c90 = c37*omega[2]; +Scalar const c91 = -c1*c90 + c18*c58 + c35; +Scalar const c92 = c37*omega[1]; +Scalar const c93 = c18*c73 - c2*c92 + c32; +Scalar const c94 = c23 + c78; +Scalar const c95 = c9*c94; +result[0] = 0; +result[1] = 0; +result[2] = 0; +result[3] = -c0*c10 + c0*c12 + c8; +result[4] = c15; +result[5] = c17; +result[6] = 0; +result[7] = 0; +result[8] = 0; +result[9] = c15; +result[10] = -c1*c10 + c1*c12 + c8; +result[11] = c19; +result[12] = 0; +result[13] = 0; +result[14] = 0; +result[15] = c17; +result[16] = c19; +result[17] = -c10*c2 + c12*c2 + c8; +result[18] = 0; +result[19] = 0; +result[20] = 0; +result[21] = -c20*omega[0]; +result[22] = -c20*omega[1]; +result[23] = -c20*omega[2]; +result[24] = c22*c26 + 1; +result[25] = -c30 + c33; +result[26] = c34 + c36; +result[27] = upsilon[0]*(c26*c40 - c38*omega[0]) + upsilon[1]*(c49 + c52) + upsilon[2]*(c43 + c48); +result[28] = upsilon[0]*(c26*c55 - c38*omega[1] + c53) + upsilon[1]*(c64 + c67) + upsilon[2]*(c56 - c57 + c59 + c62); +result[29] = upsilon[0]*(c26*c70 - c38*omega[2] + c68) + upsilon[1]*(-c71 + c72 + c74 + c75) + upsilon[2]*(c76 + c77); +result[30] = c30 + c33; +result[31] = c31*c79 + 1; +result[32] = -c80 + c81; +result[33] = upsilon[0]*(c49 + c87) + upsilon[1]*(c40*c83 - c60*c79 + c82) + upsilon[2]*(c75 - c84 + c85 + c86); +result[34] = upsilon[0]*(c64 + c76) + upsilon[1]*(c55*c83 - c88*omega[1]) + upsilon[2]*(c89 + c91); +result[35] = upsilon[0]*(c62 + c71 - c72 + c74) + upsilon[1]*(c68 + c70*c83 - c88*omega[2]) + upsilon[2]*(c52 + c93); +result[36] = -c34 + c36; +result[37] = c80 + c81; +result[38] = c31*c94 + 1; +result[39] = upsilon[0]*(c43 + c89) + upsilon[1]*(c62 + c84 - c85 + c86) + upsilon[2]*(c40*c95 - c60*c94 + c82); +result[40] = upsilon[0]*(-c56 + c57 + c59 + c75) + upsilon[1]*(c48 + c91) + upsilon[2]*(c53 + c55*c95 - c92*c94); +result[41] = upsilon[0]*(c67 + c77) + upsilon[1]*(c87 + c93) + upsilon[2]*(c70*c95 - c90*c94); diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/Se3_Dx_this_mul_exp_x_at_0.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/Se3_Dx_this_mul_exp_x_at_0.cpp new file mode 100644 index 0000000..6c69d76 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/Se3_Dx_this_mul_exp_x_at_0.cpp @@ -0,0 +1,64 @@ +Scalar const c0 = 0.5*q.w(); +Scalar const c1 = 0.5*q.z(); +Scalar const c2 = -c1; +Scalar const c3 = 0.5*q.y(); +Scalar const c4 = 0.5*q.x(); +Scalar const c5 = -c4; +Scalar const c6 = -c3; +Scalar const c7 = pow(q.x(), 2); +Scalar const c8 = pow(q.y(), 2); +Scalar const c9 = -c8; +Scalar const c10 = pow(q.w(), 2); +Scalar const c11 = pow(q.z(), 2); +Scalar const c12 = c10 - c11; +Scalar const c13 = 2*q.w(); +Scalar const c14 = c13*q.z(); +Scalar const c15 = 2*q.x(); +Scalar const c16 = c15*q.y(); +Scalar const c17 = c13*q.y(); +Scalar const c18 = c15*q.z(); +Scalar const c19 = -c7; +Scalar const c20 = c13*q.x(); +Scalar const c21 = 2*q.y()*q.z(); +result[0] = 0; +result[1] = 0; +result[2] = 0; +result[3] = c0; +result[4] = c2; +result[5] = c3; +result[6] = 0; +result[7] = 0; +result[8] = 0; +result[9] = c1; +result[10] = c0; +result[11] = c5; +result[12] = 0; +result[13] = 0; +result[14] = 0; +result[15] = c6; +result[16] = c4; +result[17] = c0; +result[18] = 0; +result[19] = 0; +result[20] = 0; +result[21] = c5; +result[22] = c6; +result[23] = c2; +result[24] = c12 + c7 + c9; +result[25] = -c14 + c16; +result[26] = c17 + c18; +result[27] = 0; +result[28] = 0; +result[29] = 0; +result[30] = c14 + c16; +result[31] = c12 + c19 + c8; +result[32] = -c20 + c21; +result[33] = 0; +result[34] = 0; +result[35] = 0; +result[36] = -c17 + c18; +result[37] = c20 + c21; +result[38] = c10 + c11 + c19 + c9; +result[39] = 0; +result[40] = 0; +result[41] = 0; diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/So2_Dx_exp_x.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/So2_Dx_exp_x.cpp new file mode 100644 index 0000000..606b9df --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/So2_Dx_exp_x.cpp @@ -0,0 +1,2 @@ +result[0] = -sin(theta); +result[1] = cos(theta); diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/So2_Dx_this_mul_exp_x_at_0.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/So2_Dx_this_mul_exp_x_at_0.cpp new file mode 100644 index 0000000..2ef21c6 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/So2_Dx_this_mul_exp_x_at_0.cpp @@ -0,0 +1,2 @@ +result[0] = -c[1]; +result[1] = c[0]; diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/So3_Dx_exp_x.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/So3_Dx_exp_x.cpp new file mode 100644 index 0000000..96e8f00 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/So3_Dx_exp_x.cpp @@ -0,0 +1,29 @@ +Scalar const c0 = pow(omega[0], 2); +Scalar const c1 = pow(omega[1], 2); +Scalar const c2 = pow(omega[2], 2); +Scalar const c3 = c0 + c1 + c2; +Scalar const c4 = sqrt(c3); +Scalar const c5 = 0.5*c4; +Scalar const c6 = sin(c5); +Scalar const c7 = c6/c4; +Scalar const c8 = c6/pow(c3, 3.0/2.0); +Scalar const c9 = 0.5*cos(c5)/c3; +Scalar const c10 = c8*omega[0]; +Scalar const c11 = c9*omega[0]; +Scalar const c12 = -c10*omega[1] + c11*omega[1]; +Scalar const c13 = -c10*omega[2] + c11*omega[2]; +Scalar const c14 = omega[1]*omega[2]; +Scalar const c15 = -c14*c8 + c14*c9; +Scalar const c16 = 0.5*c7; +result[0] = -c0*c8 + c0*c9 + c7; +result[1] = c12; +result[2] = c13; +result[3] = c12; +result[4] = -c1*c8 + c1*c9 + c7; +result[5] = c15; +result[6] = c13; +result[7] = c15; +result[8] = -c2*c8 + c2*c9 + c7; +result[9] = -c16*omega[0]; +result[10] = -c16*omega[1]; +result[11] = -c16*omega[2]; diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/So3_Dx_this_mul_exp_x_at_0.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/So3_Dx_this_mul_exp_x_at_0.cpp new file mode 100644 index 0000000..48a5783 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/cpp_gencode/So3_Dx_this_mul_exp_x_at_0.cpp @@ -0,0 +1,19 @@ +Scalar const c0 = 0.5*q.w(); +Scalar const c1 = 0.5*q.z(); +Scalar const c2 = -c1; +Scalar const c3 = 0.5*q.y(); +Scalar const c4 = 0.5*q.x(); +Scalar const c5 = -c4; +Scalar const c6 = -c3; +result[0] = c0; +result[1] = c2; +result[2] = c3; +result[3] = c1; +result[4] = c0; +result[5] = c5; +result[6] = c6; +result[7] = c4; +result[8] = c0; +result[9] = c5; +result[10] = c6; +result[11] = c2; diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/run_tests.sh b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/run_tests.sh new file mode 100755 index 0000000..045290e --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/run_tests.sh @@ -0,0 +1,13 @@ +#!/bin/bash + +EXIT=0 + +python3 -m sophus.complex || EXIT=$? +python3 -m sophus.quaternion || EXIT=$? +python3 -m sophus.dual_quaternion || EXIT=$? +python3 -m sophus.so2 || EXIT=$? +python3 -m sophus.se2 || EXIT=$? +python3 -m sophus.so3 || EXIT=$? +python3 -m sophus.se3 || EXIT=$? + +exit $EXIT diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/__init__.py b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/__init__.py new file mode 100644 index 0000000..7b21109 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/__init__.py @@ -0,0 +1,7 @@ +from sophus.cse_codegen import cse_codegen +from sophus.matrix import dot, squared_norm, Vector2, Vector3, Vector6, ZeroVector2, ZeroVector3, ZeroVector6, proj, unproj +from sophus.complex import Complex +from sophus.quaternion import Quaternion +from sophus.so2 import So2 +from sophus.so3 import So3 +from sophus.se3 import Se3 diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/complex.py b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/complex.py new file mode 100644 index 0000000..bb96ce8 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/complex.py @@ -0,0 +1,112 @@ +import sophus +import sympy +import sys +import unittest + + +class Complex: + """ Complex class """ + + def __init__(self, real, imag): + self.real = real + self.imag = imag + + def __mul__(self, right): + """ complex multiplication """ + return Complex(self.real * right.real - self.imag * right.imag, + self.imag * right.real + self.real * right.imag) + + def __add__(self, right): + return Complex(elf.real + right.real, self.imag + right.imag) + + def __neg__(self): + return Complex(-self.real, -self.image) + + def __truediv__(self, scalar): + """ scalar division """ + return Complex(self.real / scalar, self.imag / scalar) + + def __repr__(self): + return "( " + repr(self.real) + " + " + repr(self.imag) + "i )" + + def __getitem__(self, key): + """ We use the following convention [real, imag] """ + if key == 0: + return self.real + else: + return self.imag + + def squared_norm(self): + """ squared norm when considering the complex number as tuple """ + return self.real**2 + self.imag**2 + + def conj(self): + """ complex conjugate """ + return Complex(self.real, -self.imag) + + def inv(self): + """ complex inverse """ + return self.conj() / self.squared_norm() + + @staticmethod + def identity(): + return Complex(1, 0) + + @staticmethod + def zero(): + return Complex(0, 0) + + def __eq__(self, other): + if isinstance(self, other.__class__): + return self.real == other.real and self.imag == other.imag + return False + + def subs(self, x, y): + return Complex(self.real.subs(x, y), self.imag.subs(x, y)) + + def simplify(self): + return Complex(sympy.simplify(self.real), + sympy.simplify(self.imag)) + + @staticmethod + def Da_a_mul_b(a, b): + """ derivatice of complex muliplication wrt left multiplier a """ + return sympy.Matrix([[b.real, -b.imag], + [b.imag, b.real]]) + + @staticmethod + def Db_a_mul_b(a, b): + """ derivatice of complex muliplication wrt right multiplicand b """ + return sympy.Matrix([[a.real, -a.imag], + [a.imag, a.real]]) + + +class TestComplex(unittest.TestCase): + def setUp(self): + x, y = sympy.symbols('x y', real=True) + u, v = sympy.symbols('u v', real=True) + self.a = Complex(x, y) + self.b = Complex(u, v) + + def test_muliplications(self): + product = self.a * self.a.inv() + self.assertEqual(product.simplify(), + Complex.identity()) + product = self.a.inv() * self.a + self.assertEqual(product.simplify(), + Complex.identity()) + + def test_derivatives(self): + d = sympy.Matrix(2, 2, lambda r, c: sympy.diff( + (self.a * self.b)[r], self.a[c])) + self.assertEqual(d, + Complex.Da_a_mul_b(self.a, self.b)) + d = sympy.Matrix(2, 2, lambda r, c: sympy.diff( + (self.a * self.b)[r], self.b[c])) + self.assertEqual(d, + Complex.Db_a_mul_b(self.a, self.b)) + + +if __name__ == '__main__': + unittest.main() + print('hello') diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/cse_codegen.py b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/cse_codegen.py new file mode 100644 index 0000000..a14af03 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/cse_codegen.py @@ -0,0 +1,17 @@ +import sympy +import io + + +def cse_codegen(symbols): + cse_results = sympy.cse(symbols, sympy.numbered_symbols("c")) + output = io.StringIO() + for helper in cse_results[0]: + output.write("Scalar const ") + output.write(sympy.printing.ccode(helper[1], helper[0])) + output.write("\n") + assert len(cse_results[1]) == 1 + + output.write(sympy.printing.ccode(cse_results[1][0], "result")) + output.write("\n") + output.seek(0) + return output diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/dual_quaternion.py b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/dual_quaternion.py new file mode 100644 index 0000000..c1286b1 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/dual_quaternion.py @@ -0,0 +1,92 @@ +import sympy +import sys +import unittest +import sophus + + +class DualQuaternion: + """ Dual quaternion class """ + + def __init__(self, real_q, inf_q): + """ Dual quaternion consists of a real quaternion, and an infinitesimal + quaternion """ + self.real_q = real_q + self.inf_q = inf_q + + def __mul__(self, right): + """ dual quaternion multiplication """ + return DualQuaternion(self.real_q * right.real_q, + self.real_q * right.inf_q + + self.inf_q * right.real_q) + + def __truediv__(self, scalar): + """ scalar division """ + return DualQuaternion(self.real_q / scalar, self.inf_q / scalar) + + def __repr__(self): + return "( " + repr(self.real_q) + \ + " + " + repr(self.inf_q) + ")" + + def __getitem__(self, key): + assert (key >= 0 and key < 8) + if key < 4: + return self.real_q[i] + else: + return self.inf_q[i - 4] + + def squared_norm(self): + """ squared norm when considering the dual quaternion as 8-tuple """ + return self.real_q.squared_norm() + self.inf_q.squared_norm() + + def conj(self): + """ dual quaternion conjugate """ + return DualQuaternion(self.real_q.conj(), self.inf_q.conj()) + + def inv(self): + """ dual quaternion inverse """ + return DualQuaternion(self.real_q.inv(), + -self.real_q.inv() * self.inf_q * + self.real_q.inv()) + + def simplify(self): + return DualQuaternion(self.real_q.simplify(), + self.inf_q.simplify()) + + @staticmethod + def identity(): + return DualQuaternion(sophus.Quaternion.identity(), + sophus.Quaternion.zero()) + + def __eq__(self, other): + if isinstance(self, other.__class__): + return self.real_q == other.real_q and self.inf_q == other.inf_q + return False + + +class TestDualQuaternion(unittest.TestCase): + def setUp(self): + w, s0, s1, s2 = sympy.symbols('w s0 s1 s2', real=True) + x, t0, t1, t2 = sympy.symbols('x t0 t1 t2', real=True) + y, u0, u1, u2 = sympy.symbols('y u0 u1 u2', real=True) + z, v0, v1, v2 = sympy.symbols('z v0 v1 v2', real=True) + + s = sophus.Vector3(s0, s1, s2) + t = sophus.Vector3(t0, t1, t2) + u = sophus.Vector3(u0, u1, u2) + v = sophus.Vector3(v0, v1, v2) + self.a = DualQuaternion(sophus.Quaternion(w, s), + sophus.Quaternion(x, t)) + self.b = DualQuaternion(sophus.Quaternion(y, u), + sophus.Quaternion(z, v)) + + def test_muliplications(self): + product = self.a * self.a.inv() + self.assertEqual(product.simplify(), + DualQuaternion.identity()) + product = self.a.inv() * self.a + self.assertEqual(product.simplify(), + DualQuaternion.identity()) + + +if __name__ == '__main__': + unittest.main() diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/matrix.py b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/matrix.py new file mode 100644 index 0000000..3275eb3 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/matrix.py @@ -0,0 +1,60 @@ +import sympy +import sys + +assert sys.version_info >= (3, 5) + + +def dot(left, right): + assert(isinstance(left, sympy.Matrix)) + assert(isinstance(right, sympy.Matrix)) + + sum = 0 + for c in range(0, left.cols): + for r in range(0, left.rows): + sum += left[r, c] * right[r, c] + return sum + + +def squared_norm(m): + assert(isinstance(m, sympy.Matrix)) + return dot(m, m) + + +def Vector2(x, y): + return sympy.Matrix([x, y]) + + +def ZeroVector2(): + return Vector2(0, 0) + + +def Vector3(x, y, z): + return sympy.Matrix([x, y, z]) + + +def ZeroVector3(): + return Vector3(0, 0, 0) + + +def Vector6(a, b, c, d, e, f): + return sympy.Matrix([a, b, c, d, e, f]) + + +def ZeroVector6(): + return Vector6(0, 0, 0, 0, 0, 0) + + +def proj(v): + m, n = v.shape + assert m > 1 + assert n == 1 + l = [v[i] / v[m - 1] for i in range(0, m - 1)] + r = sympy.Matrix(m - 1, 1, l) + return r + + +def unproj(v): + m, n = v.shape + assert m >= 1 + assert n == 1 + return v.col_join(sympy.Matrix.ones(1, 1)) diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/quaternion.py b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/quaternion.py new file mode 100644 index 0000000..6491938 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/quaternion.py @@ -0,0 +1,135 @@ +""" run with: python3 -m sophus.quaternion """ + +import sophus +import sympy +import sys +import unittest + + +class Quaternion: + """ Quaternion class """ + + def __init__(self, real, vec): + """ Quaternion consists of a real scalar, and an imaginary 3-vector """ + assert isinstance(vec, sympy.Matrix) + assert vec.shape == (3, 1), vec.shape + self.real = real + self.vec = vec + + def __mul__(self, right): + """ quaternion multiplication """ + return Quaternion(self[3] * right[3] - self.vec.dot(right.vec), + self[3] * right.vec + right[3] * self.vec + + self.vec.cross(right.vec)) + + def __add__(self, right): + """ quaternion multiplication """ + return Quaternion(self[3] + right[3], self.vec + right.vec) + + def __neg__(self): + return Quaternion(-self[3], -self.vec) + + def __truediv__(self, scalar): + """ scalar division """ + return Quaternion(self.real / scalar, self.vec / scalar) + + def __repr__(self): + return "( " + repr(self[3]) + " + " + repr(self.vec) + "i )" + + def __getitem__(self, key): + """ We use the following convention [vec0, vec1, vec2, real] """ + assert (key >= 0 and key < 4) + if key == 3: + return self.real + else: + return self.vec[key] + + def squared_norm(self): + """ squared norm when considering the quaternion as 4-tuple """ + return sophus.squared_norm(self.vec) + self.real**2 + + def conj(self): + """ quaternion conjugate """ + return Quaternion(self.real, -self.vec) + + def inv(self): + """ quaternion inverse """ + return self.conj() / self.squared_norm() + + @staticmethod + def identity(): + return Quaternion(1, sophus.Vector3(0, 0, 0)) + + @staticmethod + def zero(): + return Quaternion(0, sophus.Vector3(0, 0, 0)) + + def subs(self, x, y): + return Quaternion(self.real.subs(x, y), self.vec.subs(x, y)) + + def simplify(self): + v = sympy.simplify(self.vec) + return Quaternion(sympy.simplify(self.real), + sophus.Vector3(v[0], v[1], v[2])) + + def __eq__(self, other): + if isinstance(self, other.__class__): + return self.real == other.real and self.vec == other.vec + return False + + @staticmethod + def Da_a_mul_b(a, b): + """ derivatice of quaternion muliplication wrt left multiplier a """ + v0 = b.vec[0] + v1 = b.vec[1] + v2 = b.vec[2] + y = b.real + return sympy.Matrix([[y, v2, -v1, v0], + [-v2, y, v0, v1], + [v1, -v0, y, v2], + [-v0, -v1, -v2, y]]) + + @staticmethod + def Db_a_mul_b(a, b): + """ derivatice of quaternion muliplication wrt right multiplicand b """ + u0 = a.vec[0] + u1 = a.vec[1] + u2 = a.vec[2] + x = a.real + return sympy.Matrix([[x, -u2, u1, u0], + [u2, x, -u0, u1], + [-u1, u0, x, u2], + [-u0, -u1, -u2, x]]) + + +class TestQuaternion(unittest.TestCase): + def setUp(self): + x, u0, u1, u2 = sympy.symbols('x u0 u1 u2', real=True) + y, v0, v1, v2 = sympy.symbols('y v0 v1 v2', real=True) + u = sophus.Vector3(u0, u1, u2) + v = sophus.Vector3(v0, v1, v2) + self.a = Quaternion(x, u) + self.b = Quaternion(y, v) + + def test_muliplications(self): + product = self.a * self.a.inv() + self.assertEqual(product.simplify(), + Quaternion.identity()) + product = self.a.inv() * self.a + self.assertEqual(product.simplify(), + Quaternion.identity()) + + def test_derivatives(self): + d = sympy.Matrix(4, 4, lambda r, c: sympy.diff( + (self.a * self.b)[r], self.a[c])) + self.assertEqual(d, + Quaternion.Da_a_mul_b(self.a, self.b)) + d = sympy.Matrix(4, 4, lambda r, c: sympy.diff( + (self.a * self.b)[r], self.b[c])) + self.assertEqual(d, + Quaternion.Db_a_mul_b(self.a, self.b)) + + +if __name__ == '__main__': + unittest.main() + print('hello') diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/se2.py b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/se2.py new file mode 100644 index 0000000..72b0190 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/se2.py @@ -0,0 +1,224 @@ +import sympy +import sys +import unittest +import sophus +import functools + + +class Se2: + """ 2 dimensional group of rigid body transformations """ + + def __init__(self, so2, t): + """ internally represented by a unit complex number z and a translation + 2-vector """ + self.so2 = so2 + self.t = t + + @staticmethod + def exp(v): + """ exponential map """ + theta = v[2] + so2 = sophus.So2.exp(theta) + + a = so2.z.imag / theta + b = (1 - so2.z.real) / theta + + t = sophus.Vector2(a * v[0] - b * v[1], + b * v[0] + a * v[1]) + return Se2(so2, t) + + def log(self): + theta = self.so2.log() + halftheta = 0.5 * theta + a = -(halftheta * self.so2.z.imag) / (self.so2.z.real - 1) + + V_inv = sympy.Matrix([[a, halftheta], + [-halftheta, a]]) + upsilon = V_inv * self.t + return sophus.Vector3(upsilon[0], upsilon[1], theta) + + def __repr__(self): + return "Se2: [" + repr(self.so2) + " " + repr(self.t) + + @staticmethod + def hat(v): + upsilon = sophus.Vector2(v[0], v[1]) + theta = v[2] + return sophus.So2.hat(theta).\ + row_join(upsilon).\ + col_join(sympy.Matrix.zeros(1, 3)) + + def matrix(self): + """ returns matrix representation """ + R = self.so2.matrix() + return (R.row_join(self.t)).col_join(sympy.Matrix(1, 3, [0, 0, 1])) + + def __mul__(self, right): + """ left-multiplication + either rotation concatenation or point-transform """ + if isinstance(right, sympy.Matrix): + assert right.shape == (2, 1), right.shape + return self.so2 * right + self.t + elif isinstance(right, Se2): + return Se2(self.so2 * right.so2, + self.t + self.so2 * right.t) + assert False, "unsupported type: {0}".format(type(right)) + + def __getitem__(self, key): + """ We use the following convention [q0, q1, q2, q3, t0, t1, t2] """ + assert (key >= 0 and key < 4) + if key < 2: + return self.so2[key] + else: + return self.t[key - 2] + + @staticmethod + def calc_Dx_exp_x(x): + return sympy.Matrix(4, 3, lambda r, c: + sympy.diff(Se2.exp(x)[r], x[c])) + + @staticmethod + def Dx_exp_x_at_0(): + return sympy.Matrix([[0, 0, 0], + [0, 0, 1], + [1, 0, 0], + [0, 1, 0]]) + + def calc_Dx_this_mul_exp_x_at_0(self, x): + v = Se2.exp(x) + return sympy.Matrix(4, 3, lambda r, c: + sympy.diff((self * Se2.exp(x))[r], x[c])). \ + subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) + + @staticmethod + def calc_Dx_exp_x_at_0(x): + return Se2.calc_Dx_exp_x(x).subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) + + @staticmethod + def Dxi_x_matrix(x, i): + if i < 2: + return sophus.So2.Dxi_x_matrix(x, i).\ + row_join(sympy.Matrix.zeros(2, 1)).\ + col_join(sympy.Matrix.zeros(1, 3)) + M = sympy.Matrix.zeros(3, 3) + M[i - 2, 2] = 1 + return M + + @staticmethod + def calc_Dxi_x_matrix(x, i): + return sympy.Matrix(3, 3, lambda r, c: + sympy.diff(x.matrix()[r, c], x[i])) + + @staticmethod + def Dxi_exp_x_matrix(x, i): + T = Se2.exp(x) + Dx_exp_x = Se2.calc_Dx_exp_x(x) + l = [Dx_exp_x[j, i] * Se2.Dxi_x_matrix(T, j) for j in range(0, 4)] + return functools.reduce((lambda a, b: a + b), l) + + @staticmethod + def calc_Dxi_exp_x_matrix(x, i): + return sympy.Matrix(3, 3, lambda r, c: + sympy.diff(Se2.exp(x).matrix()[r, c], x[i])) + + @staticmethod + def Dxi_exp_x_matrix_at_0(i): + v = sophus.ZeroVector3() + v[i] = 1 + return Se2.hat(v) + + @staticmethod + def calc_Dxi_exp_x_matrix_at_0(x, i): + return sympy.Matrix(3, 3, lambda r, c: + sympy.diff(Se2.exp(x).matrix()[r, c], x[i]) + ).subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) + + +class TestSe2(unittest.TestCase): + def setUp(self): + upsilon0, upsilon1, theta = sympy.symbols( + 'upsilon[0], upsilon[1], theta', + real=True) + x, y = sympy.symbols('c[0] c[1]', real=True) + p0, p1 = sympy.symbols('p0 p1', real=True) + t0, t1 = sympy.symbols('t[0] t[1]', real=True) + self.upsilon_theta = sophus.Vector3( + upsilon0, upsilon1, theta) + self.t = sophus.Vector2(t0, t1) + self.a = Se2(sophus.So2(sophus.Complex(x, y)), self.t) + self.p = sophus.Vector2(p0, p1) + + def test_exp_log(self): + for v in [sophus.Vector3(0., 1, 0.5), + sophus.Vector3(0.1, 0.1, 0.1), + sophus.Vector3(0.01, 0.2, 0.03)]: + w = Se2.exp(v).log() + for i in range(0, 3): + self.assertAlmostEqual(v[i], w[i]) + + def test_matrix(self): + T_foo_bar = Se2.exp(self.upsilon_theta) + Tmat_foo_bar = T_foo_bar.matrix() + point_bar = self.p + p1_foo = T_foo_bar * point_bar + p2_foo = sophus.proj(Tmat_foo_bar * sophus.unproj(point_bar)) + self.assertEqual(sympy.simplify(p1_foo - p2_foo), + sophus.ZeroVector2()) + + def test_derivatives(self): + self.assertEqual(sympy.simplify( + Se2.calc_Dx_exp_x_at_0(self.upsilon_theta) - + Se2.Dx_exp_x_at_0()), + sympy.Matrix.zeros(4, 3)) + for i in range(0, 4): + self.assertEqual(sympy.simplify(Se2.calc_Dxi_x_matrix(self.a, i) - + Se2.Dxi_x_matrix(self.a, i)), + sympy.Matrix.zeros(3, 3)) + for i in range(0, 3): + self.assertEqual(sympy.simplify( + Se2.Dxi_exp_x_matrix(self.upsilon_theta, i) - + Se2.calc_Dxi_exp_x_matrix(self.upsilon_theta, i)), + sympy.Matrix.zeros(3, 3)) + self.assertEqual(sympy.simplify( + Se2.Dxi_exp_x_matrix_at_0(i) - + Se2.calc_Dxi_exp_x_matrix_at_0(self.upsilon_theta, i)), + sympy.Matrix.zeros(3, 3)) + + def test_codegen(self): + stream = sophus.cse_codegen(Se2.calc_Dx_exp_x(self.upsilon_theta)) + filename = "cpp_gencode/Se2_Dx_exp_x.cpp" + + # set to true to generate codegen files + if False: + file = open(filename, "w") + for line in stream: + file.write(line) + file.close() + else: + file = open(filename, "r") + file_lines = file.readlines() + for i, line in enumerate(stream): + self.assertEqual(line, file_lines[i]) + file.close() + stream.close + + stream = sophus.cse_codegen(self.a.calc_Dx_this_mul_exp_x_at_0( + self.upsilon_theta)) + filename = "cpp_gencode/Se2_Dx_this_mul_exp_x_at_0.cpp" + # set to true to generate codegen files + if False: + file = open(filename, "w") + for line in stream: + file.write(line) + file.close() + else: + file = open(filename, "r") + file_lines = file.readlines() + for i, line in enumerate(stream): + self.assertEqual(line, file_lines[i]) + file.close() + stream.close + + +if __name__ == '__main__': + unittest.main() diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/se3.py b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/se3.py new file mode 100644 index 0000000..07096b5 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/se3.py @@ -0,0 +1,260 @@ +import sympy +import sys +import unittest +import sophus +import functools + + +class Se3: + """ 3 dimensional group of rigid body transformations """ + + def __init__(self, so3, t): + """ internally represented by a unit quaternion q and a translation + 3-vector """ + assert isinstance(so3, sophus.So3) + assert isinstance(t, sympy.Matrix) + assert t.shape == (3, 1), t.shape + + self.so3 = so3 + self.t = t + + @staticmethod + def exp(v): + """ exponential map """ + upsilon = v[0:3, :] + omega = sophus.Vector3(v[3], v[4], v[5]) + so3 = sophus.So3.exp(omega) + Omega = sophus.So3.hat(omega) + Omega_sq = Omega * Omega + theta = sympy.sqrt(sophus.squared_norm(omega)) + V = (sympy.Matrix.eye(3) + + (1 - sympy.cos(theta)) / (theta**2) * Omega + + (theta - sympy.sin(theta)) / (theta**3) * Omega_sq) + return Se3(so3, V * upsilon) + + def log(self): + + omega = self.so3.log() + theta = sympy.sqrt(sophus.squared_norm(omega)) + Omega = sophus.So3.hat(omega) + + half_theta = 0.5 * theta + + V_inv = sympy.Matrix.eye(3) - 0.5 * Omega + (1 - theta * sympy.cos( + half_theta) / (2 * sympy.sin(half_theta))) / (theta * theta) *\ + (Omega * Omega) + upsilon = V_inv * self.t + return upsilon.col_join(omega) + + def __repr__(self): + return "Se3: [" + repr(self.so3) + " " + repr(self.t) + + def inverse(self): + invR = self.so3.inverse() + return Se3(invR, invR * (-1 * self.t)) + + @staticmethod + def hat(v): + """ R^6 => R^4x4 """ + """ returns 4x4-matrix representation ``Omega`` """ + upsilon = sophus.Vector3(v[0], v[1], v[2]) + omega = sophus.Vector3(v[3], v[4], v[5]) + return sophus.So3.hat(omega).\ + row_join(upsilon).\ + col_join(sympy.Matrix.zeros(1, 4)) + + @staticmethod + def vee(Omega): + """ R^4x4 => R^6 """ + """ returns 6-vector representation of Lie algebra """ + """ This is the inverse of the hat-operator """ + + head = sophus.Vector3(Omega[0,3], Omega[1,3], Omega[2,3]) + tail = sophus.So3.vee(Omega[0:3,0:3]) + upsilon_omega = \ + sophus.Vector6(head[0], head[1], head[2], tail[0], tail[1], tail[2]) + return upsilon_omega + + + def matrix(self): + """ returns matrix representation """ + R = self.so3.matrix() + return (R.row_join(self.t)).col_join(sympy.Matrix(1, 4, [0, 0, 0, 1])) + + def __mul__(self, right): + """ left-multiplication + either rotation concatenation or point-transform """ + if isinstance(right, sympy.Matrix): + assert right.shape == (3, 1), right.shape + return self.so3 * right + self.t + elif isinstance(right, Se3): + r = self.so3 * right.so3 + t = self.t + self.so3 * right.t + return Se3(r, t) + assert False, "unsupported type: {0}".format(type(right)) + + def __getitem__(self, key): + """ We use the following convention [q0, q1, q2, q3, t0, t1, t2] """ + assert (key >= 0 and key < 7) + if key < 4: + return self.so3[key] + else: + return self.t[key - 4] + + @staticmethod + def calc_Dx_exp_x(x): + return sympy.Matrix(7, 6, lambda r, c: + sympy.diff(Se3.exp(x)[r], x[c])) + + @staticmethod + def Dx_exp_x_at_0(): + return sympy.Matrix([[0.0, 0.0, 0.0, 0.5, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.5, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.5], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [1.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.0, 0.0, 0.0]]) + + def calc_Dx_this_mul_exp_x_at_0(self, x): + v = Se3.exp(x) + return sympy.Matrix(7, 6, lambda r, c: + sympy.diff((self * Se3.exp(x))[r], x[c])). \ + subs(x[0], 0).subs(x[1], 0).subs(x[2], 0).\ + subs(x[3], 0).subs(x[4], 0).limit(x[5], 0) + + @staticmethod + def calc_Dx_exp_x_at_0(x): + return Se3.calc_Dx_exp_x(x).subs(x[0], 0).subs(x[1], 0).subs(x[2], 0).\ + subs(x[3], 0).subs(x[4], 0).limit(x[5], 0) + + @staticmethod + def Dxi_x_matrix(x, i): + if i < 4: + return sophus.So3.Dxi_x_matrix(x, i).\ + row_join(sympy.Matrix.zeros(3, 1)).\ + col_join(sympy.Matrix.zeros(1, 4)) + M = sympy.Matrix.zeros(4, 4) + M[i - 4, 3] = 1 + return M + + @staticmethod + def calc_Dxi_x_matrix(x, i): + return sympy.Matrix(4, 4, lambda r, c: + sympy.diff(x.matrix()[r, c], x[i])) + + @staticmethod + def Dxi_exp_x_matrix(x, i): + T = Se3.exp(x) + Dx_exp_x = Se3.calc_Dx_exp_x(x) + l = [Dx_exp_x[j, i] * Se3.Dxi_x_matrix(T, j) for j in range(0, 7)] + return functools.reduce((lambda a, b: a + b), l) + + @staticmethod + def calc_Dxi_exp_x_matrix(x, i): + return sympy.Matrix(4, 4, lambda r, c: + sympy.diff(Se3.exp(x).matrix()[r, c], x[i])) + + @staticmethod + def Dxi_exp_x_matrix_at_0(i): + v = sophus.ZeroVector6() + v[i] = 1 + return Se3.hat(v) + + @staticmethod + def calc_Dxi_exp_x_matrix_at_0(x, i): + return sympy.Matrix(4, 4, lambda r, c: + sympy.diff(Se3.exp(x).matrix()[r, c], x[i]) + ).subs(x[0], 0).subs(x[1], 0).subs(x[2], 0).\ + subs(x[3], 0).subs(x[4], 0).limit(x[5], 0) + + +class TestSe3(unittest.TestCase): + def setUp(self): + upsilon0, upsilon1, upsilon2, omega0, omega1, omega2 = sympy.symbols( + 'upsilon[0], upsilon[1], upsilon[2], omega[0], omega[1], omega[2]', + real=True) + x, v0, v1, v2 = sympy.symbols('q.w() q.x() q.y() q.z()', real=True) + p0, p1, p2 = sympy.symbols('p0 p1 p2', real=True) + t0, t1, t2 = sympy.symbols('t[0] t[1] t[2]', real=True) + v = sophus.Vector3(v0, v1, v2) + self.upsilon_omega = sophus.Vector6( + upsilon0, upsilon1, upsilon2, omega0, omega1, omega2) + self.t = sophus.Vector3(t0, t1, t2) + self.a = Se3(sophus.So3(sophus.Quaternion(x, v)), self.t) + self.p = sophus.Vector3(p0, p1, p2) + + def test_exp_log(self): + for v in [sophus.Vector6(0., 1, 0.5, 2., 1, 0.5), + sophus.Vector6(0.1, 0.1, 0.1, 0., 1, 0.5), + sophus.Vector6(0.01, 0.2, 0.03, 0.01, 0.2, 0.03)]: + w = Se3.exp(v).log() + for i in range(0, 3): + self.assertAlmostEqual(v[i], w[i]) + + def test_matrix(self): + T_foo_bar = Se3.exp(self.upsilon_omega) + Tmat_foo_bar = T_foo_bar.matrix() + point_bar = self.p + p1_foo = T_foo_bar * point_bar + p2_foo = sophus.proj(Tmat_foo_bar * sophus.unproj(point_bar)) + self.assertEqual(sympy.simplify(p1_foo - p2_foo), + sophus.ZeroVector3()) + + def test_derivatives(self): + self.assertEqual(sympy.simplify( + Se3.calc_Dx_exp_x_at_0(self.upsilon_omega) - + Se3.Dx_exp_x_at_0()), + sympy.Matrix.zeros(7, 6)) + + for i in range(0, 7): + self.assertEqual(sympy.simplify(Se3.calc_Dxi_x_matrix(self.a, i) - + Se3.Dxi_x_matrix(self.a, i)), + sympy.Matrix.zeros(4, 4)) + for i in range(0, 6): + self.assertEqual(sympy.simplify( + Se3.Dxi_exp_x_matrix(self.upsilon_omega, i) - + Se3.calc_Dxi_exp_x_matrix(self.upsilon_omega, i)), + sympy.Matrix.zeros(4, 4)) + self.assertEqual(sympy.simplify( + Se3.Dxi_exp_x_matrix_at_0(i) - + Se3.calc_Dxi_exp_x_matrix_at_0(self.upsilon_omega, i)), + sympy.Matrix.zeros(4, 4)) + + def test_codegen(self): + stream = sophus.cse_codegen(self.a.calc_Dx_exp_x(self.upsilon_omega)) + filename = "cpp_gencode/Se3_Dx_exp_x.cpp" + # set to true to generate codegen files + if False: + file = open(filename, "w") + for line in stream: + file.write(line) + file.close() + else: + file = open(filename, "r") + file_lines = file.readlines() + for i, line in enumerate(stream): + self.assertEqual(line, file_lines[i]) + file.close() + stream.close + + stream = sophus.cse_codegen(self.a.calc_Dx_this_mul_exp_x_at_0( + self.upsilon_omega)) + filename = "cpp_gencode/Se3_Dx_this_mul_exp_x_at_0.cpp" + # set to true to generate codegen files + if False: + file = open(filename, "w") + for line in stream: + file.write(line) + file.close() + else: + file = open(filename, "r") + file_lines = file.readlines() + for i, line in enumerate(stream): + self.assertEqual(line, file_lines[i]) + file.close() + stream.close + + +if __name__ == '__main__': + unittest.main() diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/so2.py b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/so2.py new file mode 100644 index 0000000..d4d7cdf --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/so2.py @@ -0,0 +1,186 @@ +import sympy +import sys +import unittest +import sophus +import functools + + +class So2: + """ 2 dimensional group of orthogonal matrices with determinant 1 """ + + def __init__(self, z): + """ internally represented by a unit complex number z """ + self.z = z + + @staticmethod + def exp(theta): + """ exponential map """ + return So2( + sophus.Complex( + sympy.cos(theta), + sympy.sin(theta))) + + def log(self): + """ logarithmic map""" + return sympy.atan2(self.z.imag, self.z.real) + + def __repr__(self): + return "So2:" + repr(self.z) + + @staticmethod + def hat(theta): + return sympy.Matrix([[0, -theta], + [theta, 0]]) + + def matrix(self): + """ returns matrix representation """ + return sympy.Matrix([ + [self.z.real, -self.z.imag], + [self.z.imag, self.z.real]]) + + def __mul__(self, right): + """ left-multiplication + either rotation concatenation or point-transform """ + if isinstance(right, sympy.Matrix): + assert right.shape == (2, 1), right.shape + return self.matrix() * right + elif isinstance(right, So2): + return So2(self.z * right.z) + assert False, "unsupported type: {0}".format(type(right)) + + def __getitem__(self, key): + return self.z[key] + + @staticmethod + def calc_Dx_exp_x(x): + return sympy.Matrix(2, 1, lambda r, c: + sympy.diff(So2.exp(x)[r], x)) + + @staticmethod + def Dx_exp_x_at_0(): + return sympy.Matrix([0, 1]) + + @staticmethod + def calc_Dx_exp_x_at_0(x): + return So2.calc_Dx_exp_x(x).limit(x, 0) + + def calc_Dx_this_mul_exp_x_at_0(self, x): + return sympy.Matrix(2, 1, lambda r, c: + sympy.diff((self * So2.exp(x))[r], x))\ + .limit(x, 0) + + @staticmethod + def Dxi_x_matrix(x, i): + if i == 0: + return sympy.Matrix([[1, 0], + [0, 1]]) + if i == 1: + return sympy.Matrix([[0, -1], + [1, 0]]) + + @staticmethod + def calc_Dxi_x_matrix(x, i): + return sympy.Matrix(2, 2, lambda r, c: + sympy.diff(x.matrix()[r, c], x[i])) + + @staticmethod + def Dx_exp_x_matrix(x): + R = So2.exp(x) + Dx_exp_x = So2.calc_Dx_exp_x(x) + l = [Dx_exp_x[j] * So2.Dxi_x_matrix(R, j) for j in [0, 1]] + return functools.reduce((lambda a, b: a + b), l) + + @staticmethod + def calc_Dx_exp_x_matrix(x): + return sympy.Matrix(2, 2, lambda r, c: + sympy.diff(So2.exp(x).matrix()[r, c], x)) + + @staticmethod + def Dx_exp_x_matrix_at_0(): + return So2.hat(1) + + @staticmethod + def calc_Dx_exp_x_matrix_at_0(x): + return sympy.Matrix(2, 2, lambda r, c: + sympy.diff(So2.exp(x).matrix()[r, c], x) + ).limit(x, 0) + + +class TestSo2(unittest.TestCase): + def setUp(self): + self.theta = sympy.symbols( + 'theta', real=True) + x, y = sympy.symbols('c[0] c[1]', real=True) + p0, p1 = sympy.symbols('p0 p1', real=True) + self.a = So2(sophus.Complex(x, y)) + self.p = sophus.Vector2(p0, p1) + + def test_exp_log(self): + for theta in [0., 0.5, 0.1]: + w = So2.exp(theta).log() + self.assertAlmostEqual(theta, w) + + def test_matrix(self): + R_foo_bar = So2.exp(self.theta) + Rmat_foo_bar = R_foo_bar.matrix() + point_bar = self.p + p1_foo = R_foo_bar * point_bar + p2_foo = Rmat_foo_bar * point_bar + self.assertEqual(sympy.simplify(p1_foo - p2_foo), + sophus.ZeroVector2()) + + def test_derivatives(self): + self.assertEqual(sympy.simplify(So2.calc_Dx_exp_x_at_0(self.theta) - + So2.Dx_exp_x_at_0()), + sympy.Matrix.zeros(2, 1)) + for i in [0, 1]: + self.assertEqual(sympy.simplify(So2.calc_Dxi_x_matrix(self.a, i) - + So2.Dxi_x_matrix(self.a, i)), + sympy.Matrix.zeros(2, 2)) + + self.assertEqual(sympy.simplify( + So2.Dx_exp_x_matrix(self.theta) - + So2.calc_Dx_exp_x_matrix(self.theta)), + sympy.Matrix.zeros(2, 2)) + self.assertEqual(sympy.simplify( + So2.Dx_exp_x_matrix_at_0() - + So2.calc_Dx_exp_x_matrix_at_0(self.theta)), + sympy.Matrix.zeros(2, 2)) + + def test_codegen(self): + stream = sophus.cse_codegen(So2.calc_Dx_exp_x(self.theta)) + filename = "cpp_gencode/So2_Dx_exp_x.cpp" + # set to true to generate codegen files + if False: + file = open(filename, "w") + for line in stream: + file.write(line) + file.close() + else: + file = open(filename, "r") + file_lines = file.readlines() + for i, line in enumerate(stream): + self.assertEqual(line, file_lines[i]) + file.close() + stream.close + + stream = sophus.cse_codegen( + self.a.calc_Dx_this_mul_exp_x_at_0(self.theta)) + filename = "cpp_gencode/So2_Dx_this_mul_exp_x_at_0.cpp" + # set to true to generate codegen files + if False: + file = open(filename, "w") + for line in stream: + file.write(line) + file.close() + else: + file = open(filename, "r") + file_lines = file.readlines() + for i, line in enumerate(stream): + self.assertEqual(line, file_lines[i]) + file.close() + stream.close + + +if __name__ == '__main__': + unittest.main() diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/so3.py b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/so3.py new file mode 100644 index 0000000..3532c2c --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/so3.py @@ -0,0 +1,253 @@ +import sympy +import sys +import unittest +import sophus +import functools + + +class So3: + """ 3 dimensional group of orthogonal matrices with determinant 1 """ + + def __init__(self, q): + """ internally represented by a unit quaternion q """ + self.q = q + + @staticmethod + def exp(v): + """ exponential map """ + theta_sq = sophus.squared_norm(v) + theta = sympy.sqrt(theta_sq) + return So3( + sophus.Quaternion( + sympy.cos(0.5 * theta), + sympy.sin(0.5 * theta) / theta * v)) + + def log(self): + """ logarithmic map""" + n = sympy.sqrt(sophus.squared_norm(self.q.vec)) + return 2 * sympy.atan(n / self.q.real) / n * self.q.vec + + def __repr__(self): + return "So3:" + repr(self.q) + + def inverse(self): + return So3(self.q.conj()) + + @staticmethod + def hat(o): + return sympy.Matrix([[0, -o[2], o[1]], + [o[2], 0, -o[0]], + [-o[1], o[0], 0]]) + + """vee-operator + + It takes the 3x3-matrix representation ``Omega`` and maps it to the + corresponding vector representation of Lie algebra. + + This is the inverse of the hat-operator, see above. + + Precondition: ``Omega`` must have the following structure: + + | 0 -c b | + | c 0 -a | + | -b a 0 | + """ + @staticmethod + def vee(Omega): + v = sophus.Vector3(Omega.row(2).col(1), Omega.row(0).col(2), Omega.row(1).col(0)) + return v + + def matrix(self): + """ returns matrix representation """ + return sympy.Matrix([[ + 1 - 2 * self.q.vec[1]**2 - 2 * self.q.vec[2]**2, + 2 * self.q.vec[0] * self.q.vec[1] - + 2 * self.q.vec[2] * self.q[3], + 2 * self.q.vec[0] * self.q.vec[2] + + 2 * self.q.vec[1] * self.q[3] + ], [ + 2 * self.q.vec[0] * self.q.vec[1] + + 2 * self.q.vec[2] * self.q[3], + 1 - 2 * self.q.vec[0]**2 - 2 * self.q.vec[2]**2, + 2 * self.q.vec[1] * self.q.vec[2] - + 2 * self.q.vec[0] * self.q[3] + ], [ + 2 * self.q.vec[0] * self.q.vec[2] - + 2 * self.q.vec[1] * self.q[3], + 2 * self.q.vec[1] * self.q.vec[2] + + 2 * self.q.vec[0] * self.q[3], + 1 - 2 * self.q.vec[0]**2 - 2 * self.q.vec[1]**2 + ]]) + + def __mul__(self, right): + """ left-multiplication + either rotation concatenation or point-transform """ + if isinstance(right, sympy.Matrix): + assert right.shape == (3, 1), right.shape + return (self.q * sophus.Quaternion(0, right) * self.q.conj()).vec + elif isinstance(right, So3): + return So3(self.q * right.q) + assert False, "unsupported type: {0}".format(type(right)) + + def __getitem__(self, key): + return self.q[key] + + @staticmethod + def calc_Dx_exp_x(x): + return sympy.Matrix(4, 3, lambda r, c: + sympy.diff(So3.exp(x)[r], x[c])) + + @staticmethod + def Dx_exp_x_at_0(): + return sympy.Matrix([[0.5, 0.0, 0.0], + [0.0, 0.5, 0.0], + [0.0, 0.0, 0.5], + [0.0, 0.0, 0.0]]) + + @staticmethod + def calc_Dx_exp_x_at_0(x): + return So3.calc_Dx_exp_x(x).subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) + + def calc_Dx_this_mul_exp_x_at_0(self, x): + return sympy.Matrix(4, 3, lambda r, c: + sympy.diff((self * So3.exp(x))[r], x[c]))\ + .subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) + + def calc_Dx_exp_x_mul_this_at_0(self, x): + return sympy.Matrix(3, 4, lambda r, c: + sympy.diff((self * So3.exp(x))[c], x[r, 0]))\ + .subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) + + @staticmethod + def Dxi_x_matrix(x, i): + if i == 0: + return sympy.Matrix([[0, 2 * x[1], 2 * x[2]], + [2 * x[1], -4 * x[0], -2 * x[3]], + [2 * x[2], 2 * x[3], -4 * x[0]]]) + if i == 1: + return sympy.Matrix([[-4 * x[1], 2 * x[0], 2 * x[3]], + [2 * x[0], 0, 2 * x[2]], + [-2 * x[3], 2 * x[2], -4 * x[1]]]) + if i == 2: + return sympy.Matrix([[-4 * x[2], -2 * x[3], 2 * x[0]], + [2 * x[3], -4 * x[2], 2 * x[1]], + [2 * x[0], 2 * x[1], 0]]) + if i == 3: + return sympy.Matrix([[0, -2 * x[2], 2 * x[1]], + [2 * x[2], 0, -2 * x[0]], + [-2 * x[1], 2 * x[0], 0]]) + + @staticmethod + def calc_Dxi_x_matrix(x, i): + return sympy.Matrix(3, 3, lambda r, c: + sympy.diff(x.matrix()[r, c], x[i])) + + @staticmethod + def Dxi_exp_x_matrix(x, i): + R = So3.exp(x) + Dx_exp_x = So3.calc_Dx_exp_x(x) + l = [Dx_exp_x[j, i] * So3.Dxi_x_matrix(R, j) for j in [0, 1, 2, 3]] + return functools.reduce((lambda a, b: a + b), l) + + @staticmethod + def calc_Dxi_exp_x_matrix(x, i): + return sympy.Matrix(3, 3, lambda r, c: + sympy.diff(So3.exp(x).matrix()[r, c], x[i])) + + @staticmethod + def Dxi_exp_x_matrix_at_0(i): + v = sophus.ZeroVector3() + v[i] = 1 + return So3.hat(v) + + @staticmethod + def calc_Dxi_exp_x_matrix_at_0(x, i): + return sympy.Matrix(3, 3, lambda r, c: + sympy.diff(So3.exp(x).matrix()[r, c], x[i]) + ).subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) + + +class TestSo3(unittest.TestCase): + def setUp(self): + omega0, omega1, omega2 = sympy.symbols( + 'omega[0], omega[1], omega[2]', real=True) + x, v0, v1, v2 = sympy.symbols('q.w() q.x() q.y() q.z()', real=True) + p0, p1, p2 = sympy.symbols('p0 p1 p2', real=True) + v = sophus.Vector3(v0, v1, v2) + self.omega = sophus.Vector3(omega0, omega1, omega2) + self.a = So3(sophus.Quaternion(x, v)) + self.p = sophus.Vector3(p0, p1, p2) + + def test_exp_log(self): + for o in [sophus.Vector3(0., 1, 0.5), + sophus.Vector3(0.1, 0.1, 0.1), + sophus.Vector3(0.01, 0.2, 0.03)]: + w = So3.exp(o).log() + for i in range(0, 3): + self.assertAlmostEqual(o[i], w[i]) + + def test_matrix(self): + R_foo_bar = So3.exp(self.omega) + Rmat_foo_bar = R_foo_bar.matrix() + point_bar = self.p + p1_foo = R_foo_bar * point_bar + p2_foo = Rmat_foo_bar * point_bar + self.assertEqual(sympy.simplify(p1_foo - p2_foo), + sophus.ZeroVector3()) + + def test_derivatives(self): + self.assertEqual(sympy.simplify(So3.calc_Dx_exp_x_at_0(self.omega) - + So3.Dx_exp_x_at_0()), + sympy.Matrix.zeros(4, 3)) + + for i in [0, 1, 2, 3]: + self.assertEqual(sympy.simplify(So3.calc_Dxi_x_matrix(self.a, i) - + So3.Dxi_x_matrix(self.a, i)), + sympy.Matrix.zeros(3, 3)) + for i in [0, 1, 2]: + self.assertEqual(sympy.simplify( + So3.Dxi_exp_x_matrix(self.omega, i) - + So3.calc_Dxi_exp_x_matrix(self.omega, i)), + sympy.Matrix.zeros(3, 3)) + self.assertEqual(sympy.simplify( + So3.Dxi_exp_x_matrix_at_0(i) - + So3.calc_Dxi_exp_x_matrix_at_0(self.omega, i)), + sympy.Matrix.zeros(3, 3)) + + def test_codegen(self): + stream = sophus.cse_codegen(So3.calc_Dx_exp_x(self.omega)) + filename = "cpp_gencode/So3_Dx_exp_x.cpp" + # set to true to generate codegen files + if False: + file = open(filename, "w") + for line in stream: + file.write(line) + file.close() + else: + file = open(filename, "r") + file_lines = file.readlines() + for i, line in enumerate(stream): + self.assertEqual(line, file_lines[i]) + file.close() + stream.close + + stream = sophus.cse_codegen( + self.a.calc_Dx_this_mul_exp_x_at_0(self.omega)) + filename = "cpp_gencode/So3_Dx_this_mul_exp_x_at_0.cpp" + # set to true to generate codegen files + if False: + file = open(filename, "w") + for line in stream: + file.write(line) + file.close() + else: + file = open(filename, "r") + file_lines = file.readlines() + for i, line in enumerate(stream): + self.assertEqual(line, file_lines[i]) + file.close() + stream.close + + +if __name__ == '__main__': + unittest.main() diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/so3_codegen.py b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/so3_codegen.py new file mode 100644 index 0000000..2505e11 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/py/sophus/so3_codegen.py @@ -0,0 +1,3 @@ +import sophus +from sympy.utilities.codegen import codegen +import sympy diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/rst-dir/conf.py b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/rst-dir/conf.py new file mode 100644 index 0000000..89241bd --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/rst-dir/conf.py @@ -0,0 +1,54 @@ +# Configuration file for the Sphinx documentation builder. +# +# This file only contains a selection of the most common options. For a full +# list see the documentation: +# http://www.sphinx-doc.org/en/master/config + +# -- Path setup -------------------------------------------------------------- + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +# +import os +import sys +sys.path.insert(0, os.path.abspath('../py')) + + +sys.path.insert(1, os.path.abspath('../doxyrest_b/doxyrest/sphinx')) +extensions = ['doxyrest', 'cpplexer', 'sphinx.ext.autodoc'] + +# -- Project information ----------------------------------------------------- + +project = 'Sophus' +copyright = '2019, Hauke Strasdat' +author = 'Hauke Strasdat' + + +# Tell sphinx what the primary language being documented is. +primary_domain = 'cpp' + +# Tell sphinx what the pygments highlight language should be. +highlight_language = 'cpp' + + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +# This pattern also affects html_static_path and html_extra_path. +exclude_patterns = [] + + +# -- Options for HTML output ------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +# +html_theme = "sphinx_rtd_theme" + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ['_static'] diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/rst-dir/page_index.rst b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/rst-dir/page_index.rst new file mode 100644 index 0000000..9955aca --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/rst-dir/page_index.rst @@ -0,0 +1,9 @@ +Sophus - Lie groups for 2d/3d Geometry +======================================= + +.. toctree:: + :maxdepth: 2 + :caption: Contents: + + GitHub Page + pysophus \ No newline at end of file diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/rst-dir/pysophus.rst b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/rst-dir/pysophus.rst new file mode 100644 index 0000000..3e532a0 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/rst-dir/pysophus.rst @@ -0,0 +1,23 @@ +Python API +========== + +.. automodule:: sophus.matrix + :members: + +.. automodule:: sophus.complex + :members: + +.. automodule:: sophus.quaternion + :members: + +.. automodule:: sophus.so2 + :members: + +.. automodule:: sophus.so3 + :members: + +.. automodule:: sophus.se2 + :members: + +.. automodule:: sophus.se3 + :members: \ No newline at end of file diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/run_format.sh b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/run_format.sh new file mode 100755 index 0000000..a512c1d --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/run_format.sh @@ -0,0 +1 @@ +find . -type d \( -path ./py -o -path ./doxyrest_b -o -path "./*/CMakeFiles/*" \) -prune -o -iname *.hpp -o -iname *.cpp -print | xargs clang-format -i diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/scripts/install_docs_deps.sh b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/scripts/install_docs_deps.sh new file mode 100755 index 0000000..c2a1b7f --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/scripts/install_docs_deps.sh @@ -0,0 +1,21 @@ +#!/bin/bash + +set -x # echo on +set -e # exit on error + +sudo apt-get -qq update +sudo apt-get install doxygen liblua5.3-dev +pip3 install 'sphinx==2.0.1' +pip3 install sphinx_rtd_theme +pip3 install sympy + +git clone https://github.com/vovkos/doxyrest_b +cd doxyrest_b +git reset --hard ad45c064d1199e71b8cae5aa66d4251c4228b958 +git submodule update --init +mkdir build +cd build +cmake .. +cmake --build . + +cd ../.. \ No newline at end of file diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/scripts/install_linux_deps.sh b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/scripts/install_linux_deps.sh new file mode 100755 index 0000000..e72ba76 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/scripts/install_linux_deps.sh @@ -0,0 +1,25 @@ +#!/bin/bash + +set -x # echo on +set -e # exit on error + +cmake --version + +sudo apt-get -qq update +sudo apt-get install gfortran libc++-dev libgoogle-glog-dev libatlas-base-dev libsuitesparse-dev +wget https://gitlab.com/libeigen/eigen/-/archive/3.3.4/eigen-3.3.4.tar.bz2 +tar xvf eigen-3.3.4.tar.bz2 +mkdir build-eigen +cd build-eigen +cmake ../eigen-3.3.4 -DEIGEN_DEFAULT_TO_ROW_MAJOR=$ROW_MAJOR_DEFAULT +sudo make install +git clone https://ceres-solver.googlesource.com/ceres-solver ceres-solver +cd ceres-solver +git reset --hard afe93546b67cee0ad205fe8044325646ed5deea9 +mkdir build +cd build +ccache -M 50G +ccache -s +cmake -DCXX11=On -DCMAKE_CXX_COMPILER_LAUNCHER=ccache -DOPENMP=Off .. +make -j3 +sudo make install diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/scripts/install_osx_deps.sh b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/scripts/install_osx_deps.sh new file mode 100755 index 0000000..01894b6 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/scripts/install_osx_deps.sh @@ -0,0 +1,21 @@ +#!/bin/bash + +set -x # echo on +set -e # exit on error +brew update +brew install eigen +brew install glog +brew install suite-sparse +brew install ccache +export PATH="/usr/local/opt/ccache/libexec:$PATH" +whereis ccache +git clone https://ceres-solver.googlesource.com/ceres-solver ceres-solver +cd ceres-solver +git reset --hard afe93546b67cee0ad205fe8044325646ed5deea9 +mkdir build +cd build +ccache -M 50G +ccache -s +cmake -DCXX11=On -DCMAKE_CXX_COMPILER_LAUNCHER=ccache .. +make -j3 +make install \ No newline at end of file diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/scripts/run_cpp_tests.sh b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/scripts/run_cpp_tests.sh new file mode 100755 index 0000000..4304b56 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/scripts/run_cpp_tests.sh @@ -0,0 +1,11 @@ +#!/bin/bash + +set -x # echo on +set -e # exit on error + +mkdir build +cd build +pwd +cmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccache -DCMAKE_BUILD_TYPE=$BUILD_TYPE .. +make -j2 +make CTEST_OUTPUT_ON_FAILURE=1 test diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/average.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/average.hpp new file mode 100644 index 0000000..107255f --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/average.hpp @@ -0,0 +1,231 @@ +/// @file +/// Calculation of biinvariant means. + +#ifndef SOPHUS_AVERAGE_HPP +#define SOPHUS_AVERAGE_HPP + +#include "common.hpp" +#include "rxso2.hpp" +#include "rxso3.hpp" +#include "se2.hpp" +#include "se3.hpp" +#include "sim2.hpp" +#include "sim3.hpp" +#include "so2.hpp" +#include "so3.hpp" + +namespace Sophus { + +/// Calculates mean iteratively. +/// +/// Returns ``nullopt`` if it does not converge. +/// +template +optional iterativeMean( + SequenceContainer const& foo_Ts_bar, int max_num_iterations) { + size_t N = foo_Ts_bar.size(); + SOPHUS_ENSURE(N >= 1, "N must be >= 1."); + + using Group = typename SequenceContainer::value_type; + using Scalar = typename Group::Scalar; + using Tangent = typename Group::Tangent; + + // This implements the algorithm in the beginning of Sec. 4.2 in + // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. + Group foo_T_average = foo_Ts_bar.front(); + Scalar w = Scalar(1. / N); + for (int i = 0; i < max_num_iterations; ++i) { + Tangent average; + setToZero(average); + for (Group const& foo_T_bar : foo_Ts_bar) { + average += w * (foo_T_average.inverse() * foo_T_bar).log(); + } + Group foo_T_newaverage = foo_T_average * Group::exp(average); + if (squaredNorm( + (foo_T_newaverage.inverse() * foo_T_average).log()) < + Constants::epsilon()) { + return foo_T_newaverage; + } + + foo_T_average = foo_T_newaverage; + } + // LCOV_EXCL_START + return nullopt; + // LCOV_EXCL_STOP +} + +#ifdef DOXYGEN_SHOULD_SKIP_THIS +/// Mean implementation for any Lie group. +template +optional average( + SequenceContainer const& foo_Ts_bar); +#else + +// Mean implementation for SO(2). +template +enable_if_t< + std::is_same >::value, + optional > +average(SequenceContainer const& foo_Ts_bar) { + // This implements rotational part of Proposition 12 from Sec. 6.2 of + // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. + size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); + SOPHUS_ENSURE(N >= 1, "N must be >= 1."); + SO2 foo_T_average = foo_Ts_bar.front(); + Scalar w = Scalar(1. / N); + + Scalar average(0); + for (SO2 const& foo_T_bar : foo_Ts_bar) { + average += w * (foo_T_average.inverse() * foo_T_bar).log(); + } + return foo_T_average * SO2::exp(average); +} + +// Mean implementation for RxSO(2). +template +enable_if_t< + std::is_same >::value, + optional > +average(SequenceContainer const& foo_Ts_bar) { + size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); + SOPHUS_ENSURE(N >= 1, "N must be >= 1."); + RxSO2 foo_T_average = foo_Ts_bar.front(); + Scalar w = Scalar(1. / N); + + Vector2 average(Scalar(0), Scalar(0)); + for (RxSO2 const& foo_T_bar : foo_Ts_bar) { + average += w * (foo_T_average.inverse() * foo_T_bar).log(); + } + return foo_T_average * RxSO2::exp(average); +} + +namespace details { +template +void getQuaternion(T const&); + +template +Eigen::Quaternion getUnitQuaternion(SO3 const& R) { + return R.unit_quaternion(); +} + +template +Eigen::Quaternion getUnitQuaternion(RxSO3 const& sR) { + return sR.so3().unit_quaternion(); +} + +template +Eigen::Quaternion averageUnitQuaternion( + SequenceContainer const& foo_Ts_bar) { + // This: http://stackoverflow.com/a/27410865/1221742 + size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); + SOPHUS_ENSURE(N >= 1, "N must be >= 1."); + Eigen::Matrix Q(4, N); + int i = 0; + Scalar w = Scalar(1. / N); + for (auto const& foo_T_bar : foo_Ts_bar) { + Q.col(i) = w * details::getUnitQuaternion(foo_T_bar).coeffs(); + ++i; + } + + Eigen::Matrix QQt = Q * Q.transpose(); + // TODO: Figure out why we can't use SelfAdjointEigenSolver here. + Eigen::EigenSolver > es(QQt); + + std::complex max_eigenvalue = es.eigenvalues()[0]; + Eigen::Matrix, 4, 1> max_eigenvector = + es.eigenvectors().col(0); + + for (int i = 1; i < 4; i++) { + if (std::norm(es.eigenvalues()[i]) > std::norm(max_eigenvalue)) { + max_eigenvalue = es.eigenvalues()[i]; + max_eigenvector = es.eigenvectors().col(i); + } + } + Eigen::Quaternion quat; + quat.coeffs() << // + max_eigenvector[0].real(), // + max_eigenvector[1].real(), // + max_eigenvector[2].real(), // + max_eigenvector[3].real(); + return quat; +} +} // namespace details + +// Mean implementation for SO(3). +// +// TODO: Detect degenerated cases and return nullopt. +template +enable_if_t< + std::is_same >::value, + optional > +average(SequenceContainer const& foo_Ts_bar) { + return SO3(details::averageUnitQuaternion(foo_Ts_bar)); +} + +// Mean implementation for R x SO(3). +template +enable_if_t< + std::is_same >::value, + optional > +average(SequenceContainer const& foo_Ts_bar) { + size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); + + SOPHUS_ENSURE(N >= 1, "N must be >= 1."); + Scalar scale_sum = Scalar(0); + using std::exp; + using std::log; + for (RxSO3 const& foo_T_bar : foo_Ts_bar) { + scale_sum += log(foo_T_bar.scale()); + } + return RxSO3(exp(scale_sum / Scalar(N)), + SO3(details::averageUnitQuaternion(foo_Ts_bar))); +} + +template +enable_if_t< + std::is_same >::value, + optional > +average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { + // TODO: Implement Proposition 12 from Sec. 6.2 of + // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. + return iterativeMean(foo_Ts_bar, max_num_iterations); +} + +template +enable_if_t< + std::is_same >::value, + optional > +average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { + return iterativeMean(foo_Ts_bar, max_num_iterations); +} + +template +enable_if_t< + std::is_same >::value, + optional > +average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { + return iterativeMean(foo_Ts_bar, max_num_iterations); +} + +template +enable_if_t< + std::is_same >::value, + optional > +average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { + return iterativeMean(foo_Ts_bar, max_num_iterations); +} + +#endif // DOXYGEN_SHOULD_SKIP_THIS + +} // namespace Sophus + +#endif // SOPHUS_AVERAGE_HPP diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/common.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/common.hpp new file mode 100644 index 0000000..e751e48 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/common.hpp @@ -0,0 +1,178 @@ +/// @file +/// Common functionality. + +#ifndef SOPHUS_COMMON_HPP +#define SOPHUS_COMMON_HPP + +#include +#include +#include +#include +#include + +#include + +#if !defined(SOPHUS_DISABLE_ENSURES) +#include "formatstring.hpp" +#endif //!defined(SOPHUS_DISABLE_ENSURES) + +// following boost's assert.hpp +#undef SOPHUS_ENSURE + +// ENSURES are similar to ASSERTS, but they are always checked for (including in +// release builds). At the moment there are no ASSERTS in Sophus which should +// only be used for checks which are performance critical. + +#ifdef __GNUC__ +#define SOPHUS_FUNCTION __PRETTY_FUNCTION__ +#elif (_MSC_VER >= 1310) +#define SOPHUS_FUNCTION __FUNCTION__ +#else +#define SOPHUS_FUNCTION "unknown" +#endif + +// Make sure this compiles with older versions of Eigen which do not have +// EIGEN_DEVICE_FUNC defined. +#ifndef EIGEN_DEVICE_FUNC +#define EIGEN_DEVICE_FUNC +#endif + +// NVCC on windows has issues with defaulting the Map specialization constructors, so +// special case that specific platform case. +#if defined(_MSC_VER) && defined(__CUDACC__) +#define SOPHUS_WINDOW_NVCC_FALLBACK +#endif + +#define SOPHUS_FUNC EIGEN_DEVICE_FUNC + +#if defined(SOPHUS_DISABLE_ENSURES) + +#define SOPHUS_ENSURE(expr, ...) ((void)0) + +#elif defined(SOPHUS_ENABLE_ENSURE_HANDLER) + +namespace Sophus { +void ensureFailed(char const* function, char const* file, int line, + char const* description); +} + +#define SOPHUS_ENSURE(expr, ...) \ + ((expr) ? ((void)0) \ + : ::Sophus::ensureFailed( \ + SOPHUS_FUNCTION, __FILE__, __LINE__, \ + Sophus::details::FormatString(__VA_ARGS__).c_str())) +#else +// LCOV_EXCL_START + +namespace Sophus { +template +SOPHUS_FUNC void defaultEnsure(char const* function, char const* file, int line, + char const* description, Args&&... args) { + std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n", + function, file, line); +#ifdef __CUDACC__ + std::printf("%s", description); +#else + std::cout << details::FormatString(description, std::forward(args)...) + << std::endl; + std::abort(); +#endif +} +} // namespace Sophus + +// LCOV_EXCL_STOP +#define SOPHUS_ENSURE(expr, ...) \ + ((expr) ? ((void)0) \ + : Sophus::defaultEnsure(SOPHUS_FUNCTION, __FILE__, __LINE__, \ + ##__VA_ARGS__)) +#endif + +namespace Sophus { + +template +struct Constants { + SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); } + + SOPHUS_FUNC static Scalar epsilonSqrt() { + using std::sqrt; + return sqrt(epsilon()); + } + + SOPHUS_FUNC static Scalar pi() { + return Scalar(3.141592653589793238462643383279502884); + } +}; + +template <> +struct Constants { + SOPHUS_FUNC static float constexpr epsilon() { + return static_cast(1e-5); + } + + SOPHUS_FUNC static float epsilonSqrt() { return std::sqrt(epsilon()); } + + SOPHUS_FUNC static float constexpr pi() { + return 3.141592653589793238462643383279502884f; + } +}; + +/// Nullopt type of lightweight optional class. +struct nullopt_t { + explicit constexpr nullopt_t() {} +}; + +constexpr nullopt_t nullopt{}; + +/// Lightweight optional implementation which requires ``T`` to have a +/// default constructor. +/// +/// TODO: Replace with std::optional once Sophus moves to c++17. +/// +template +class optional { + public: + optional() : is_valid_(false) {} + + optional(nullopt_t) : is_valid_(false) {} + + optional(T const& type) : type_(type), is_valid_(true) {} + + explicit operator bool() const { return is_valid_; } + + T const* operator->() const { + SOPHUS_ENSURE(is_valid_, "must be valid"); + return &type_; + } + + T* operator->() { + SOPHUS_ENSURE(is_valid_, "must be valid"); + return &type_; + } + + T const& operator*() const { + SOPHUS_ENSURE(is_valid_, "must be valid"); + return type_; + } + + T& operator*() { + SOPHUS_ENSURE(is_valid_, "must be valid"); + return type_; + } + + private: + T type_; + bool is_valid_; +}; + +template +using enable_if_t = typename std::enable_if::type; + +template +struct IsUniformRandomBitGenerator { + static const bool value = std::is_unsigned::value && + std::is_unsigned::value && + std::is_unsigned::value; +}; +} // namespace Sophus + +#endif // SOPHUS_COMMON_HPP diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/example_ensure_handler.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/example_ensure_handler.cpp new file mode 100644 index 0000000..5acf26a --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/example_ensure_handler.cpp @@ -0,0 +1,14 @@ +#include "common.hpp" + +#include +#include + +namespace Sophus { +void ensureFailed(char const* function, char const* file, int line, + char const* description) { + std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n", + file, function, line); + std::printf("Description: %s\n", description); + std::abort(); +} +} // namespace Sophus diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/formatstring.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/formatstring.hpp new file mode 100644 index 0000000..65d2308 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/formatstring.hpp @@ -0,0 +1,72 @@ +/// @file +/// FormatString functionality + +#ifndef SOPHUS_FORMATSTRING_HPP +#define SOPHUS_FORMATSTRING_HPP + +#include + +namespace Sophus { +namespace details { + +// Following: http://stackoverflow.com/a/22759544 +template +class IsStreamable { + private: + template + static auto test(int) + -> decltype(std::declval() << std::declval(), + std::true_type()); + + template + static auto test(...) -> std::false_type; + + public: + static bool const value = decltype(test(0))::value; +}; + +template +class ArgToStream { + public: + static void impl(std::stringstream& stream, T&& arg) { + stream << std::forward(arg); + } +}; + +inline void FormatStream(std::stringstream& stream, char const* text) { + stream << text; + return; +} + +// Following: http://en.cppreference.com/w/cpp/language/parameter_pack +template +void FormatStream(std::stringstream& stream, char const* text, T&& arg, + Args&&... args) { + static_assert(IsStreamable::value, + "One of the args has no ostream overload!"); + for (; *text != '\0'; ++text) { + if (*text == '%') { + ArgToStream::impl(stream, std::forward(arg)); + FormatStream(stream, text + 1, std::forward(args)...); + return; + } + stream << *text; + } + stream << "\nFormat-Warning: There are " << sizeof...(Args) + 1 + << " args unused."; + return; +} + +template +std::string FormatString(char const* text, Args&&... args) { + std::stringstream stream; + FormatStream(stream, text, std::forward(args)...); + return stream.str(); +} + +inline std::string FormatString() { return std::string(); } + +} // namespace details +} // namespace Sophus + +#endif //SOPHUS_FORMATSTRING_HPP diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/geometry.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/geometry.hpp new file mode 100644 index 0000000..1a4f5e2 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/geometry.hpp @@ -0,0 +1,179 @@ +/// @file +/// Transformations between poses and hyperplanes. + +#ifndef GEOMETRY_HPP +#define GEOMETRY_HPP + +#include "se2.hpp" +#include "se3.hpp" +#include "so2.hpp" +#include "so3.hpp" +#include "types.hpp" + +namespace Sophus { + +/// Takes in a rotation ``R_foo_plane`` and returns the corresponding line +/// normal along the y-axis (in reference frame ``foo``). +/// +template +Vector2 normalFromSO2(SO2 const& R_foo_line) { + return R_foo_line.matrix().col(1); +} + +/// Takes in line normal in reference frame foo and constructs a corresponding +/// rotation matrix ``R_foo_line``. +/// +/// Precondition: ``normal_foo`` must not be close to zero. +/// +template +SO2 SO2FromNormal(Vector2 normal_foo) { + SOPHUS_ENSURE(normal_foo.squaredNorm() > Constants::epsilon(), "%", + normal_foo.transpose()); + normal_foo.normalize(); + return SO2(normal_foo.y(), -normal_foo.x()); +} + +/// Takes in a rotation ``R_foo_plane`` and returns the corresponding plane +/// normal along the z-axis +/// (in reference frame ``foo``). +/// +template +Vector3 normalFromSO3(SO3 const& R_foo_plane) { + return R_foo_plane.matrix().col(2); +} + +/// Takes in plane normal in reference frame foo and constructs a corresponding +/// rotation matrix ``R_foo_plane``. +/// +/// Note: The ``plane`` frame is defined as such that the normal points along +/// the positive z-axis. One can specify hints for the x-axis and y-axis +/// of the ``plane`` frame. +/// +/// Preconditions: +/// - ``normal_foo``, ``xDirHint_foo``, ``yDirHint_foo`` must not be close to +/// zero. +/// - ``xDirHint_foo`` and ``yDirHint_foo`` must be approx. perpendicular. +/// +template +Matrix3 rotationFromNormal(Vector3 const& normal_foo, + Vector3 xDirHint_foo = Vector3(T(1), T(0), + T(0)), + Vector3 yDirHint_foo = Vector3(T(0), T(1), + T(0))) { + SOPHUS_ENSURE(xDirHint_foo.dot(yDirHint_foo) < Constants::epsilon(), + "xDirHint (%) and yDirHint (%) must be perpendicular.", + xDirHint_foo.transpose(), yDirHint_foo.transpose()); + using std::abs; + using std::sqrt; + T const xDirHint_foo_sqr_length = xDirHint_foo.squaredNorm(); + T const yDirHint_foo_sqr_length = yDirHint_foo.squaredNorm(); + T const normal_foo_sqr_length = normal_foo.squaredNorm(); + SOPHUS_ENSURE(xDirHint_foo_sqr_length > Constants::epsilon(), "%", + xDirHint_foo.transpose()); + SOPHUS_ENSURE(yDirHint_foo_sqr_length > Constants::epsilon(), "%", + yDirHint_foo.transpose()); + SOPHUS_ENSURE(normal_foo_sqr_length > Constants::epsilon(), "%", + normal_foo.transpose()); + + Matrix3 basis_foo; + basis_foo.col(2) = normal_foo; + + if (abs(xDirHint_foo_sqr_length - T(1)) > Constants::epsilon()) { + xDirHint_foo.normalize(); + } + if (abs(yDirHint_foo_sqr_length - T(1)) > Constants::epsilon()) { + yDirHint_foo.normalize(); + } + if (abs(normal_foo_sqr_length - T(1)) > Constants::epsilon()) { + basis_foo.col(2).normalize(); + } + + T abs_x_dot_z = abs(basis_foo.col(2).dot(xDirHint_foo)); + T abs_y_dot_z = abs(basis_foo.col(2).dot(yDirHint_foo)); + if (abs_x_dot_z < abs_y_dot_z) { + // basis_foo.z and xDirHint are far from parallel. + basis_foo.col(1) = basis_foo.col(2).cross(xDirHint_foo).normalized(); + basis_foo.col(0) = basis_foo.col(1).cross(basis_foo.col(2)); + } else { + // basis_foo.z and yDirHint are far from parallel. + basis_foo.col(0) = yDirHint_foo.cross(basis_foo.col(2)).normalized(); + basis_foo.col(1) = basis_foo.col(2).cross(basis_foo.col(0)); + } + T det = basis_foo.determinant(); + // sanity check + SOPHUS_ENSURE(abs(det - T(1)) < Constants::epsilon(), + "Determinant of basis is not 1, but %. Basis is \n%\n", det, + basis_foo); + return basis_foo; +} + +/// Takes in plane normal in reference frame foo and constructs a corresponding +/// rotation matrix ``R_foo_plane``. +/// +/// See ``rotationFromNormal`` for details. +/// +template +SO3 SO3FromNormal(Vector3 const& normal_foo) { + return SO3(rotationFromNormal(normal_foo)); +} + +/// Returns a line (wrt. to frame ``foo``), given a pose of the ``line`` in +/// reference frame ``foo``. +/// +/// Note: The plane is defined by X-axis of the ``line`` frame. +/// +template +Line2 lineFromSE2(SE2 const& T_foo_line) { + return Line2(normalFromSO2(T_foo_line.so2()), T_foo_line.translation()); +} + +/// Returns the pose ``T_foo_line``, given a line in reference frame ``foo``. +/// +/// Note: The line is defined by X-axis of the frame ``line``. +/// +template +SE2 SE2FromLine(Line2 const& line_foo) { + T const d = line_foo.offset(); + Vector2 const n = line_foo.normal(); + SO2 const R_foo_plane = SO2FromNormal(n); + return SE2(R_foo_plane, -d * n); +} + +/// Returns a plane (wrt. to frame ``foo``), given a pose of the ``plane`` in +/// reference frame ``foo``. +/// +/// Note: The plane is defined by XY-plane of the frame ``plane``. +/// +template +Plane3 planeFromSE3(SE3 const& T_foo_plane) { + return Plane3(normalFromSO3(T_foo_plane.so3()), T_foo_plane.translation()); +} + +/// Returns the pose ``T_foo_plane``, given a plane in reference frame ``foo``. +/// +/// Note: The plane is defined by XY-plane of the frame ``plane``. +/// +template +SE3 SE3FromPlane(Plane3 const& plane_foo) { + T const d = plane_foo.offset(); + Vector3 const n = plane_foo.normal(); + SO3 const R_foo_plane = SO3FromNormal(n); + return SE3(R_foo_plane, -d * n); +} + +/// Takes in a hyperplane and returns unique representation by ensuring that the +/// ``offset`` is not negative. +/// +template +Eigen::Hyperplane makeHyperplaneUnique( + Eigen::Hyperplane const& plane) { + if (plane.offset() >= 0) { + return plane; + } + + return Eigen::Hyperplane(-plane.normal(), -plane.offset()); +} + +} // namespace Sophus + +#endif // GEOMETRY_HPP diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/interpolate.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/interpolate.hpp new file mode 100644 index 0000000..9f3249a --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/interpolate.hpp @@ -0,0 +1,38 @@ +/// @file +/// Interpolation for Lie groups. + +#ifndef SOPHUS_INTERPOLATE_HPP +#define SOPHUS_INTERPOLATE_HPP + +#include + +#include "interpolate_details.hpp" + +namespace Sophus { + +/// This function interpolates between two Lie group elements ``foo_T_bar`` +/// and ``foo_T_baz`` with an interpolation factor of ``alpha`` in [0, 1]. +/// +/// It returns a pose ``foo_T_quiz`` with ``quiz`` being a frame between ``bar`` +/// and ``baz``. If ``alpha=0`` it returns ``foo_T_bar``. If it is 1, it returns +/// ``foo_T_bar``. +/// +/// (Since interpolation on Lie groups is inverse-invariant, we can equivalently +/// think of the input arguments as being ``bar_T_foo``, ``baz_T_foo`` and the +/// return value being ``quiz_T_foo``.) +/// +/// Precondition: ``p`` must be in [0, 1]. +/// +template +enable_if_t::supported, G> interpolate( + G const& foo_T_bar, G const& foo_T_baz, Scalar2 p = Scalar2(0.5f)) { + using Scalar = typename G::Scalar; + Scalar inter_p(p); + SOPHUS_ENSURE(inter_p >= Scalar(0) && inter_p <= Scalar(1), + "p (%) must in [0, 1]."); + return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log()); +} + +} // namespace Sophus + +#endif // SOPHUS_INTERPOLATE_HPP diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/interpolate_details.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/interpolate_details.hpp new file mode 100644 index 0000000..9818682 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/interpolate_details.hpp @@ -0,0 +1,104 @@ +#ifndef SOPHUS_INTERPOLATE_DETAILS_HPP +#define SOPHUS_INTERPOLATE_DETAILS_HPP + +#include "rxso2.hpp" +#include "rxso3.hpp" +#include "se2.hpp" +#include "se3.hpp" +#include "sim2.hpp" +#include "sim3.hpp" +#include "so2.hpp" +#include "so3.hpp" + +namespace Sophus { +namespace interp_details { + +template +struct Traits; + +template +struct Traits> { + static bool constexpr supported = true; + + static bool hasShortestPathAmbiguity(SO2 const& foo_T_bar) { + using std::abs; + Scalar angle = foo_T_bar.log(); + return abs(abs(angle) - Constants::pi()) < + Constants::epsilon(); + } +}; + +template +struct Traits> { + static bool constexpr supported = true; + + static bool hasShortestPathAmbiguity(RxSO2 const& foo_T_bar) { + return Traits>::hasShortestPathAmbiguity(foo_T_bar.so2()); + } +}; + +template +struct Traits> { + static bool constexpr supported = true; + + static bool hasShortestPathAmbiguity(SO3 const& foo_T_bar) { + using std::abs; + Scalar angle = foo_T_bar.logAndTheta().theta; + return abs(abs(angle) - Constants::pi()) < + Constants::epsilon(); + } +}; + +template +struct Traits> { + static bool constexpr supported = true; + + static bool hasShortestPathAmbiguity(RxSO3 const& foo_T_bar) { + return Traits>::hasShortestPathAmbiguity(foo_T_bar.so3()); + } +}; + +template +struct Traits> { + static bool constexpr supported = true; + + static bool hasShortestPathAmbiguity(SE2 const& foo_T_bar) { + return Traits>::hasShortestPathAmbiguity(foo_T_bar.so2()); + } +}; + +template +struct Traits> { + static bool constexpr supported = true; + + static bool hasShortestPathAmbiguity(SE3 const& foo_T_bar) { + return Traits>::hasShortestPathAmbiguity(foo_T_bar.so3()); + } +}; + +template +struct Traits> { + static bool constexpr supported = true; + + static bool hasShortestPathAmbiguity(Sim2 const& foo_T_bar) { + return Traits>::hasShortestPathAmbiguity( + foo_T_bar.rxso2().so2()); + ; + } +}; + +template +struct Traits> { + static bool constexpr supported = true; + + static bool hasShortestPathAmbiguity(Sim3 const& foo_T_bar) { + return Traits>::hasShortestPathAmbiguity( + foo_T_bar.rxso3().so3()); + ; + } +}; + +} // namespace interp_details +} // namespace Sophus + +#endif // SOPHUS_INTERPOLATE_DETAILS_HPP diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/num_diff.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/num_diff.hpp new file mode 100644 index 0000000..e3c1349 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/num_diff.hpp @@ -0,0 +1,93 @@ +/// @file +/// Numerical differentiation using finite differences + +#ifndef SOPHUS_NUM_DIFF_HPP +#define SOPHUS_NUM_DIFF_HPP + +#include +#include +#include + +#include "types.hpp" + +namespace Sophus { + +namespace details { +template +class Curve { + public: + template + static auto num_diff(Fn curve, Scalar t, Scalar h) -> decltype(curve(t)) { + using ReturnType = decltype(curve(t)); + static_assert(std::is_floating_point::value, + "Scalar must be a floating point type."); + static_assert(IsFloatingPoint::value, + "ReturnType must be either a floating point scalar, " + "vector or matrix."); + + return (curve(t + h) - curve(t - h)) / (Scalar(2) * h); + } +}; + +template +class VectorField { + public: + static Eigen::Matrix num_diff( + std::function(Sophus::Vector)> + vector_field, + Sophus::Vector const& a, Scalar eps) { + static_assert(std::is_floating_point::value, + "Scalar must be a floating point type."); + Eigen::Matrix J; + Sophus::Vector h; + h.setZero(); + for (int i = 0; i < M; ++i) { + h[i] = eps; + J.col(i) = + (vector_field(a + h) - vector_field(a - h)) / (Scalar(2) * eps); + h[i] = Scalar(0); + } + + return J; + } +}; + +template +class VectorField { + public: + static Eigen::Matrix num_diff( + std::function(Scalar)> vector_field, + Scalar const& a, Scalar eps) { + return details::Curve::num_diff(std::move(vector_field), a, eps); + } +}; +} // namespace details + +/// Calculates the derivative of a curve at a point ``t``. +/// +/// Here, a curve is a function from a Scalar to a Euclidean space. Thus, it +/// returns either a Scalar, a vector or a matrix. +/// +template +auto curveNumDiff(Fn curve, Scalar t, + Scalar h = Constants::epsilonSqrt()) + -> decltype(details::Curve::num_diff(std::move(curve), t, h)) { + return details::Curve::num_diff(std::move(curve), t, h); +} + +/// Calculates the derivative of a vector field at a point ``a``. +/// +/// Here, a vector field is a function from a vector space to another vector +/// space. +/// +template +Eigen::Matrix vectorFieldNumDiff( + Fn vector_field, ScalarOrVector const& a, + Scalar eps = Constants::epsilonSqrt()) { + return details::VectorField::num_diff(std::move(vector_field), + a, eps); +} + +} // namespace Sophus + +#endif // SOPHUS_NUM_DIFF_HPP diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/rotation_matrix.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/rotation_matrix.hpp new file mode 100644 index 0000000..898d412 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/rotation_matrix.hpp @@ -0,0 +1,84 @@ +/// @file +/// Rotation matrix helper functions. + +#ifndef SOPHUS_ROTATION_MATRIX_HPP +#define SOPHUS_ROTATION_MATRIX_HPP + +#include +#include + +#include "types.hpp" + +namespace Sophus { + +/// Takes in arbitrary square matrix and returns true if it is +/// orthogonal. +template +SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase const& R) { + using Scalar = typename D::Scalar; + static int const N = D::RowsAtCompileTime; + static int const M = D::ColsAtCompileTime; + + static_assert(N == M, "must be a square matrix"); + static_assert(N >= 2, "must have compile time dimension >= 2"); + + return (R * R.transpose() - Matrix::Identity()).norm() < + Constants::epsilon(); +} + +/// Takes in arbitrary square matrix and returns true if it is +/// "scaled-orthogonal" with positive determinant. +/// +template +SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase const& sR) { + using Scalar = typename D::Scalar; + static int const N = D::RowsAtCompileTime; + static int const M = D::ColsAtCompileTime; + using std::pow; + using std::sqrt; + + Scalar det = sR.determinant(); + + if (det <= Scalar(0)) { + return false; + } + + Scalar scale_sqr = pow(det, Scalar(2. / N)); + + static_assert(N == M, "must be a square matrix"); + static_assert(N >= 2, "must have compile time dimension >= 2"); + + return (sR * sR.transpose() - scale_sqr * Matrix::Identity()) + .template lpNorm() < + sqrt(Constants::epsilon()); +} + +/// Takes in arbitrary square matrix (2x2 or larger) and returns closest +/// orthogonal matrix with positive determinant. +template +SOPHUS_FUNC enable_if_t< + std::is_floating_point::value, + Matrix> +makeRotationMatrix(Eigen::MatrixBase const& R) { + using Scalar = typename D::Scalar; + static int const N = D::RowsAtCompileTime; + static int const M = D::ColsAtCompileTime; + + static_assert(N == M, "must be a square matrix"); + static_assert(N >= 2, "must have compile time dimension >= 2"); + + Eigen::JacobiSVD> svd( + R, Eigen::ComputeFullU | Eigen::ComputeFullV); + + // Determine determinant of orthogonal matrix U*V'. + Scalar d = (svd.matrixU() * svd.matrixV().transpose()).determinant(); + // Starting from the identity matrix D, set the last entry to d (+1 or + // -1), so that det(U*D*V') = 1. + Matrix Diag = Matrix::Identity(); + Diag(N - 1, N - 1) = d; + return svd.matrixU() * Diag * svd.matrixV().transpose(); +} + +} // namespace Sophus + +#endif // SOPHUS_ROTATION_MATRIX_HPP diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/rxso2.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/rxso2.hpp new file mode 100644 index 0000000..eb679b3 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/rxso2.hpp @@ -0,0 +1,656 @@ +/// @file +/// Direct product R X SO(2) - rotation and scaling in 2d. + +#ifndef SOPHUS_RXSO2_HPP +#define SOPHUS_RXSO2_HPP + +#include "so2.hpp" + +namespace Sophus { +template +class RxSO2; +using RxSO2d = RxSO2; +using RxSO2f = RxSO2; +} // namespace Sophus + +namespace Eigen { +namespace internal { + +template +struct traits> { + static constexpr int Options = Options_; + using Scalar = Scalar_; + using ComplexType = Sophus::Vector2; +}; + +template +struct traits, Options_>> + : traits> { + static constexpr int Options = Options_; + using Scalar = Scalar_; + using ComplexType = Map, Options>; +}; + +template +struct traits const, Options_>> + : traits const> { + static constexpr int Options = Options_; + using Scalar = Scalar_; + using ComplexType = Map const, Options>; +}; +} // namespace internal +} // namespace Eigen + +namespace Sophus { + +/// RxSO2 base type - implements RxSO2 class but is storage agnostic +/// +/// This class implements the group ``R+ x SO(2)``, the direct product of the +/// group of positive scalar 2x2 matrices (= isomorph to the positive +/// real numbers) and the two-dimensional special orthogonal group SO(2). +/// Geometrically, it is the group of rotation and scaling in two dimensions. +/// As a matrix groups, R+ x SO(2) consists of matrices of the form ``s * R`` +/// where ``R`` is an orthogonal matrix with ``det(R) = 1`` and ``s > 0`` +/// being a positive real number. In particular, it has the following form: +/// +/// | s * cos(theta) s * -sin(theta) | +/// | s * sin(theta) s * cos(theta) | +/// +/// where ``theta`` being the rotation angle. Internally, it is represented by +/// the first column of the rotation matrix, or in other words by a non-zero +/// complex number. +/// +/// R+ x SO(2) is not compact, but a commutative group. First it is not compact +/// since the scale factor is not bound. Second it is commutative since +/// ``sR(alpha, s1) * sR(beta, s2) = sR(beta, s2) * sR(alpha, s1)``, simply +/// because ``alpha + beta = beta + alpha`` and ``s1 * s2 = s2 * s1`` with +/// ``alpha`` and ``beta`` being rotation angles and ``s1``, ``s2`` being scale +/// factors. +/// +/// This class has the explicit class invariant that the scale ``s`` is not +/// too close to zero. Strictly speaking, it must hold that: +/// +/// ``complex().norm() >= Constants::epsilon()``. +/// +/// In order to obey this condition, group multiplication is implemented with +/// saturation such that a product always has a scale which is equal or greater +/// this threshold. +template +class RxSO2Base { + public: + static constexpr int Options = Eigen::internal::traits::Options; + using Scalar = typename Eigen::internal::traits::Scalar; + using ComplexType = typename Eigen::internal::traits::ComplexType; + using ComplexTemporaryType = Sophus::Vector2; + + /// Degrees of freedom of manifold, number of dimensions in tangent space + /// (one for rotation and one for scaling). + static int constexpr DoF = 2; + /// Number of internal parameters used (complex number is a tuple). + static int constexpr num_parameters = 2; + /// Group transformations are 2x2 matrices. + static int constexpr N = 2; + using Transformation = Matrix; + using Point = Vector2; + using HomogeneousPoint = Vector3; + using Line = ParametrizedLine2; + using Tangent = Vector; + using Adjoint = Matrix; + + /// For binary operations the return type is determined with the + /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map + /// types, as well as other compatible scalar types such as Ceres::Jet and + /// double scalars with RxSO2 operations. + template + using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< + Scalar, typename OtherDerived::Scalar>::ReturnType; + + template + using RxSO2Product = RxSO2>; + + template + using PointProduct = Vector2>; + + template + using HomogeneousPointProduct = Vector3>; + + /// Adjoint transformation + /// + /// This function return the adjoint transformation ``Ad`` of the group + /// element ``A`` such that for all ``x`` it holds that + /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. + /// + /// For RxSO(2), it simply returns the identity matrix. + /// + SOPHUS_FUNC Adjoint Adj() const { return Adjoint::Identity(); } + + /// Returns rotation angle. + /// + SOPHUS_FUNC Scalar angle() const { return SO2(complex()).log(); } + + /// Returns copy of instance casted to NewScalarType. + /// + template + SOPHUS_FUNC RxSO2 cast() const { + return RxSO2(complex().template cast()); + } + + /// This provides unsafe read/write access to internal data. RxSO(2) is + /// represented by a complex number (two parameters). When using direct + /// write access, the user needs to take care of that the complex number is + /// not set close to zero. + /// + /// Note: The first parameter represents the real part, while the + /// second parameter represent the imaginary part. + /// + SOPHUS_FUNC Scalar* data() { return complex_nonconst().data(); } + + /// Const version of data() above. + /// + SOPHUS_FUNC Scalar const* data() const { return complex().data(); } + + /// Returns group inverse. + /// + SOPHUS_FUNC RxSO2 inverse() const { + Scalar squared_scale = complex().squaredNorm(); + return RxSO2(complex().x() / squared_scale, + -complex().y() / squared_scale); + } + + /// Logarithmic map + /// + /// Computes the logarithm, the inverse of the group exponential which maps + /// element of the group (scaled rotation matrices) to elements of the tangent + /// space (rotation-vector plus logarithm of scale factor). + /// + /// To be specific, this function computes ``vee(logmat(.))`` with + /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator + /// of RxSO2. + /// + SOPHUS_FUNC Tangent log() const { + using std::log; + Tangent theta_sigma; + theta_sigma[1] = log(scale()); + theta_sigma[0] = SO2(complex()).log(); + return theta_sigma; + } + + /// Returns 2x2 matrix representation of the instance. + /// + /// For RxSO2, the matrix representation is an scaled orthogonal matrix ``sR`` + /// with ``det(R)=s^2``, thus a scaled rotation matrix ``R`` with scale + /// ``s``. + /// + SOPHUS_FUNC Transformation matrix() const { + Transformation sR; + // clang-format off + sR << complex()[0], -complex()[1], + complex()[1], complex()[0]; + // clang-format on + return sR; + } + + /// Assignment-like operator from OtherDerived. + /// + template + SOPHUS_FUNC RxSO2Base& operator=( + RxSO2Base const& other) { + complex_nonconst() = other.complex(); + return *this; + } + + /// Group multiplication, which is rotation concatenation and scale + /// multiplication. + /// + /// Note: This function performs saturation for products close to zero in + /// order to ensure the class invariant. + /// + template + SOPHUS_FUNC RxSO2Product operator*( + RxSO2Base const& other) const { + using ResultT = ReturnScalar; + + Scalar lhs_real = complex().x(); + Scalar lhs_imag = complex().y(); + typename OtherDerived::Scalar const& rhs_real = other.complex().x(); + typename OtherDerived::Scalar const& rhs_imag = other.complex().y(); + /// complex multiplication + typename RxSO2Product::ComplexType result_complex( + lhs_real * rhs_real - lhs_imag * rhs_imag, + lhs_real * rhs_imag + lhs_imag * rhs_real); + + const ResultT squared_scale = result_complex.squaredNorm(); + + if (squared_scale < + Constants::epsilon() * Constants::epsilon()) { + /// Saturation to ensure class invariant. + result_complex.normalize(); + result_complex *= Constants::epsilon(); + } + return RxSO2Product(result_complex); + } + + /// Group action on 2-points. + /// + /// This function rotates a 2 dimensional point ``p`` by the SO2 element + /// ``bar_R_foo`` (= rotation matrix) and scales it by the scale factor ``s``: + /// + /// ``p_bar = s * (bar_R_foo * p_foo)``. + /// + template ::value>::type> + SOPHUS_FUNC PointProduct operator*( + Eigen::MatrixBase const& p) const { + return matrix() * p; + } + + /// Group action on homogeneous 2-points. See above for more details. + /// + template ::value>::type> + SOPHUS_FUNC HomogeneousPointProduct operator*( + Eigen::MatrixBase const& p) const { + const auto rsp = *this * p.template head<2>(); + return HomogeneousPointProduct(rsp(0), rsp(1), p(2)); + } + + /// Group action on lines. + /// + /// This function rotates a parameterized line ``l(t) = o + t * d`` by the SO2 + /// element and scales it by the scale factor + /// + /// Origin ``o`` is rotated and scaled + /// Direction ``d`` is rotated (preserving it's norm) + /// + SOPHUS_FUNC Line operator*(Line const& l) const { + return Line((*this) * l.origin(), (*this) * l.direction() / scale()); + } + + /// In-place group multiplication. This method is only valid if the return + /// type of the multiplication is compatible with this SO2's Scalar type. + /// + /// Note: This function performs saturation for products close to zero in + /// order to ensure the class invariant. + /// + template >::value>::type> + SOPHUS_FUNC RxSO2Base& operator*=( + RxSO2Base const& other) { + *static_cast(this) = *this * other; + return *this; + } + + /// Returns internal parameters of RxSO(2). + /// + /// It returns (c[0], c[1]), with c being the complex number. + /// + SOPHUS_FUNC Sophus::Vector params() const { + return complex(); + } + + /// Sets non-zero complex + /// + /// Precondition: ``z`` must not be close to zero. + SOPHUS_FUNC void setComplex(Vector2 const& z) { + SOPHUS_ENSURE(z.squaredNorm() > Constants::epsilon() * + Constants::epsilon(), + "Scale factor must be greater-equal epsilon."); + static_cast(this)->complex_nonconst() = z; + } + + /// Accessor of complex. + /// + SOPHUS_FUNC ComplexType const& complex() const { + return static_cast(this)->complex(); + } + + /// Returns rotation matrix. + /// + SOPHUS_FUNC Transformation rotationMatrix() const { + ComplexTemporaryType norm_quad = complex(); + norm_quad.normalize(); + return SO2(norm_quad).matrix(); + } + + /// Returns scale. + /// + SOPHUS_FUNC + Scalar scale() const { return complex().norm(); } + + /// Setter of rotation angle, leaves scale as is. + /// + SOPHUS_FUNC void setAngle(Scalar const& theta) { setSO2(SO2(theta)); } + + /// Setter of complex using rotation matrix ``R``, leaves scale as is. + /// + /// Precondition: ``R`` must be orthogonal with determinant of one. + /// + SOPHUS_FUNC void setRotationMatrix(Transformation const& R) { + setSO2(SO2(R)); + } + + /// Sets scale and leaves rotation as is. + /// + SOPHUS_FUNC void setScale(Scalar const& scale) { + using std::sqrt; + complex_nonconst().normalize(); + complex_nonconst() *= scale; + } + + /// Setter of complex number using scaled rotation matrix ``sR``. + /// + /// Precondition: The 2x2 matrix must be "scaled orthogonal" + /// and have a positive determinant. + /// + SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) { + SOPHUS_ENSURE(isScaledOrthogonalAndPositive(sR), + "sR must be scaled orthogonal:\n %", sR); + complex_nonconst() = sR.col(0); + } + + /// Setter of SO(2) rotations, leaves scale as is. + /// + SOPHUS_FUNC void setSO2(SO2 const& so2) { + using std::sqrt; + Scalar saved_scale = scale(); + complex_nonconst() = so2.unit_complex(); + complex_nonconst() *= saved_scale; + } + + SOPHUS_FUNC SO2 so2() const { return SO2(complex()); } + + private: + /// Mutator of complex is private to ensure class invariant. + /// + SOPHUS_FUNC ComplexType& complex_nonconst() { + return static_cast(this)->complex_nonconst(); + } +}; + +/// RxSO2 using storage; derived from RxSO2Base. +template +class RxSO2 : public RxSO2Base> { + public: + using Base = RxSO2Base>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + using ComplexMember = Eigen::Matrix; + + /// ``Base`` is friend so complex_nonconst can be accessed from ``Base``. + friend class RxSO2Base>; + + using Base::operator=; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// Default constructor initializes complex number to identity rotation and + /// scale to 1. + /// + SOPHUS_FUNC RxSO2() : complex_(Scalar(1), Scalar(0)) {} + + /// Copy constructor + /// + SOPHUS_FUNC RxSO2(RxSO2 const& other) = default; + + /// Copy-like constructor from OtherDerived. + /// + template + SOPHUS_FUNC RxSO2(RxSO2Base const& other) + : complex_(other.complex()) {} + + /// Constructor from scaled rotation matrix + /// + /// Precondition: rotation matrix need to be scaled orthogonal with + /// determinant of ``s^2``. + /// + SOPHUS_FUNC explicit RxSO2(Transformation const& sR) { + this->setScaledRotationMatrix(sR); + } + + /// Constructor from scale factor and rotation matrix ``R``. + /// + /// Precondition: Rotation matrix ``R`` must to be orthogonal with determinant + /// of 1 and ``scale`` must to be close to zero. + /// + SOPHUS_FUNC RxSO2(Scalar const& scale, Transformation const& R) + : RxSO2((scale * SO2(R).unit_complex()).eval()) {} + + /// Constructor from scale factor and SO2 + /// + /// Precondition: ``scale`` must be close to zero. + /// + SOPHUS_FUNC RxSO2(Scalar const& scale, SO2 const& so2) + : RxSO2((scale * so2.unit_complex()).eval()) {} + + /// Constructor from complex number. + /// + /// Precondition: complex number must not be close to zero. + /// + SOPHUS_FUNC explicit RxSO2(Vector2 const& z) : complex_(z) { + SOPHUS_ENSURE(complex_.squaredNorm() >= Constants::epsilon() * + Constants::epsilon(), + "Scale factor must be greater-equal epsilon: % vs %", + complex_.squaredNorm(), + Constants::epsilon() * Constants::epsilon()); + } + + /// Constructor from complex number. + /// + /// Precondition: complex number must not be close to zero. + /// + SOPHUS_FUNC explicit RxSO2(Scalar const& real, Scalar const& imag) + : RxSO2(Vector2(real, imag)) {} + + /// Accessor of complex. + /// + SOPHUS_FUNC ComplexMember const& complex() const { return complex_; } + + /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. + /// + SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { + return generator(i); + } + /// Group exponential + /// + /// This functions takes in an element of tangent space (= rotation angle + /// plus logarithm of scale) and returns the corresponding element of the + /// group RxSO2. + /// + /// To be more specific, this function computes ``expmat(hat(theta))`` + /// with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the + /// hat()-operator of RSO2. + /// + SOPHUS_FUNC static RxSO2 exp(Tangent const& a) { + using std::exp; + + Scalar const theta = a[0]; + Scalar const sigma = a[1]; + Scalar s = exp(sigma); + Vector2 z = SO2::exp(theta).unit_complex(); + z *= s; + return RxSO2(z); + } + + /// Returns the ith infinitesimal generators of ``R+ x SO(2)``. + /// + /// The infinitesimal generators of RxSO2 are: + /// + /// ``` + /// | 0 -1 | + /// G_0 = | 1 0 | + /// + /// | 1 0 | + /// G_1 = | 0 1 | + /// ``` + /// + /// Precondition: ``i`` must be 0, or 1. + /// + SOPHUS_FUNC static Transformation generator(int i) { + SOPHUS_ENSURE(i >= 0 && i <= 1, "i should be 0 or 1."); + Tangent e; + e.setZero(); + e[i] = Scalar(1); + return hat(e); + } + + /// hat-operator + /// + /// It takes in the 2-vector representation ``a`` (= rotation angle plus + /// logarithm of scale) and returns the corresponding matrix representation + /// of Lie algebra element. + /// + /// Formally, the hat()-operator of RxSO2 is defined as + /// + /// ``hat(.): R^2 -> R^{2x2}, hat(a) = sum_i a_i * G_i`` (for i=0,1,2) + /// + /// with ``G_i`` being the ith infinitesimal generator of RxSO2. + /// + /// The corresponding inverse is the vee()-operator, see below. + /// + SOPHUS_FUNC static Transformation hat(Tangent const& a) { + Transformation A; + // clang-format off + A << a(1), -a(0), + a(0), a(1); + // clang-format on + return A; + } + + /// Lie bracket + /// + /// It computes the Lie bracket of RxSO(2). To be more specific, it computes + /// + /// ``[omega_1, omega_2]_rxso2 := vee([hat(omega_1), hat(omega_2)])`` + /// + /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the + /// hat()-operator and ``vee(.)`` the vee()-operator of RxSO2. + /// + SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) { + Vector2 res; + res.setZero(); + return res; + } + + /// Draw uniform sample from RxSO(2) manifold. + /// + /// The scale factor is drawn uniformly in log2-space from [-1, 1], + /// hence the scale is in [0.5, 2)]. + /// + template + static RxSO2 sampleUniform(UniformRandomBitGenerator& generator) { + std::uniform_real_distribution uniform(Scalar(-1), Scalar(1)); + using std::exp2; + return RxSO2(exp2(uniform(generator)), + SO2::sampleUniform(generator)); + } + + /// vee-operator + /// + /// It takes the 2x2-matrix representation ``Omega`` and maps it to the + /// corresponding vector representation of Lie algebra. + /// + /// This is the inverse of the hat()-operator, see above. + /// + /// Precondition: ``Omega`` must have the following structure: + /// + /// | d -x | + /// | x d | + /// + SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { + using std::abs; + return Tangent(Omega(1, 0), Omega(0, 0)); + } + + protected: + SOPHUS_FUNC ComplexMember& complex_nonconst() { return complex_; } + + ComplexMember complex_; +}; + +} // namespace Sophus + +namespace Eigen { + +/// Specialization of Eigen::Map for ``RxSO2``; derived from RxSO2Base. +/// +/// Allows us to wrap RxSO2 objects around POD array (e.g. external z style +/// complex). +template +class Map, Options> + : public Sophus::RxSO2Base, Options>> { + using Base = Sophus::RxSO2Base, Options>>; + + public: + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + + /// ``Base`` is friend so complex_nonconst can be accessed from ``Base``. + friend class Sophus::RxSO2Base, Options>>; + + using Base::operator=; + using Base::operator*=; + using Base::operator*; + + SOPHUS_FUNC Map(Scalar* coeffs) : complex_(coeffs) {} + + /// Accessor of complex. + /// + SOPHUS_FUNC + Map, Options> const& complex() const { + return complex_; + } + + protected: + SOPHUS_FUNC Map, Options>& complex_nonconst() { + return complex_; + } + + Map, Options> complex_; +}; + +/// Specialization of Eigen::Map for ``RxSO2 const``; derived from RxSO2Base. +/// +/// Allows us to wrap RxSO2 objects around POD array (e.g. external z style +/// complex). +template +class Map const, Options> + : public Sophus::RxSO2Base const, Options>> { + public: + using Base = Sophus::RxSO2Base const, Options>>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + + using Base::operator*=; + using Base::operator*; + + SOPHUS_FUNC + Map(Scalar const* coeffs) : complex_(coeffs) {} + + /// Accessor of complex. + /// + SOPHUS_FUNC + Map const, Options> const& complex() const { + return complex_; + } + + protected: + Map const, Options> const complex_; +}; +} // namespace Eigen + +#endif /// SOPHUS_RXSO2_HPP diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/rxso3.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/rxso3.hpp new file mode 100644 index 0000000..de9989c --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/rxso3.hpp @@ -0,0 +1,725 @@ +/// @file +/// Direct product R X SO(3) - rotation and scaling in 3d. + +#ifndef SOPHUS_RXSO3_HPP +#define SOPHUS_RXSO3_HPP + +#include "so3.hpp" + +namespace Sophus { +template +class RxSO3; +using RxSO3d = RxSO3; +using RxSO3f = RxSO3; +} // namespace Sophus + +namespace Eigen { +namespace internal { + +template +struct traits> { + static constexpr int Options = Options_; + using Scalar = Scalar_; + using QuaternionType = Eigen::Quaternion; +}; + +template +struct traits, Options_>> + : traits> { + static constexpr int Options = Options_; + using Scalar = Scalar_; + using QuaternionType = Map, Options>; +}; + +template +struct traits const, Options_>> + : traits const> { + static constexpr int Options = Options_; + using Scalar = Scalar_; + using QuaternionType = Map const, Options>; +}; +} // namespace internal +} // namespace Eigen + +namespace Sophus { + +/// RxSO3 base type - implements RxSO3 class but is storage agnostic +/// +/// This class implements the group ``R+ x SO(3)``, the direct product of the +/// group of positive scalar 3x3 matrices (= isomorph to the positive +/// real numbers) and the three-dimensional special orthogonal group SO(3). +/// Geometrically, it is the group of rotation and scaling in three dimensions. +/// As a matrix groups, RxSO3 consists of matrices of the form ``s * R`` +/// where ``R`` is an orthogonal matrix with ``det(R)=1`` and ``s > 0`` +/// being a positive real number. +/// +/// Internally, RxSO3 is represented by the group of non-zero quaternions. +/// In particular, the scale equals the squared(!) norm of the quaternion ``q``, +/// ``s = |q|^2``. This is a most compact representation since the degrees of +/// freedom (DoF) of RxSO3 (=4) equals the number of internal parameters (=4). +/// +/// This class has the explicit class invariant that the scale ``s`` is not +/// too close to zero. Strictly speaking, it must hold that: +/// +/// ``quaternion().squaredNorm() >= Constants::epsilon()``. +/// +/// In order to obey this condition, group multiplication is implemented with +/// saturation such that a product always has a scale which is equal or greater +/// this threshold. +template +class RxSO3Base { + public: + static constexpr int Options = Eigen::internal::traits::Options; + using Scalar = typename Eigen::internal::traits::Scalar; + using QuaternionType = + typename Eigen::internal::traits::QuaternionType; + using QuaternionTemporaryType = Eigen::Quaternion; + + /// Degrees of freedom of manifold, number of dimensions in tangent space + /// (three for rotation and one for scaling). + static int constexpr DoF = 4; + /// Number of internal parameters used (quaternion is a 4-tuple). + static int constexpr num_parameters = 4; + /// Group transformations are 3x3 matrices. + static int constexpr N = 3; + using Transformation = Matrix; + using Point = Vector3; + using HomogeneousPoint = Vector4; + using Line = ParametrizedLine3; + using Tangent = Vector; + using Adjoint = Matrix; + + struct TangentAndTheta { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Tangent tangent; + Scalar theta; + }; + + /// For binary operations the return type is determined with the + /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map + /// types, as well as other compatible scalar types such as Ceres::Jet and + /// double scalars with RxSO3 operations. + template + using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< + Scalar, typename OtherDerived::Scalar>::ReturnType; + + template + using RxSO3Product = RxSO3>; + + template + using PointProduct = Vector3>; + + template + using HomogeneousPointProduct = Vector4>; + + /// Adjoint transformation + /// + /// This function return the adjoint transformation ``Ad`` of the group + /// element ``A`` such that for all ``x`` it holds that + /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. + /// + /// For RxSO(3), it simply returns the rotation matrix corresponding to ``A``. + /// + SOPHUS_FUNC Adjoint Adj() const { + Adjoint res; + res.setIdentity(); + res.template topLeftCorner<3, 3>() = rotationMatrix(); + return res; + } + + /// Returns copy of instance casted to NewScalarType. + /// + template + SOPHUS_FUNC RxSO3 cast() const { + return RxSO3(quaternion().template cast()); + } + + /// This provides unsafe read/write access to internal data. RxSO(3) is + /// represented by an Eigen::Quaternion (four parameters). When using direct + /// write access, the user needs to take care of that the quaternion is not + /// set close to zero. + /// + /// Note: The first three Scalars represent the imaginary parts, while the + /// forth Scalar represent the real part. + /// + SOPHUS_FUNC Scalar* data() { return quaternion_nonconst().coeffs().data(); } + + /// Const version of data() above. + /// + SOPHUS_FUNC Scalar const* data() const { + return quaternion().coeffs().data(); + } + + /// Returns group inverse. + /// + SOPHUS_FUNC RxSO3 inverse() const { + return RxSO3(quaternion().inverse()); + } + + /// Logarithmic map + /// + /// Computes the logarithm, the inverse of the group exponential which maps + /// element of the group (scaled rotation matrices) to elements of the tangent + /// space (rotation-vector plus logarithm of scale factor). + /// + /// To be specific, this function computes ``vee(logmat(.))`` with + /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator + /// of RxSO3. + /// + SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; } + + /// As above, but also returns ``theta = |omega|``. + /// + SOPHUS_FUNC TangentAndTheta logAndTheta() const { + using std::log; + + Scalar scale = quaternion().squaredNorm(); + TangentAndTheta result; + result.tangent[3] = log(scale); + auto omega_and_theta = SO3(quaternion()).logAndTheta(); + result.tangent.template head<3>() = omega_and_theta.tangent; + result.theta = omega_and_theta.theta; + return result; + } + /// Returns 3x3 matrix representation of the instance. + /// + /// For RxSO3, the matrix representation is an scaled orthogonal matrix ``sR`` + /// with ``det(R)=s^3``, thus a scaled rotation matrix ``R`` with scale + /// ``s``. + /// + SOPHUS_FUNC Transformation matrix() const { + Transformation sR; + + Scalar const vx_sq = quaternion().vec().x() * quaternion().vec().x(); + Scalar const vy_sq = quaternion().vec().y() * quaternion().vec().y(); + Scalar const vz_sq = quaternion().vec().z() * quaternion().vec().z(); + Scalar const w_sq = quaternion().w() * quaternion().w(); + Scalar const two_vx = Scalar(2) * quaternion().vec().x(); + Scalar const two_vy = Scalar(2) * quaternion().vec().y(); + Scalar const two_vz = Scalar(2) * quaternion().vec().z(); + Scalar const two_vx_vy = two_vx * quaternion().vec().y(); + Scalar const two_vx_vz = two_vx * quaternion().vec().z(); + Scalar const two_vx_w = two_vx * quaternion().w(); + Scalar const two_vy_vz = two_vy * quaternion().vec().z(); + Scalar const two_vy_w = two_vy * quaternion().w(); + Scalar const two_vz_w = two_vz * quaternion().w(); + + sR(0, 0) = vx_sq - vy_sq - vz_sq + w_sq; + sR(1, 0) = two_vx_vy + two_vz_w; + sR(2, 0) = two_vx_vz - two_vy_w; + + sR(0, 1) = two_vx_vy - two_vz_w; + sR(1, 1) = -vx_sq + vy_sq - vz_sq + w_sq; + sR(2, 1) = two_vx_w + two_vy_vz; + + sR(0, 2) = two_vx_vz + two_vy_w; + sR(1, 2) = -two_vx_w + two_vy_vz; + sR(2, 2) = -vx_sq - vy_sq + vz_sq + w_sq; + return sR; + } + + /// Assignment-like operator from OtherDerived. + /// + template + SOPHUS_FUNC RxSO3Base& operator=( + RxSO3Base const& other) { + quaternion_nonconst() = other.quaternion(); + return *this; + } + + /// Group multiplication, which is rotation concatenation and scale + /// multiplication. + /// + /// Note: This function performs saturation for products close to zero in + /// order to ensure the class invariant. + /// + template + SOPHUS_FUNC RxSO3Product operator*( + RxSO3Base const& other) const { + using ResultT = ReturnScalar; + typename RxSO3Product::QuaternionType result_quaternion( + quaternion() * other.quaternion()); + + ResultT scale = result_quaternion.squaredNorm(); + if (scale < Constants::epsilon()) { + SOPHUS_ENSURE(scale > ResultT(0), "Scale must be greater zero."); + /// Saturation to ensure class invariant. + result_quaternion.normalize(); + result_quaternion.coeffs() *= sqrt(Constants::epsilon()); + } + return RxSO3Product(result_quaternion); + } + + /// Group action on 3-points. + /// + /// This function rotates a 3 dimensional point ``p`` by the SO3 element + /// ``bar_R_foo`` (= rotation matrix) and scales it by the scale factor + /// ``s``: + /// + /// ``p_bar = s * (bar_R_foo * p_foo)``. + /// + template ::value>::type> + SOPHUS_FUNC PointProduct operator*( + Eigen::MatrixBase const& p) const { + // Follows http:///eigen.tuxfamily.org/bz/show_bug.cgi?id=459 + Scalar scale = quaternion().squaredNorm(); + PointProduct two_vec_cross_p = quaternion().vec().cross(p); + two_vec_cross_p += two_vec_cross_p; + return scale * p + (quaternion().w() * two_vec_cross_p + + quaternion().vec().cross(two_vec_cross_p)); + } + + /// Group action on homogeneous 3-points. See above for more details. + /// + template ::value>::type> + SOPHUS_FUNC HomogeneousPointProduct operator*( + Eigen::MatrixBase const& p) const { + const auto rsp = *this * p.template head<3>(); + return HomogeneousPointProduct(rsp(0), rsp(1), rsp(2), p(3)); + } + + /// Group action on lines. + /// + /// This function rotates a parametrized line ``l(t) = o + t * d`` by the SO3 + /// element ans scales it by the scale factor: + /// + /// Origin ``o`` is rotated and scaled + /// Direction ``d`` is rotated (preserving it's norm) + /// + SOPHUS_FUNC Line operator*(Line const& l) const { + return Line((*this) * l.origin(), + (*this) * l.direction() / quaternion().squaredNorm()); + } + + /// In-place group multiplication. This method is only valid if the return + /// type of the multiplication is compatible with this SO3's Scalar type. + /// + /// Note: This function performs saturation for products close to zero in + /// order to ensure the class invariant. + /// + template >::value>::type> + SOPHUS_FUNC RxSO3Base& operator*=( + RxSO3Base const& other) { + *static_cast(this) = *this * other; + return *this; + } + + /// Returns internal parameters of RxSO(3). + /// + /// It returns (q.imag[0], q.imag[1], q.imag[2], q.real), with q being the + /// quaternion. + /// + SOPHUS_FUNC Sophus::Vector params() const { + return quaternion().coeffs(); + } + + /// Sets non-zero quaternion + /// + /// Precondition: ``quat`` must not be close to zero. + SOPHUS_FUNC void setQuaternion(Eigen::Quaternion const& quat) { + SOPHUS_ENSURE(quat.squaredNorm() > Constants::epsilon() * + Constants::epsilon(), + "Scale factor must be greater-equal epsilon."); + static_cast(this)->quaternion_nonconst() = quat; + } + + /// Accessor of quaternion. + /// + SOPHUS_FUNC QuaternionType const& quaternion() const { + return static_cast(this)->quaternion(); + } + + /// Returns rotation matrix. + /// + SOPHUS_FUNC Transformation rotationMatrix() const { + QuaternionTemporaryType norm_quad = quaternion(); + norm_quad.normalize(); + return norm_quad.toRotationMatrix(); + } + + /// Returns scale. + /// + SOPHUS_FUNC + Scalar scale() const { return quaternion().squaredNorm(); } + + /// Setter of quaternion using rotation matrix ``R``, leaves scale as is. + /// + SOPHUS_FUNC void setRotationMatrix(Transformation const& R) { + using std::sqrt; + Scalar saved_scale = scale(); + quaternion_nonconst() = R; + quaternion_nonconst().coeffs() *= sqrt(saved_scale); + } + + /// Sets scale and leaves rotation as is. + /// + /// Note: This function as a significant computational cost, since it has to + /// call the square root twice. + /// + SOPHUS_FUNC + void setScale(Scalar const& scale) { + using std::sqrt; + quaternion_nonconst().normalize(); + quaternion_nonconst().coeffs() *= sqrt(scale); + } + + /// Setter of quaternion using scaled rotation matrix ``sR``. + /// + /// Precondition: The 3x3 matrix must be "scaled orthogonal" + /// and have a positive determinant. + /// + SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) { + Transformation squared_sR = sR * sR.transpose(); + Scalar squared_scale = + Scalar(1. / 3.) * + (squared_sR(0, 0) + squared_sR(1, 1) + squared_sR(2, 2)); + SOPHUS_ENSURE(squared_scale >= Constants::epsilon() * + Constants::epsilon(), + "Scale factor must be greater-equal epsilon."); + Scalar scale = sqrt(squared_scale); + quaternion_nonconst() = sR / scale; + quaternion_nonconst().coeffs() *= sqrt(scale); + } + + /// Setter of SO(3) rotations, leaves scale as is. + /// + SOPHUS_FUNC void setSO3(SO3 const& so3) { + using std::sqrt; + Scalar saved_scale = scale(); + quaternion_nonconst() = so3.unit_quaternion(); + quaternion_nonconst().coeffs() *= sqrt(saved_scale); + } + + SOPHUS_FUNC SO3 so3() const { return SO3(quaternion()); } + + private: + /// Mutator of quaternion is private to ensure class invariant. + /// + SOPHUS_FUNC QuaternionType& quaternion_nonconst() { + return static_cast(this)->quaternion_nonconst(); + } +}; + +/// RxSO3 using storage; derived from RxSO3Base. +template +class RxSO3 : public RxSO3Base> { + public: + using Base = RxSO3Base>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + using QuaternionMember = Eigen::Quaternion; + + /// ``Base`` is friend so quaternion_nonconst can be accessed from ``Base``. + friend class RxSO3Base>; + + using Base::operator=; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// Default constructor initializes quaternion to identity rotation and scale + /// to 1. + /// + SOPHUS_FUNC RxSO3() + : quaternion_(Scalar(1), Scalar(0), Scalar(0), Scalar(0)) {} + + /// Copy constructor + /// + SOPHUS_FUNC RxSO3(RxSO3 const& other) = default; + + /// Copy-like constructor from OtherDerived + /// + template + SOPHUS_FUNC RxSO3(RxSO3Base const& other) + : quaternion_(other.quaternion()) {} + + /// Constructor from scaled rotation matrix + /// + /// Precondition: rotation matrix need to be scaled orthogonal with + /// determinant of ``s^3``. + /// + SOPHUS_FUNC explicit RxSO3(Transformation const& sR) { + this->setScaledRotationMatrix(sR); + } + + /// Constructor from scale factor and rotation matrix ``R``. + /// + /// Precondition: Rotation matrix ``R`` must to be orthogonal with determinant + /// of 1 and ``scale`` must not be close to zero. + /// + SOPHUS_FUNC RxSO3(Scalar const& scale, Transformation const& R) + : quaternion_(R) { + SOPHUS_ENSURE(scale >= Constants::epsilon(), + "Scale factor must be greater-equal epsilon."); + using std::sqrt; + quaternion_.coeffs() *= sqrt(scale); + } + + /// Constructor from scale factor and SO3 + /// + /// Precondition: ``scale`` must not to be close to zero. + /// + SOPHUS_FUNC RxSO3(Scalar const& scale, SO3 const& so3) + : quaternion_(so3.unit_quaternion()) { + SOPHUS_ENSURE(scale >= Constants::epsilon(), + "Scale factor must be greater-equal epsilon."); + using std::sqrt; + quaternion_.coeffs() *= sqrt(scale); + } + + /// Constructor from quaternion + /// + /// Precondition: quaternion must not be close to zero. + /// + template + SOPHUS_FUNC explicit RxSO3(Eigen::QuaternionBase const& quat) + : quaternion_(quat) { + static_assert(std::is_same::value, + "must be same Scalar type."); + SOPHUS_ENSURE(quaternion_.squaredNorm() >= Constants::epsilon(), + "Scale factor must be greater-equal epsilon."); + } + + /// Accessor of quaternion. + /// + SOPHUS_FUNC QuaternionMember const& quaternion() const { return quaternion_; } + + /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. + /// + SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { + return generator(i); + } + /// Group exponential + /// + /// This functions takes in an element of tangent space (= rotation 3-vector + /// plus logarithm of scale) and returns the corresponding element of the + /// group RxSO3. + /// + /// To be more specific, thixs function computes ``expmat(hat(omega))`` + /// with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the + /// hat()-operator of RSO3. + /// + SOPHUS_FUNC static RxSO3 exp(Tangent const& a) { + Scalar theta; + return expAndTheta(a, &theta); + } + + /// As above, but also returns ``theta = |omega|`` as out-parameter. + /// + /// Precondition: ``theta`` must not be ``nullptr``. + /// + SOPHUS_FUNC static RxSO3 expAndTheta(Tangent const& a, + Scalar* theta) { + SOPHUS_ENSURE(theta != nullptr, "must not be nullptr."); + using std::exp; + using std::sqrt; + + Vector3 const omega = a.template head<3>(); + Scalar sigma = a[3]; + Scalar sqrt_scale = sqrt(exp(sigma)); + Eigen::Quaternion quat = + SO3::expAndTheta(omega, theta).unit_quaternion(); + quat.coeffs() *= sqrt_scale; + return RxSO3(quat); + } + + /// Returns the ith infinitesimal generators of ``R+ x SO(3)``. + /// + /// The infinitesimal generators of RxSO3 are: + /// + /// ``` + /// | 0 0 0 | + /// G_0 = | 0 0 -1 | + /// | 0 1 0 | + /// + /// | 0 0 1 | + /// G_1 = | 0 0 0 | + /// | -1 0 0 | + /// + /// | 0 -1 0 | + /// G_2 = | 1 0 0 | + /// | 0 0 0 | + /// + /// | 1 0 0 | + /// G_2 = | 0 1 0 | + /// | 0 0 1 | + /// ``` + /// + /// Precondition: ``i`` must be 0, 1, 2 or 3. + /// + SOPHUS_FUNC static Transformation generator(int i) { + SOPHUS_ENSURE(i >= 0 && i <= 3, "i should be in range [0,3]."); + Tangent e; + e.setZero(); + e[i] = Scalar(1); + return hat(e); + } + + /// hat-operator + /// + /// It takes in the 4-vector representation ``a`` (= rotation vector plus + /// logarithm of scale) and returns the corresponding matrix representation + /// of Lie algebra element. + /// + /// Formally, the hat()-operator of RxSO3 is defined as + /// + /// ``hat(.): R^4 -> R^{3x3}, hat(a) = sum_i a_i * G_i`` (for i=0,1,2,3) + /// + /// with ``G_i`` being the ith infinitesimal generator of RxSO3. + /// + /// The corresponding inverse is the vee()-operator, see below. + /// + SOPHUS_FUNC static Transformation hat(Tangent const& a) { + Transformation A; + // clang-format off + A << a(3), -a(2), a(1), + a(2), a(3), -a(0), + -a(1), a(0), a(3); + // clang-format on + return A; + } + + /// Lie bracket + /// + /// It computes the Lie bracket of RxSO(3). To be more specific, it computes + /// + /// ``[omega_1, omega_2]_rxso3 := vee([hat(omega_1), hat(omega_2)])`` + /// + /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the + /// hat()-operator and ``vee(.)`` the vee()-operator of RxSO3. + /// + SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) { + Vector3 const omega1 = a.template head<3>(); + Vector3 const omega2 = b.template head<3>(); + Vector4 res; + res.template head<3>() = omega1.cross(omega2); + res[3] = Scalar(0); + return res; + } + + /// Draw uniform sample from RxSO(3) manifold. + /// + /// The scale factor is drawn uniformly in log2-space from [-1, 1], + /// hence the scale is in [0.5, 2]. + /// + template + static RxSO3 sampleUniform(UniformRandomBitGenerator& generator) { + std::uniform_real_distribution uniform(Scalar(-1), Scalar(1)); + using std::exp2; + return RxSO3(exp2(uniform(generator)), + SO3::sampleUniform(generator)); + } + + /// vee-operator + /// + /// It takes the 3x3-matrix representation ``Omega`` and maps it to the + /// corresponding vector representation of Lie algebra. + /// + /// This is the inverse of the hat()-operator, see above. + /// + /// Precondition: ``Omega`` must have the following structure: + /// + /// | d -c b | + /// | c d -a | + /// | -b a d | + /// + SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { + using std::abs; + return Tangent(Omega(2, 1), Omega(0, 2), Omega(1, 0), Omega(0, 0)); + } + + protected: + SOPHUS_FUNC QuaternionMember& quaternion_nonconst() { return quaternion_; } + + QuaternionMember quaternion_; +}; + +} // namespace Sophus + +namespace Eigen { + +/// Specialization of Eigen::Map for ``RxSO3``; derived from RxSO3Base +/// +/// Allows us to wrap RxSO3 objects around POD array (e.g. external c style +/// quaternion). +template +class Map, Options> + : public Sophus::RxSO3Base, Options>> { + public: + using Base = Sophus::RxSO3Base, Options>>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + + /// ``Base`` is friend so quaternion_nonconst can be accessed from ``Base``. + friend class Sophus::RxSO3Base, Options>>; + + using Base::operator=; + using Base::operator*=; + using Base::operator*; + + SOPHUS_FUNC Map(Scalar* coeffs) : quaternion_(coeffs) {} + + /// Accessor of quaternion. + /// + SOPHUS_FUNC + Map, Options> const& quaternion() const { + return quaternion_; + } + + protected: + SOPHUS_FUNC Map, Options>& quaternion_nonconst() { + return quaternion_; + } + + Map, Options> quaternion_; +}; + +/// Specialization of Eigen::Map for ``RxSO3 const``; derived from RxSO3Base. +/// +/// Allows us to wrap RxSO3 objects around POD array (e.g. external c style +/// quaternion). +template +class Map const, Options> + : public Sophus::RxSO3Base const, Options>> { + public: + using Base = Sophus::RxSO3Base const, Options>>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + + using Base::operator*=; + using Base::operator*; + + SOPHUS_FUNC + Map(Scalar const* coeffs) : quaternion_(coeffs) {} + + /// Accessor of quaternion. + /// + SOPHUS_FUNC + Map const, Options> const& quaternion() const { + return quaternion_; + } + + protected: + Map const, Options> const quaternion_; +}; +} // namespace Eigen + +#endif /// SOPHUS_RXSO3_HPP diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/se2.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/se2.hpp new file mode 100644 index 0000000..996f29a --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/se2.hpp @@ -0,0 +1,835 @@ +/// @file +/// Special Euclidean group SE(2) - rotation and translation in 2d. + +#ifndef SOPHUS_SE2_HPP +#define SOPHUS_SE2_HPP + +#include "so2.hpp" + +namespace Sophus { +template +class SE2; +using SE2d = SE2; +using SE2f = SE2; +} // namespace Sophus + +namespace Eigen { +namespace internal { + +template +struct traits> { + using Scalar = Scalar_; + using TranslationType = Sophus::Vector2; + using SO2Type = Sophus::SO2; +}; + +template +struct traits, Options>> + : traits> { + using Scalar = Scalar_; + using TranslationType = Map, Options>; + using SO2Type = Map, Options>; +}; + +template +struct traits const, Options>> + : traits const> { + using Scalar = Scalar_; + using TranslationType = Map const, Options>; + using SO2Type = Map const, Options>; +}; +} // namespace internal +} // namespace Eigen + +namespace Sophus { + +/// SE2 base type - implements SE2 class but is storage agnostic. +/// +/// SE(2) is the group of rotations and translation in 2d. It is the +/// semi-direct product of SO(2) and the 2d Euclidean vector space. The class +/// is represented using a composition of SO2Group for rotation and a 2-vector +/// for translation. +/// +/// SE(2) is neither compact, nor a commutative group. +/// +/// See SO2Group for more details of the rotation representation in 2d. +/// +template +class SE2Base { + public: + using Scalar = typename Eigen::internal::traits::Scalar; + using TranslationType = + typename Eigen::internal::traits::TranslationType; + using SO2Type = typename Eigen::internal::traits::SO2Type; + + /// Degrees of freedom of manifold, number of dimensions in tangent space + /// (two for translation, three for rotation). + static int constexpr DoF = 3; + /// Number of internal parameters used (tuple for complex, two for + /// translation). + static int constexpr num_parameters = 4; + /// Group transformations are 3x3 matrices. + static int constexpr N = 3; + using Transformation = Matrix; + using Point = Vector2; + using HomogeneousPoint = Vector3; + using Line = ParametrizedLine2; + using Tangent = Vector; + using Adjoint = Matrix; + + /// For binary operations the return type is determined with the + /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map + /// types, as well as other compatible scalar types such as Ceres::Jet and + /// double scalars with SE2 operations. + template + using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< + Scalar, typename OtherDerived::Scalar>::ReturnType; + + template + using SE2Product = SE2>; + + template + using PointProduct = Vector2>; + + template + using HomogeneousPointProduct = Vector3>; + + /// Adjoint transformation + /// + /// This function return the adjoint transformation ``Ad`` of the group + /// element ``A`` such that for all ``x`` it holds that + /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. + /// + SOPHUS_FUNC Adjoint Adj() const { + Matrix const& R = so2().matrix(); + Transformation res; + res.setIdentity(); + res.template topLeftCorner<2, 2>() = R; + res(0, 2) = translation()[1]; + res(1, 2) = -translation()[0]; + return res; + } + + /// Returns copy of instance casted to NewScalarType. + /// + template + SOPHUS_FUNC SE2 cast() const { + return SE2(so2().template cast(), + translation().template cast()); + } + + /// Returns derivative of this * exp(x) wrt x at x=0. + /// + SOPHUS_FUNC Matrix Dx_this_mul_exp_x_at_0() + const { + Matrix J; + Sophus::Vector2 const c = unit_complex(); + Scalar o(0); + J(0, 0) = o; + J(0, 1) = o; + J(0, 2) = -c[1]; + J(1, 0) = o; + J(1, 1) = o; + J(1, 2) = c[0]; + J(2, 0) = c[0]; + J(2, 1) = -c[1]; + J(2, 2) = o; + J(3, 0) = c[1]; + J(3, 1) = c[0]; + J(3, 2) = o; + return J; + } + + /// Returns group inverse. + /// + SOPHUS_FUNC SE2 inverse() const { + SO2 const invR = so2().inverse(); + return SE2(invR, invR * (translation() * Scalar(-1))); + } + + /// Logarithmic map + /// + /// Computes the logarithm, the inverse of the group exponential which maps + /// element of the group (rigid body transformations) to elements of the + /// tangent space (twist). + /// + /// To be specific, this function computes ``vee(logmat(.))`` with + /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator + /// of SE(2). + /// + SOPHUS_FUNC Tangent log() const { + using std::abs; + + Tangent upsilon_theta; + Scalar theta = so2().log(); + upsilon_theta[2] = theta; + Scalar halftheta = Scalar(0.5) * theta; + Scalar halftheta_by_tan_of_halftheta; + + Vector2 z = so2().unit_complex(); + Scalar real_minus_one = z.x() - Scalar(1.); + if (abs(real_minus_one) < Constants::epsilon()) { + halftheta_by_tan_of_halftheta = + Scalar(1.) - Scalar(1. / 12) * theta * theta; + } else { + halftheta_by_tan_of_halftheta = -(halftheta * z.y()) / (real_minus_one); + } + Matrix V_inv; + V_inv << halftheta_by_tan_of_halftheta, halftheta, -halftheta, + halftheta_by_tan_of_halftheta; + upsilon_theta.template head<2>() = V_inv * translation(); + return upsilon_theta; + } + + /// Normalize SO2 element + /// + /// It re-normalizes the SO2 element. + /// + SOPHUS_FUNC void normalize() { so2().normalize(); } + + /// Returns 3x3 matrix representation of the instance. + /// + /// It has the following form: + /// + /// | R t | + /// | o 1 | + /// + /// where ``R`` is a 2x2 rotation matrix, ``t`` a translation 2-vector and + /// ``o`` a 2-column vector of zeros. + /// + SOPHUS_FUNC Transformation matrix() const { + Transformation homogenious_matrix; + homogenious_matrix.template topLeftCorner<2, 3>() = matrix2x3(); + homogenious_matrix.row(2) = + Matrix(Scalar(0), Scalar(0), Scalar(1)); + return homogenious_matrix; + } + + /// Returns the significant first two rows of the matrix above. + /// + SOPHUS_FUNC Matrix matrix2x3() const { + Matrix matrix; + matrix.template topLeftCorner<2, 2>() = rotationMatrix(); + matrix.col(2) = translation(); + return matrix; + } + + /// Assignment-like operator from OtherDerived. + /// + template + SOPHUS_FUNC SE2Base& operator=(SE2Base const& other) { + so2() = other.so2(); + translation() = other.translation(); + return *this; + } + + /// Group multiplication, which is rotation concatenation. + /// + template + SOPHUS_FUNC SE2Product operator*( + SE2Base const& other) const { + return SE2Product( + so2() * other.so2(), translation() + so2() * other.translation()); + } + + /// Group action on 2-points. + /// + /// This function rotates and translates a two dimensional point ``p`` by the + /// SE(2) element ``bar_T_foo = (bar_R_foo, t_bar)`` (= rigid body + /// transformation): + /// + /// ``p_bar = bar_R_foo * p_foo + t_bar``. + /// + template ::value>::type> + SOPHUS_FUNC PointProduct operator*( + Eigen::MatrixBase const& p) const { + return so2() * p + translation(); + } + + /// Group action on homogeneous 2-points. See above for more details. + /// + template ::value>::type> + SOPHUS_FUNC HomogeneousPointProduct operator*( + Eigen::MatrixBase const& p) const { + const PointProduct tp = + so2() * p.template head<2>() + p(2) * translation(); + return HomogeneousPointProduct(tp(0), tp(1), p(2)); + } + + /// Group action on lines. + /// + /// This function rotates and translates a parametrized line + /// ``l(t) = o + t * d`` by the SE(2) element: + /// + /// Origin ``o`` is rotated and translated using SE(2) action + /// Direction ``d`` is rotated using SO(2) action + /// + SOPHUS_FUNC Line operator*(Line const& l) const { + return Line((*this) * l.origin(), so2() * l.direction()); + } + + /// In-place group multiplication. This method is only valid if the return + /// type of the multiplication is compatible with this SO2's Scalar type. + /// + template >::value>::type> + SOPHUS_FUNC SE2Base& operator*=(SE2Base const& other) { + *static_cast(this) = *this * other; + return *this; + } + + /// Returns internal parameters of SE(2). + /// + /// It returns (c[0], c[1], t[0], t[1]), + /// with c being the unit complex number, t the translation 3-vector. + /// + SOPHUS_FUNC Sophus::Vector params() const { + Sophus::Vector p; + p << so2().params(), translation(); + return p; + } + + /// Returns rotation matrix. + /// + SOPHUS_FUNC Matrix rotationMatrix() const { + return so2().matrix(); + } + + /// Takes in complex number, and normalizes it. + /// + /// Precondition: The complex number must not be close to zero. + /// + SOPHUS_FUNC void setComplex(Sophus::Vector2 const& complex) { + return so2().setComplex(complex); + } + + /// Sets ``so3`` using ``rotation_matrix``. + /// + /// Precondition: ``R`` must be orthogonal and ``det(R)=1``. + /// + SOPHUS_FUNC void setRotationMatrix(Matrix const& R) { + SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %", R); + SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: %", + R.determinant()); + typename SO2Type::ComplexTemporaryType const complex( + Scalar(0.5) * (R(0, 0) + R(1, 1)), Scalar(0.5) * (R(1, 0) - R(0, 1))); + so2().setComplex(complex); + } + + /// Mutator of SO3 group. + /// + SOPHUS_FUNC + SO2Type& so2() { return static_cast(this)->so2(); } + + /// Accessor of SO3 group. + /// + SOPHUS_FUNC + SO2Type const& so2() const { + return static_cast(this)->so2(); + } + + /// Mutator of translation vector. + /// + SOPHUS_FUNC + TranslationType& translation() { + return static_cast(this)->translation(); + } + + /// Accessor of translation vector + /// + SOPHUS_FUNC + TranslationType const& translation() const { + return static_cast(this)->translation(); + } + + /// Accessor of unit complex number. + /// + SOPHUS_FUNC + typename Eigen::internal::traits::SO2Type::ComplexT const& + unit_complex() const { + return so2().unit_complex(); + } +}; + +/// SE2 using default storage; derived from SE2Base. +template +class SE2 : public SE2Base> { + public: + using Base = SE2Base>; + static int constexpr DoF = Base::DoF; + static int constexpr num_parameters = Base::num_parameters; + + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + using SO2Member = SO2; + using TranslationMember = Vector2; + + using Base::operator=; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// Default constructor initializes rigid body motion to the identity. + /// + SOPHUS_FUNC SE2(); + + /// Copy constructor + /// + SOPHUS_FUNC SE2(SE2 const& other) = default; + + /// Copy-like constructor from OtherDerived + /// + template + SOPHUS_FUNC SE2(SE2Base const& other) + : so2_(other.so2()), translation_(other.translation()) { + static_assert(std::is_same::value, + "must be same Scalar type"); + } + + /// Constructor from SO3 and translation vector + /// + template + SOPHUS_FUNC SE2(SO2Base const& so2, + Eigen::MatrixBase const& translation) + : so2_(so2), translation_(translation) { + static_assert(std::is_same::value, + "must be same Scalar type"); + static_assert(std::is_same::value, + "must be same Scalar type"); + } + + /// Constructor from rotation matrix and translation vector + /// + /// Precondition: Rotation matrix needs to be orthogonal with determinant + /// of 1. + /// + SOPHUS_FUNC + SE2(typename SO2::Transformation const& rotation_matrix, + Point const& translation) + : so2_(rotation_matrix), translation_(translation) {} + + /// Constructor from rotation angle and translation vector. + /// + SOPHUS_FUNC SE2(Scalar const& theta, Point const& translation) + : so2_(theta), translation_(translation) {} + + /// Constructor from complex number and translation vector + /// + /// Precondition: ``complex`` must not be close to zero. + SOPHUS_FUNC SE2(Vector2 const& complex, Point const& translation) + : so2_(complex), translation_(translation) {} + + /// Constructor from 3x3 matrix + /// + /// Precondition: Rotation matrix needs to be orthogonal with determinant + /// of 1. The last row must be ``(0, 0, 1)``. + /// + SOPHUS_FUNC explicit SE2(Transformation const& T) + : so2_(T.template topLeftCorner<2, 2>().eval()), + translation_(T.template block<2, 1>(0, 2)) {} + + /// This provides unsafe read/write access to internal data. SO(2) is + /// represented by a complex number (two parameters). When using direct write + /// access, the user needs to take care of that the complex number stays + /// normalized. + /// + SOPHUS_FUNC Scalar* data() { + // so2_ and translation_ are layed out sequentially with no padding + return so2_.data(); + } + + /// Const version of data() above. + /// + SOPHUS_FUNC Scalar const* data() const { + /// so2_ and translation_ are layed out sequentially with no padding + return so2_.data(); + } + + /// Accessor of SO3 + /// + SOPHUS_FUNC SO2Member& so2() { return so2_; } + + /// Mutator of SO3 + /// + SOPHUS_FUNC SO2Member const& so2() const { return so2_; } + + /// Mutator of translation vector + /// + SOPHUS_FUNC TranslationMember& translation() { return translation_; } + + /// Accessor of translation vector + /// + SOPHUS_FUNC TranslationMember const& translation() const { + return translation_; + } + + /// Returns derivative of exp(x) wrt. x. + /// + SOPHUS_FUNC static Sophus::Matrix Dx_exp_x( + Tangent const& upsilon_theta) { + using std::abs; + using std::cos; + using std::pow; + using std::sin; + Sophus::Matrix J; + Sophus::Vector upsilon = upsilon_theta.template head<2>(); + Scalar theta = upsilon_theta[2]; + + if (abs(theta) < Constants::epsilon()) { + Scalar const o(0); + Scalar const i(1); + + // clang-format off + J << o, o, o, o, o, i, i, o, -Scalar(0.5) * upsilon[1], o, i, + Scalar(0.5) * upsilon[0]; + // clang-format on + return J; + } + + Scalar const c0 = sin(theta); + Scalar const c1 = cos(theta); + Scalar const c2 = 1.0 / theta; + Scalar const c3 = c0 * c2; + Scalar const c4 = -c1 + Scalar(1); + Scalar const c5 = c2 * c4; + Scalar const c6 = c1 * c2; + Scalar const c7 = pow(theta, -2); + Scalar const c8 = c0 * c7; + Scalar const c9 = c4 * c7; + + Scalar const o = Scalar(0); + J(0, 0) = o; + J(0, 1) = o; + J(0, 2) = -c0; + J(1, 0) = o; + J(1, 1) = o; + J(1, 2) = c1; + J(2, 0) = c3; + J(2, 1) = -c5; + J(2, 2) = + -c3 * upsilon[1] + c6 * upsilon[0] - c8 * upsilon[0] + c9 * upsilon[1]; + J(3, 0) = c5; + J(3, 1) = c3; + J(3, 2) = + c3 * upsilon[0] + c6 * upsilon[1] - c8 * upsilon[1] - c9 * upsilon[0]; + return J; + } + + /// Returns derivative of exp(x) wrt. x_i at x=0. + /// + SOPHUS_FUNC static Sophus::Matrix + Dx_exp_x_at_0() { + Sophus::Matrix J; + Scalar const o(0); + Scalar const i(1); + + // clang-format off + J << o, o, o, o, o, i, i, o, o, o, i, o; + // clang-format on + return J; + } + + /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. + /// + SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { + return generator(i); + } + + /// Group exponential + /// + /// This functions takes in an element of tangent space (= twist ``a``) and + /// returns the corresponding element of the group SE(2). + /// + /// The first two components of ``a`` represent the translational part + /// ``upsilon`` in the tangent space of SE(2), while the last three components + /// of ``a`` represents the rotation vector ``omega``. + /// To be more specific, this function computes ``expmat(hat(a))`` with + /// ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator + /// of SE(2), see below. + /// + SOPHUS_FUNC static SE2 exp(Tangent const& a) { + Scalar theta = a[2]; + SO2 so2 = SO2::exp(theta); + Scalar sin_theta_by_theta; + Scalar one_minus_cos_theta_by_theta; + using std::abs; + + if (abs(theta) < Constants::epsilon()) { + Scalar theta_sq = theta * theta; + sin_theta_by_theta = Scalar(1.) - Scalar(1. / 6.) * theta_sq; + one_minus_cos_theta_by_theta = + Scalar(0.5) * theta - Scalar(1. / 24.) * theta * theta_sq; + } else { + sin_theta_by_theta = so2.unit_complex().y() / theta; + one_minus_cos_theta_by_theta = + (Scalar(1.) - so2.unit_complex().x()) / theta; + } + Vector2 trans( + sin_theta_by_theta * a[0] - one_minus_cos_theta_by_theta * a[1], + one_minus_cos_theta_by_theta * a[0] + sin_theta_by_theta * a[1]); + return SE2(so2, trans); + } + + /// Returns closest SE3 given arbitrary 4x4 matrix. + /// + template + static SOPHUS_FUNC enable_if_t::value, SE2> + fitToSE2(Matrix3 const& T) { + return SE2(SO2::fitToSO2(T.template block<2, 2>(0, 0)), + T.template block<2, 1>(0, 2)); + } + + /// Returns the ith infinitesimal generators of SE(2). + /// + /// The infinitesimal generators of SE(2) are: + /// + /// ``` + /// | 0 0 1 | + /// G_0 = | 0 0 0 | + /// | 0 0 0 | + /// + /// | 0 0 0 | + /// G_1 = | 0 0 1 | + /// | 0 0 0 | + /// + /// | 0 -1 0 | + /// G_2 = | 1 0 0 | + /// | 0 0 0 | + /// ``` + /// + /// Precondition: ``i`` must be in 0, 1 or 2. + /// + SOPHUS_FUNC static Transformation generator(int i) { + SOPHUS_ENSURE(i >= 0 || i <= 2, "i should be in range [0,2]."); + Tangent e; + e.setZero(); + e[i] = Scalar(1); + return hat(e); + } + + /// hat-operator + /// + /// It takes in the 3-vector representation (= twist) and returns the + /// corresponding matrix representation of Lie algebra element. + /// + /// Formally, the hat()-operator of SE(3) is defined as + /// + /// ``hat(.): R^3 -> R^{3x33}, hat(a) = sum_i a_i * G_i`` (for i=0,1,2) + /// + /// with ``G_i`` being the ith infinitesimal generator of SE(2). + /// + /// The corresponding inverse is the vee()-operator, see below. + /// + SOPHUS_FUNC static Transformation hat(Tangent const& a) { + Transformation Omega; + Omega.setZero(); + Omega.template topLeftCorner<2, 2>() = SO2::hat(a[2]); + Omega.col(2).template head<2>() = a.template head<2>(); + return Omega; + } + + /// Lie bracket + /// + /// It computes the Lie bracket of SE(2). To be more specific, it computes + /// + /// ``[omega_1, omega_2]_se2 := vee([hat(omega_1), hat(omega_2)])`` + /// + /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the + /// hat()-operator and ``vee(.)`` the vee()-operator of SE(2). + /// + SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) { + Vector2 upsilon1 = a.template head<2>(); + Vector2 upsilon2 = b.template head<2>(); + Scalar theta1 = a[2]; + Scalar theta2 = b[2]; + + return Tangent(-theta1 * upsilon2[1] + theta2 * upsilon1[1], + theta1 * upsilon2[0] - theta2 * upsilon1[0], Scalar(0)); + } + + /// Construct pure rotation. + /// + static SOPHUS_FUNC SE2 rot(Scalar const& x) { + return SE2(SO2(x), Sophus::Vector2::Zero()); + } + + /// Draw uniform sample from SE(2) manifold. + /// + /// Translations are drawn component-wise from the range [-1, 1]. + /// + template + static SE2 sampleUniform(UniformRandomBitGenerator& generator) { + std::uniform_real_distribution uniform(Scalar(-1), Scalar(1)); + return SE2(SO2::sampleUniform(generator), + Vector2(uniform(generator), uniform(generator))); + } + + /// Construct a translation only SE(2) instance. + /// + template + static SOPHUS_FUNC SE2 trans(T0 const& x, T1 const& y) { + return SE2(SO2(), Vector2(x, y)); + } + + static SOPHUS_FUNC SE2 trans(Vector2 const& xy) { + return SE2(SO2(), xy); + } + + /// Construct x-axis translation. + /// + static SOPHUS_FUNC SE2 transX(Scalar const& x) { + return SE2::trans(x, Scalar(0)); + } + + /// Construct y-axis translation. + /// + static SOPHUS_FUNC SE2 transY(Scalar const& y) { + return SE2::trans(Scalar(0), y); + } + + /// vee-operator + /// + /// It takes the 3x3-matrix representation ``Omega`` and maps it to the + /// corresponding 3-vector representation of Lie algebra. + /// + /// This is the inverse of the hat()-operator, see above. + /// + /// Precondition: ``Omega`` must have the following structure: + /// + /// | 0 -d a | + /// | d 0 b | + /// | 0 0 0 | + /// + SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { + SOPHUS_ENSURE( + Omega.row(2).template lpNorm<1>() < Constants::epsilon(), + "Omega: \n%", Omega); + Tangent upsilon_omega; + upsilon_omega.template head<2>() = Omega.col(2).template head<2>(); + upsilon_omega[2] = SO2::vee(Omega.template topLeftCorner<2, 2>()); + return upsilon_omega; + } + + protected: + SO2Member so2_; + TranslationMember translation_; +}; + +template +SE2::SE2() : translation_(TranslationMember::Zero()) { + static_assert(std::is_standard_layout::value, + "Assume standard layout for the use of offsetof check below."); + static_assert( + offsetof(SE2, so2_) + sizeof(Scalar) * SO2::num_parameters == + offsetof(SE2, translation_), + "This class assumes packed storage and hence will only work " + "correctly depending on the compiler (options) - in " + "particular when using [this->data(), this-data() + " + "num_parameters] to access the raw data in a contiguous fashion."); +} + +} // namespace Sophus + +namespace Eigen { + +/// Specialization of Eigen::Map for ``SE2``; derived from SE2Base. +/// +/// Allows us to wrap SE2 objects around POD array. +template +class Map, Options> + : public Sophus::SE2Base, Options>> { + public: + using Base = Sophus::SE2Base, Options>>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + + using Base::operator=; + using Base::operator*=; + using Base::operator*; + + SOPHUS_FUNC + Map(Scalar* coeffs) + : so2_(coeffs), + translation_(coeffs + Sophus::SO2::num_parameters) {} + + /// Mutator of SO3 + /// + SOPHUS_FUNC Map, Options>& so2() { return so2_; } + + /// Accessor of SO3 + /// + SOPHUS_FUNC Map, Options> const& so2() const { + return so2_; + } + + /// Mutator of translation vector + /// + SOPHUS_FUNC Map, Options>& translation() { + return translation_; + } + + /// Accessor of translation vector + /// + SOPHUS_FUNC Map, Options> const& translation() const { + return translation_; + } + + protected: + Map, Options> so2_; + Map, Options> translation_; +}; + +/// Specialization of Eigen::Map for ``SE2 const``; derived from SE2Base. +/// +/// Allows us to wrap SE2 objects around POD array. +template +class Map const, Options> + : public Sophus::SE2Base const, Options>> { + public: + using Base = Sophus::SE2Base const, Options>>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + + using Base::operator*=; + using Base::operator*; + + SOPHUS_FUNC Map(Scalar const* coeffs) + : so2_(coeffs), + translation_(coeffs + Sophus::SO2::num_parameters) {} + + /// Accessor of SO3 + /// + SOPHUS_FUNC Map const, Options> const& so2() const { + return so2_; + } + + /// Accessor of translation vector + /// + SOPHUS_FUNC Map const, Options> const& translation() + const { + return translation_; + } + + protected: + Map const, Options> const so2_; + Map const, Options> const translation_; +}; +} // namespace Eigen + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/se3.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/se3.hpp new file mode 100644 index 0000000..536eff0 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/se3.hpp @@ -0,0 +1,1076 @@ +/// @file +/// Special Euclidean group SE(3) - rotation and translation in 3d. + +#ifndef SOPHUS_SE3_HPP +#define SOPHUS_SE3_HPP + +#include "so3.hpp" + +namespace Sophus { +template +class SE3; +using SE3d = SE3; +using SE3f = SE3; +} // namespace Sophus + +namespace Eigen { +namespace internal { + +template +struct traits> { + using Scalar = Scalar_; + using TranslationType = Sophus::Vector3; + using SO3Type = Sophus::SO3; +}; + +template +struct traits, Options>> + : traits> { + using Scalar = Scalar_; + using TranslationType = Map, Options>; + using SO3Type = Map, Options>; +}; + +template +struct traits const, Options>> + : traits const> { + using Scalar = Scalar_; + using TranslationType = Map const, Options>; + using SO3Type = Map const, Options>; +}; +} // namespace internal +} // namespace Eigen + +namespace Sophus { + +/// SE3 base type - implements SE3 class but is storage agnostic. +/// +/// SE(3) is the group of rotations and translation in 3d. It is the +/// semi-direct product of SO(3) and the 3d Euclidean vector space. The class +/// is represented using a composition of SO3 for rotation and a one 3-vector +/// for translation. +/// +/// SE(3) is neither compact, nor a commutative group. +/// +/// See SO3 for more details of the rotation representation in 3d. +/// +template +class SE3Base { + public: + using Scalar = typename Eigen::internal::traits::Scalar; + using TranslationType = + typename Eigen::internal::traits::TranslationType; + using SO3Type = typename Eigen::internal::traits::SO3Type; + using QuaternionType = typename SO3Type::QuaternionType; + /// Degrees of freedom of manifold, number of dimensions in tangent space + /// (three for translation, three for rotation). + static int constexpr DoF = 6; + /// Number of internal parameters used (4-tuple for quaternion, three for + /// translation). + static int constexpr num_parameters = 7; + /// Group transformations are 4x4 matrices. + static int constexpr N = 4; + using Transformation = Matrix; + using Point = Vector3; + using HomogeneousPoint = Vector4; + using Line = ParametrizedLine3; + using Tangent = Vector; + using Adjoint = Matrix; + + /// For binary operations the return type is determined with the + /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map + /// types, as well as other compatible scalar types such as Ceres::Jet and + /// double scalars with SE3 operations. + template + using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< + Scalar, typename OtherDerived::Scalar>::ReturnType; + + template + using SE3Product = SE3>; + + template + using PointProduct = Vector3>; + + template + using HomogeneousPointProduct = Vector4>; + + /// Adjoint transformation + /// + /// This function return the adjoint transformation ``Ad`` of the group + /// element ``A`` such that for all ``x`` it holds that + /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. + /// + SOPHUS_FUNC Adjoint Adj() const { + Sophus::Matrix3 const R = so3().matrix(); + Adjoint res; + res.block(0, 0, 3, 3) = R; + res.block(3, 3, 3, 3) = R; + res.block(0, 3, 3, 3) = SO3::hat(translation()) * R; + res.block(3, 0, 3, 3) = Matrix3::Zero(3, 3); + return res; + } + + /// Extract rotation angle about canonical X-axis + /// + Scalar angleX() const { return so3().angleX(); } + + /// Extract rotation angle about canonical Y-axis + /// + Scalar angleY() const { return so3().angleY(); } + + /// Extract rotation angle about canonical Z-axis + /// + Scalar angleZ() const { return so3().angleZ(); } + + /// Returns copy of instance casted to NewScalarType. + /// + template + SOPHUS_FUNC SE3 cast() const { + return SE3(so3().template cast(), + translation().template cast()); + } + + /// Returns derivative of this * exp(x) wrt x at x=0. + /// + SOPHUS_FUNC Matrix Dx_this_mul_exp_x_at_0() + const { + Matrix J; + Eigen::Quaternion const q = unit_quaternion(); + Scalar const c0 = Scalar(0.5) * q.w(); + Scalar const c1 = Scalar(0.5) * q.z(); + Scalar const c2 = -c1; + Scalar const c3 = Scalar(0.5) * q.y(); + Scalar const c4 = Scalar(0.5) * q.x(); + Scalar const c5 = -c4; + Scalar const c6 = -c3; + Scalar const c7 = q.w() * q.w(); + Scalar const c8 = q.x() * q.x(); + Scalar const c9 = q.y() * q.y(); + Scalar const c10 = -c9; + Scalar const c11 = q.z() * q.z(); + Scalar const c12 = -c11; + Scalar const c13 = Scalar(2) * q.w(); + Scalar const c14 = c13 * q.z(); + Scalar const c15 = Scalar(2) * q.x(); + Scalar const c16 = c15 * q.y(); + Scalar const c17 = c13 * q.y(); + Scalar const c18 = c15 * q.z(); + Scalar const c19 = c7 - c8; + Scalar const c20 = c13 * q.x(); + Scalar const c21 = Scalar(2) * q.y() * q.z(); + J(0, 0) = 0; + J(0, 1) = 0; + J(0, 2) = 0; + J(0, 3) = c0; + J(0, 4) = c2; + J(0, 5) = c3; + J(1, 0) = 0; + J(1, 1) = 0; + J(1, 2) = 0; + J(1, 3) = c1; + J(1, 4) = c0; + J(1, 5) = c5; + J(2, 0) = 0; + J(2, 1) = 0; + J(2, 2) = 0; + J(2, 3) = c6; + J(2, 4) = c4; + J(2, 5) = c0; + J(3, 0) = 0; + J(3, 1) = 0; + J(3, 2) = 0; + J(3, 3) = c5; + J(3, 4) = c6; + J(3, 5) = c2; + J(4, 0) = c10 + c12 + c7 + c8; + J(4, 1) = -c14 + c16; + J(4, 2) = c17 + c18; + J(4, 3) = 0; + J(4, 4) = 0; + J(4, 5) = 0; + J(5, 0) = c14 + c16; + J(5, 1) = c12 + c19 + c9; + J(5, 2) = -c20 + c21; + J(5, 3) = 0; + J(5, 4) = 0; + J(5, 5) = 0; + J(6, 0) = -c17 + c18; + J(6, 1) = c20 + c21; + J(6, 2) = c10 + c11 + c19; + J(6, 3) = 0; + J(6, 4) = 0; + J(6, 5) = 0; + return J; + } + + /// Returns group inverse. + /// + SOPHUS_FUNC SE3 inverse() const { + SO3 invR = so3().inverse(); + return SE3(invR, invR * (translation() * Scalar(-1))); + } + + /// Logarithmic map + /// + /// Computes the logarithm, the inverse of the group exponential which maps + /// element of the group (rigid body transformations) to elements of the + /// tangent space (twist). + /// + /// To be specific, this function computes ``vee(logmat(.))`` with + /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator + /// of SE(3). + /// + SOPHUS_FUNC Tangent log() const { + // For the derivation of the logarithm of SE(3), see + // J. Gallier, D. Xu, "Computing exponentials of skew symmetric matrices + // and logarithms of orthogonal matrices", IJRA 2002. + // https:///pdfs.semanticscholar.org/cfe3/e4b39de63c8cabd89bf3feff7f5449fc981d.pdf + // (Sec. 6., pp. 8) + using std::abs; + using std::cos; + using std::sin; + Tangent upsilon_omega; + auto omega_and_theta = so3().logAndTheta(); + Scalar theta = omega_and_theta.theta; + upsilon_omega.template tail<3>() = omega_and_theta.tangent; + Matrix3 const Omega = + SO3::hat(upsilon_omega.template tail<3>()); + + if (abs(theta) < Constants::epsilon()) { + Matrix3 const V_inv = Matrix3::Identity() - + Scalar(0.5) * Omega + + Scalar(1. / 12.) * (Omega * Omega); + + upsilon_omega.template head<3>() = V_inv * translation(); + } else { + Scalar const half_theta = Scalar(0.5) * theta; + + Matrix3 const V_inv = + (Matrix3::Identity() - Scalar(0.5) * Omega + + (Scalar(1) - + theta * cos(half_theta) / (Scalar(2) * sin(half_theta))) / + (theta * theta) * (Omega * Omega)); + upsilon_omega.template head<3>() = V_inv * translation(); + } + return upsilon_omega; + } + + /// It re-normalizes the SO3 element. + /// + /// Note: Because of the class invariant of SO3, there is typically no need to + /// call this function directly. + /// + SOPHUS_FUNC void normalize() { so3().normalize(); } + + /// Returns 4x4 matrix representation of the instance. + /// + /// It has the following form: + /// + /// | R t | + /// | o 1 | + /// + /// where ``R`` is a 3x3 rotation matrix, ``t`` a translation 3-vector and + /// ``o`` a 3-column vector of zeros. + /// + SOPHUS_FUNC Transformation matrix() const { + Transformation homogenious_matrix; + homogenious_matrix.template topLeftCorner<3, 4>() = matrix3x4(); + homogenious_matrix.row(3) = + Matrix(Scalar(0), Scalar(0), Scalar(0), Scalar(1)); + return homogenious_matrix; + } + + /// Returns the significant first three rows of the matrix above. + /// + SOPHUS_FUNC Matrix matrix3x4() const { + Matrix matrix; + matrix.template topLeftCorner<3, 3>() = rotationMatrix(); + matrix.col(3) = translation(); + return matrix; + } + + /// Assignment-like operator from OtherDerived. + /// + template + SOPHUS_FUNC SE3Base& operator=(SE3Base const& other) { + so3() = other.so3(); + translation() = other.translation(); + return *this; + } + + /// Group multiplication, which is rotation concatenation. + /// + template + SOPHUS_FUNC SE3Product operator*( + SE3Base const& other) const { + return SE3Product( + so3() * other.so3(), translation() + so3() * other.translation()); + } + + /// Group action on 3-points. + /// + /// This function rotates and translates a three dimensional point ``p`` by + /// the SE(3) element ``bar_T_foo = (bar_R_foo, t_bar)`` (= rigid body + /// transformation): + /// + /// ``p_bar = bar_R_foo * p_foo + t_bar``. + /// + template ::value>::type> + SOPHUS_FUNC PointProduct operator*( + Eigen::MatrixBase const& p) const { + return so3() * p + translation(); + } + + /// Group action on homogeneous 3-points. See above for more details. + /// + template ::value>::type> + SOPHUS_FUNC HomogeneousPointProduct operator*( + Eigen::MatrixBase const& p) const { + const PointProduct tp = + so3() * p.template head<3>() + p(3) * translation(); + return HomogeneousPointProduct(tp(0), tp(1), tp(2), p(3)); + } + + /// Group action on lines. + /// + /// This function rotates and translates a parametrized line + /// ``l(t) = o + t * d`` by the SE(3) element: + /// + /// Origin is transformed using SE(3) action + /// Direction is transformed using rotation part + /// + SOPHUS_FUNC Line operator*(Line const& l) const { + return Line((*this) * l.origin(), so3() * l.direction()); + } + + /// In-place group multiplication. This method is only valid if the return + /// type of the multiplication is compatible with this SE3's Scalar type. + /// + template >::value>::type> + SOPHUS_FUNC SE3Base& operator*=(SE3Base const& other) { + *static_cast(this) = *this * other; + return *this; + } + + /// Returns rotation matrix. + /// + SOPHUS_FUNC Matrix3 rotationMatrix() const { return so3().matrix(); } + + /// Mutator of SO3 group. + /// + SOPHUS_FUNC SO3Type& so3() { return static_cast(this)->so3(); } + + /// Accessor of SO3 group. + /// + SOPHUS_FUNC SO3Type const& so3() const { + return static_cast(this)->so3(); + } + + /// Takes in quaternion, and normalizes it. + /// + /// Precondition: The quaternion must not be close to zero. + /// + SOPHUS_FUNC void setQuaternion(Eigen::Quaternion const& quat) { + so3().setQuaternion(quat); + } + + /// Sets ``so3`` using ``rotation_matrix``. + /// + /// Precondition: ``R`` must be orthogonal and ``det(R)=1``. + /// + SOPHUS_FUNC void setRotationMatrix(Matrix3 const& R) { + SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %", R); + SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: %", + R.determinant()); + so3().setQuaternion(Eigen::Quaternion(R)); + } + + /// Returns internal parameters of SE(3). + /// + /// It returns (q.imag[0], q.imag[1], q.imag[2], q.real, t[0], t[1], t[2]), + /// with q being the unit quaternion, t the translation 3-vector. + /// + SOPHUS_FUNC Sophus::Vector params() const { + Sophus::Vector p; + p << so3().params(), translation(); + return p; + } + + /// Mutator of translation vector. + /// + SOPHUS_FUNC TranslationType& translation() { + return static_cast(this)->translation(); + } + + /// Accessor of translation vector + /// + SOPHUS_FUNC TranslationType const& translation() const { + return static_cast(this)->translation(); + } + + /// Accessor of unit quaternion. + /// + SOPHUS_FUNC QuaternionType const& unit_quaternion() const { + return this->so3().unit_quaternion(); + } +}; + +/// SE3 using default storage; derived from SE3Base. +template +class SE3 : public SE3Base> { + using Base = SE3Base>; + + public: + static int constexpr DoF = Base::DoF; + static int constexpr num_parameters = Base::num_parameters; + + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + using SO3Member = SO3; + using TranslationMember = Vector3; + + using Base::operator=; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// Default constructor initializes rigid body motion to the identity. + /// + SOPHUS_FUNC SE3(); + + /// Copy constructor + /// + SOPHUS_FUNC SE3(SE3 const& other) = default; + + /// Copy-like constructor from OtherDerived. + /// + template + SOPHUS_FUNC SE3(SE3Base const& other) + : so3_(other.so3()), translation_(other.translation()) { + static_assert(std::is_same::value, + "must be same Scalar type"); + } + + /// Constructor from SO3 and translation vector + /// + template + SOPHUS_FUNC SE3(SO3Base const& so3, + Eigen::MatrixBase const& translation) + : so3_(so3), translation_(translation) { + static_assert(std::is_same::value, + "must be same Scalar type"); + static_assert(std::is_same::value, + "must be same Scalar type"); + } + + /// Constructor from rotation matrix and translation vector + /// + /// Precondition: Rotation matrix needs to be orthogonal with determinant + /// of 1. + /// + SOPHUS_FUNC + SE3(Matrix3 const& rotation_matrix, Point const& translation) + : so3_(rotation_matrix), translation_(translation) {} + + /// Constructor from quaternion and translation vector. + /// + /// Precondition: ``quaternion`` must not be close to zero. + /// + SOPHUS_FUNC SE3(Eigen::Quaternion const& quaternion, + Point const& translation) + : so3_(quaternion), translation_(translation) {} + + /// Constructor from 4x4 matrix + /// + /// Precondition: Rotation matrix needs to be orthogonal with determinant + /// of 1. The last row must be ``(0, 0, 0, 1)``. + /// + SOPHUS_FUNC explicit SE3(Matrix4 const& T) + : so3_(T.template topLeftCorner<3, 3>()), + translation_(T.template block<3, 1>(0, 3)) { + SOPHUS_ENSURE((T.row(3) - Matrix(Scalar(0), Scalar(0), + Scalar(0), Scalar(1))) + .squaredNorm() < Constants::epsilon(), + "Last row is not (0,0,0,1), but (%).", T.row(3)); + } + + /// This provides unsafe read/write access to internal data. SO(3) is + /// represented by an Eigen::Quaternion (four parameters). When using direct + /// write access, the user needs to take care of that the quaternion stays + /// normalized. + /// + SOPHUS_FUNC Scalar* data() { + // so3_ and translation_ are laid out sequentially with no padding + return so3_.data(); + } + + /// Const version of data() above. + /// + SOPHUS_FUNC Scalar const* data() const { + // so3_ and translation_ are laid out sequentially with no padding + return so3_.data(); + } + + /// Mutator of SO3 + /// + SOPHUS_FUNC SO3Member& so3() { return so3_; } + + /// Accessor of SO3 + /// + SOPHUS_FUNC SO3Member const& so3() const { return so3_; } + + /// Mutator of translation vector + /// + SOPHUS_FUNC TranslationMember& translation() { return translation_; } + + /// Accessor of translation vector + /// + SOPHUS_FUNC TranslationMember const& translation() const { + return translation_; + } + + /// Returns derivative of exp(x) wrt. x. + /// + SOPHUS_FUNC static Sophus::Matrix Dx_exp_x( + Tangent const& upsilon_omega) { + using std::cos; + using std::pow; + using std::sin; + using std::sqrt; + Sophus::Matrix J; + Sophus::Vector upsilon = upsilon_omega.template head<3>(); + Sophus::Vector omega = upsilon_omega.template tail<3>(); + + Scalar const c0 = omega[0] * omega[0]; + Scalar const c1 = omega[1] * omega[1]; + Scalar const c2 = omega[2] * omega[2]; + Scalar const c3 = c0 + c1 + c2; + Scalar const o(0); + Scalar const h(0.5); + Scalar const i(1); + + if (c3 < Constants::epsilon()) { + Scalar const ux = Scalar(0.5) * upsilon[0]; + Scalar const uy = Scalar(0.5) * upsilon[1]; + Scalar const uz = Scalar(0.5) * upsilon[2]; + + /// clang-format off + J << o, o, o, h, o, o, o, o, o, o, h, o, o, o, o, o, o, h, o, o, o, o, o, + o, i, o, o, o, uz, -uy, o, i, o, -uz, o, ux, o, o, i, uy, -ux, o; + /// clang-format on + return J; + } + + Scalar const c4 = sqrt(c3); + Scalar const c5 = Scalar(1.0) / c4; + Scalar const c6 = Scalar(0.5) * c4; + Scalar const c7 = sin(c6); + Scalar const c8 = c5 * c7; + Scalar const c9 = pow(c3, -3.0L / 2.0L); + Scalar const c10 = c7 * c9; + Scalar const c11 = Scalar(1.0) / c3; + Scalar const c12 = cos(c6); + Scalar const c13 = Scalar(0.5) * c11 * c12; + Scalar const c14 = c7 * c9 * omega[0]; + Scalar const c15 = Scalar(0.5) * c11 * c12 * omega[0]; + Scalar const c16 = -c14 * omega[1] + c15 * omega[1]; + Scalar const c17 = -c14 * omega[2] + c15 * omega[2]; + Scalar const c18 = omega[1] * omega[2]; + Scalar const c19 = -c10 * c18 + c13 * c18; + Scalar const c20 = c5 * omega[0]; + Scalar const c21 = Scalar(0.5) * c7; + Scalar const c22 = c5 * omega[1]; + Scalar const c23 = c5 * omega[2]; + Scalar const c24 = -c1; + Scalar const c25 = -c2; + Scalar const c26 = c24 + c25; + Scalar const c27 = sin(c4); + Scalar const c28 = -c27 + c4; + Scalar const c29 = c28 * c9; + Scalar const c30 = cos(c4); + Scalar const c31 = -c30 + Scalar(1); + Scalar const c32 = c11 * c31; + Scalar const c33 = c32 * omega[2]; + Scalar const c34 = c29 * omega[0]; + Scalar const c35 = c34 * omega[1]; + Scalar const c36 = c32 * omega[1]; + Scalar const c37 = c34 * omega[2]; + Scalar const c38 = pow(c3, -5.0L / 2.0L); + Scalar const c39 = Scalar(3) * c28 * c38 * omega[0]; + Scalar const c40 = c26 * c9; + Scalar const c41 = -c20 * c30 + c20; + Scalar const c42 = c27 * c9 * omega[0]; + Scalar const c43 = c42 * omega[1]; + Scalar const c44 = pow(c3, -2); + Scalar const c45 = Scalar(2) * c31 * c44 * omega[0]; + Scalar const c46 = c45 * omega[1]; + Scalar const c47 = c29 * omega[2]; + Scalar const c48 = c43 - c46 + c47; + Scalar const c49 = Scalar(3) * c0 * c28 * c38; + Scalar const c50 = c9 * omega[0] * omega[2]; + Scalar const c51 = c41 * c50 - c49 * omega[2]; + Scalar const c52 = c9 * omega[0] * omega[1]; + Scalar const c53 = c41 * c52 - c49 * omega[1]; + Scalar const c54 = c42 * omega[2]; + Scalar const c55 = c45 * omega[2]; + Scalar const c56 = c29 * omega[1]; + Scalar const c57 = -c54 + c55 + c56; + Scalar const c58 = Scalar(-2) * c56; + Scalar const c59 = Scalar(3) * c28 * c38 * omega[1]; + Scalar const c60 = -c22 * c30 + c22; + Scalar const c61 = -c18 * c39; + Scalar const c62 = c32 + c61; + Scalar const c63 = c27 * c9; + Scalar const c64 = c1 * c63; + Scalar const c65 = Scalar(2) * c31 * c44; + Scalar const c66 = c1 * c65; + Scalar const c67 = c50 * c60; + Scalar const c68 = -c1 * c39 + c52 * c60; + Scalar const c69 = c18 * c63; + Scalar const c70 = c18 * c65; + Scalar const c71 = c34 - c69 + c70; + Scalar const c72 = Scalar(-2) * c47; + Scalar const c73 = Scalar(3) * c28 * c38 * omega[2]; + Scalar const c74 = -c23 * c30 + c23; + Scalar const c75 = -c32 + c61; + Scalar const c76 = c2 * c63; + Scalar const c77 = c2 * c65; + Scalar const c78 = c52 * c74; + Scalar const c79 = c34 + c69 - c70; + Scalar const c80 = -c2 * c39 + c50 * c74; + Scalar const c81 = -c0; + Scalar const c82 = c25 + c81; + Scalar const c83 = c32 * omega[0]; + Scalar const c84 = c18 * c29; + Scalar const c85 = Scalar(-2) * c34; + Scalar const c86 = c82 * c9; + Scalar const c87 = c0 * c63; + Scalar const c88 = c0 * c65; + Scalar const c89 = c9 * omega[1] * omega[2]; + Scalar const c90 = c41 * c89; + Scalar const c91 = c54 - c55 + c56; + Scalar const c92 = -c1 * c73 + c60 * c89; + Scalar const c93 = -c43 + c46 + c47; + Scalar const c94 = -c2 * c59 + c74 * c89; + Scalar const c95 = c24 + c81; + Scalar const c96 = c9 * c95; + J(0, 0) = o; + J(0, 1) = o; + J(0, 2) = o; + J(0, 3) = -c0 * c10 + c0 * c13 + c8; + J(0, 4) = c16; + J(0, 5) = c17; + J(1, 0) = o; + J(1, 1) = o; + J(1, 2) = o; + J(1, 3) = c16; + J(1, 4) = -c1 * c10 + c1 * c13 + c8; + J(1, 5) = c19; + J(2, 0) = o; + J(2, 1) = o; + J(2, 2) = o; + J(2, 3) = c17; + J(2, 4) = c19; + J(2, 5) = -c10 * c2 + c13 * c2 + c8; + J(3, 0) = o; + J(3, 1) = o; + J(3, 2) = o; + J(3, 3) = -c20 * c21; + J(3, 4) = -c21 * c22; + J(3, 5) = -c21 * c23; + J(4, 0) = c26 * c29 + Scalar(1); + J(4, 1) = -c33 + c35; + J(4, 2) = c36 + c37; + J(4, 3) = upsilon[0] * (-c26 * c39 + c40 * c41) + upsilon[1] * (c53 + c57) + + upsilon[2] * (c48 + c51); + J(4, 4) = upsilon[0] * (-c26 * c59 + c40 * c60 + c58) + + upsilon[1] * (c68 + c71) + upsilon[2] * (c62 + c64 - c66 + c67); + J(4, 5) = upsilon[0] * (-c26 * c73 + c40 * c74 + c72) + + upsilon[1] * (c75 - c76 + c77 + c78) + upsilon[2] * (c79 + c80); + J(5, 0) = c33 + c35; + J(5, 1) = c29 * c82 + Scalar(1); + J(5, 2) = -c83 + c84; + J(5, 3) = upsilon[0] * (c53 + c91) + + upsilon[1] * (-c39 * c82 + c41 * c86 + c85) + + upsilon[2] * (c75 - c87 + c88 + c90); + J(5, 4) = upsilon[0] * (c68 + c79) + upsilon[1] * (-c59 * c82 + c60 * c86) + + upsilon[2] * (c92 + c93); + J(5, 5) = upsilon[0] * (c62 + c76 - c77 + c78) + + upsilon[1] * (c72 - c73 * c82 + c74 * c86) + + upsilon[2] * (c57 + c94); + J(6, 0) = -c36 + c37; + J(6, 1) = c83 + c84; + J(6, 2) = c29 * c95 + Scalar(1); + J(6, 3) = upsilon[0] * (c51 + c93) + upsilon[1] * (c62 + c87 - c88 + c90) + + upsilon[2] * (-c39 * c95 + c41 * c96 + c85); + J(6, 4) = upsilon[0] * (-c64 + c66 + c67 + c75) + upsilon[1] * (c48 + c92) + + upsilon[2] * (c58 - c59 * c95 + c60 * c96); + J(6, 5) = upsilon[0] * (c71 + c80) + upsilon[1] * (c91 + c94) + + upsilon[2] * (-c73 * c95 + c74 * c96); + + return J; + } + + /// Returns derivative of exp(x) wrt. x_i at x=0. + /// + SOPHUS_FUNC static Sophus::Matrix + Dx_exp_x_at_0() { + Sophus::Matrix J; + Scalar const o(0); + Scalar const h(0.5); + Scalar const i(1); + + // clang-format off + J << o, o, o, h, o, o, o, + o, o, o, h, o, o, o, + o, o, o, h, o, o, o, + o, o, o, i, o, o, o, + o, o, o, i, o, o, o, + o, o, o, i, o, o, o; + // clang-format on + return J; + } + + /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. + /// + SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { + return generator(i); + } + + /// Group exponential + /// + /// This functions takes in an element of tangent space (= twist ``a``) and + /// returns the corresponding element of the group SE(3). + /// + /// The first three components of ``a`` represent the translational part + /// ``upsilon`` in the tangent space of SE(3), while the last three components + /// of ``a`` represents the rotation vector ``omega``. + /// To be more specific, this function computes ``expmat(hat(a))`` with + /// ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator + /// of SE(3), see below. + /// + SOPHUS_FUNC static SE3 exp(Tangent const& a) { + using std::cos; + using std::sin; + Vector3 const omega = a.template tail<3>(); + + Scalar theta; + SO3 const so3 = SO3::expAndTheta(omega, &theta); + Matrix3 const Omega = SO3::hat(omega); + Matrix3 const Omega_sq = Omega * Omega; + Matrix3 V; + + if (theta < Constants::epsilon()) { + V = so3.matrix(); + /// Note: That is an accurate expansion! + } else { + Scalar theta_sq = theta * theta; + V = (Matrix3::Identity() + + (Scalar(1) - cos(theta)) / (theta_sq)*Omega + + (theta - sin(theta)) / (theta_sq * theta) * Omega_sq); + } + return SE3(so3, V * a.template head<3>()); + } + + /// Returns closest SE3 given arbirary 4x4 matrix. + /// + template + SOPHUS_FUNC static enable_if_t::value, SE3> + fitToSE3(Matrix4 const& T) { + return SE3(SO3::fitToSO3(T.template block<3, 3>(0, 0)), + T.template block<3, 1>(0, 3)); + } + + /// Returns the ith infinitesimal generators of SE(3). + /// + /// The infinitesimal generators of SE(3) are: + /// + /// ``` + /// | 0 0 0 1 | + /// G_0 = | 0 0 0 0 | + /// | 0 0 0 0 | + /// | 0 0 0 0 | + /// + /// | 0 0 0 0 | + /// G_1 = | 0 0 0 1 | + /// | 0 0 0 0 | + /// | 0 0 0 0 | + /// + /// | 0 0 0 0 | + /// G_2 = | 0 0 0 0 | + /// | 0 0 0 1 | + /// | 0 0 0 0 | + /// + /// | 0 0 0 0 | + /// G_3 = | 0 0 -1 0 | + /// | 0 1 0 0 | + /// | 0 0 0 0 | + /// + /// | 0 0 1 0 | + /// G_4 = | 0 0 0 0 | + /// | -1 0 0 0 | + /// | 0 0 0 0 | + /// + /// | 0 -1 0 0 | + /// G_5 = | 1 0 0 0 | + /// | 0 0 0 0 | + /// | 0 0 0 0 | + /// ``` + /// + /// Precondition: ``i`` must be in [0, 5]. + /// + SOPHUS_FUNC static Transformation generator(int i) { + SOPHUS_ENSURE(i >= 0 && i <= 5, "i should be in range [0,5]."); + Tangent e; + e.setZero(); + e[i] = Scalar(1); + return hat(e); + } + + /// hat-operator + /// + /// It takes in the 6-vector representation (= twist) and returns the + /// corresponding matrix representation of Lie algebra element. + /// + /// Formally, the hat()-operator of SE(3) is defined as + /// + /// ``hat(.): R^6 -> R^{4x4}, hat(a) = sum_i a_i * G_i`` (for i=0,...,5) + /// + /// with ``G_i`` being the ith infinitesimal generator of SE(3). + /// + /// The corresponding inverse is the vee()-operator, see below. + /// + SOPHUS_FUNC static Transformation hat(Tangent const& a) { + Transformation Omega; + Omega.setZero(); + Omega.template topLeftCorner<3, 3>() = + SO3::hat(a.template tail<3>()); + Omega.col(3).template head<3>() = a.template head<3>(); + return Omega; + } + + /// Lie bracket + /// + /// It computes the Lie bracket of SE(3). To be more specific, it computes + /// + /// ``[omega_1, omega_2]_se3 := vee([hat(omega_1), hat(omega_2)])`` + /// + /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the + /// hat()-operator and ``vee(.)`` the vee()-operator of SE(3). + /// + SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) { + Vector3 const upsilon1 = a.template head<3>(); + Vector3 const upsilon2 = b.template head<3>(); + Vector3 const omega1 = a.template tail<3>(); + Vector3 const omega2 = b.template tail<3>(); + + Tangent res; + res.template head<3>() = omega1.cross(upsilon2) + upsilon1.cross(omega2); + res.template tail<3>() = omega1.cross(omega2); + + return res; + } + + /// Construct x-axis rotation. + /// + static SOPHUS_FUNC SE3 rotX(Scalar const& x) { + return SE3(SO3::rotX(x), Sophus::Vector3::Zero()); + } + + /// Construct y-axis rotation. + /// + static SOPHUS_FUNC SE3 rotY(Scalar const& y) { + return SE3(SO3::rotY(y), Sophus::Vector3::Zero()); + } + + /// Construct z-axis rotation. + /// + static SOPHUS_FUNC SE3 rotZ(Scalar const& z) { + return SE3(SO3::rotZ(z), Sophus::Vector3::Zero()); + } + + /// Draw uniform sample from SE(3) manifold. + /// + /// Translations are drawn component-wise from the range [-1, 1]. + /// + template + static SE3 sampleUniform(UniformRandomBitGenerator& generator) { + std::uniform_real_distribution uniform(Scalar(-1), Scalar(1)); + return SE3(SO3::sampleUniform(generator), + Vector3(uniform(generator), uniform(generator), + uniform(generator))); + } + + /// Construct a translation only SE3 instance. + /// + template + static SOPHUS_FUNC SE3 trans(T0 const& x, T1 const& y, T2 const& z) { + return SE3(SO3(), Vector3(x, y, z)); + } + + static SOPHUS_FUNC SE3 trans(Vector3 const& xyz) { + return SE3(SO3(), xyz); + } + + /// Construct x-axis translation. + /// + static SOPHUS_FUNC SE3 transX(Scalar const& x) { + return SE3::trans(x, Scalar(0), Scalar(0)); + } + + /// Construct y-axis translation. + /// + static SOPHUS_FUNC SE3 transY(Scalar const& y) { + return SE3::trans(Scalar(0), y, Scalar(0)); + } + + /// Construct z-axis translation. + /// + static SOPHUS_FUNC SE3 transZ(Scalar const& z) { + return SE3::trans(Scalar(0), Scalar(0), z); + } + + /// vee-operator + /// + /// It takes 4x4-matrix representation ``Omega`` and maps it to the + /// corresponding 6-vector representation of Lie algebra. + /// + /// This is the inverse of the hat()-operator, see above. + /// + /// Precondition: ``Omega`` must have the following structure: + /// + /// | 0 -f e a | + /// | f 0 -d b | + /// | -e d 0 c + /// | 0 0 0 0 | . + /// + SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { + Tangent upsilon_omega; + upsilon_omega.template head<3>() = Omega.col(3).template head<3>(); + upsilon_omega.template tail<3>() = + SO3::vee(Omega.template topLeftCorner<3, 3>()); + return upsilon_omega; + } + + protected: + SO3Member so3_; + TranslationMember translation_; +}; + +template +SE3::SE3() : translation_(TranslationMember::Zero()) { + static_assert(std::is_standard_layout::value, + "Assume standard layout for the use of offsetof check below."); + static_assert( + offsetof(SE3, so3_) + sizeof(Scalar) * SO3::num_parameters == + offsetof(SE3, translation_), + "This class assumes packed storage and hence will only work " + "correctly depending on the compiler (options) - in " + "particular when using [this->data(), this-data() + " + "num_parameters] to access the raw data in a contiguous fashion."); +} +} // namespace Sophus + +namespace Eigen { + +/// Specialization of Eigen::Map for ``SE3``; derived from SE3Base. +/// +/// Allows us to wrap SE3 objects around POD array. +template +class Map, Options> + : public Sophus::SE3Base, Options>> { + public: + using Base = Sophus::SE3Base, Options>>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + + using Base::operator=; + using Base::operator*=; + using Base::operator*; + + SOPHUS_FUNC Map(Scalar* coeffs) + : so3_(coeffs), + translation_(coeffs + Sophus::SO3::num_parameters) {} + + /// Mutator of SO3 + /// + SOPHUS_FUNC Map, Options>& so3() { return so3_; } + + /// Accessor of SO3 + /// + SOPHUS_FUNC Map, Options> const& so3() const { + return so3_; + } + + /// Mutator of translation vector + /// + SOPHUS_FUNC Map>& translation() { + return translation_; + } + + /// Accessor of translation vector + /// + SOPHUS_FUNC Map> const& translation() const { + return translation_; + } + + protected: + Map, Options> so3_; + Map, Options> translation_; +}; + +/// Specialization of Eigen::Map for ``SE3 const``; derived from SE3Base. +/// +/// Allows us to wrap SE3 objects around POD array. +template +class Map const, Options> + : public Sophus::SE3Base const, Options>> { + public: + using Base = Sophus::SE3Base const, Options>>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + + using Base::operator*=; + using Base::operator*; + + SOPHUS_FUNC Map(Scalar const* coeffs) + : so3_(coeffs), + translation_(coeffs + Sophus::SO3::num_parameters) {} + + /// Accessor of SO3 + /// + SOPHUS_FUNC Map const, Options> const& so3() const { + return so3_; + } + + /// Accessor of translation vector + /// + SOPHUS_FUNC Map const, Options> const& translation() + const { + return translation_; + } + + protected: + Map const, Options> const so3_; + Map const, Options> const translation_; +}; +} // namespace Eigen + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/sim2.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/sim2.hpp new file mode 100644 index 0000000..25c946e --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/sim2.hpp @@ -0,0 +1,722 @@ +/// @file +/// Similarity group Sim(2) - scaling, rotation and translation in 2d. + +#ifndef SOPHUS_SIM2_HPP +#define SOPHUS_SIM2_HPP + +#include "rxso2.hpp" +#include "sim_details.hpp" + +namespace Sophus { +template +class Sim2; +using Sim2d = Sim2; +using Sim2f = Sim2; +} // namespace Sophus + +namespace Eigen { +namespace internal { + +template +struct traits> { + using Scalar = Scalar_; + using TranslationType = Sophus::Vector2; + using RxSO2Type = Sophus::RxSO2; +}; + +template +struct traits, Options>> + : traits> { + using Scalar = Scalar_; + using TranslationType = Map, Options>; + using RxSO2Type = Map, Options>; +}; + +template +struct traits const, Options>> + : traits const> { + using Scalar = Scalar_; + using TranslationType = Map const, Options>; + using RxSO2Type = Map const, Options>; +}; +} // namespace internal +} // namespace Eigen + +namespace Sophus { + +/// Sim2 base type - implements Sim2 class but is storage agnostic. +/// +/// Sim(2) is the group of rotations and translation and scaling in 2d. It is +/// the semi-direct product of R+xSO(2) and the 2d Euclidean vector space. The +/// class is represented using a composition of RxSO2 for scaling plus +/// rotation and a 2-vector for translation. +/// +/// Sim(2) is neither compact, nor a commutative group. +/// +/// See RxSO2 for more details of the scaling + rotation representation in 2d. +/// +template +class Sim2Base { + public: + using Scalar = typename Eigen::internal::traits::Scalar; + using TranslationType = + typename Eigen::internal::traits::TranslationType; + using RxSO2Type = typename Eigen::internal::traits::RxSO2Type; + + /// Degrees of freedom of manifold, number of dimensions in tangent space + /// (two for translation, one for rotation and one for scaling). + static int constexpr DoF = 4; + /// Number of internal parameters used (2-tuple for complex number, two for + /// translation). + static int constexpr num_parameters = 4; + /// Group transformations are 3x3 matrices. + static int constexpr N = 3; + using Transformation = Matrix; + using Point = Vector2; + using HomogeneousPoint = Vector3; + using Line = ParametrizedLine2; + using Tangent = Vector; + using Adjoint = Matrix; + + /// For binary operations the return type is determined with the + /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map + /// types, as well as other compatible scalar types such as Ceres::Jet and + /// double scalars with SIM2 operations. + template + using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< + Scalar, typename OtherDerived::Scalar>::ReturnType; + + template + using Sim2Product = Sim2>; + + template + using PointProduct = Vector2>; + + template + using HomogeneousPointProduct = Vector3>; + + /// Adjoint transformation + /// + /// This function return the adjoint transformation ``Ad`` of the group + /// element ``A`` such that for all ``x`` it holds that + /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. + /// + SOPHUS_FUNC Adjoint Adj() const { + Adjoint res; + res.setZero(); + res.template block<2, 2>(0, 0) = rxso2().matrix(); + res(0, 2) = translation()[1]; + res(1, 2) = -translation()[0]; + res.template block<2, 1>(0, 3) = -translation(); + + res(2, 2) = Scalar(1); + + res(3, 3) = Scalar(1); + return res; + } + + /// Returns copy of instance casted to NewScalarType. + /// + template + SOPHUS_FUNC Sim2 cast() const { + return Sim2(rxso2().template cast(), + translation().template cast()); + } + + /// Returns group inverse. + /// + SOPHUS_FUNC Sim2 inverse() const { + RxSO2 invR = rxso2().inverse(); + return Sim2(invR, invR * (translation() * Scalar(-1))); + } + + /// Logarithmic map + /// + /// Computes the logarithm, the inverse of the group exponential which maps + /// element of the group (rigid body transformations) to elements of the + /// tangent space (twist). + /// + /// To be specific, this function computes ``vee(logmat(.))`` with + /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator + /// of Sim(2). + /// + SOPHUS_FUNC Tangent log() const { + /// The derivation of the closed-form Sim(2) logarithm for is done + /// analogously to the closed-form solution of the SE(2) logarithm, see + /// J. Gallier, D. Xu, "Computing exponentials of skew symmetric matrices + /// and logarithms of orthogonal matrices", IJRA 2002. + /// https:///pdfs.semanticscholar.org/cfe3/e4b39de63c8cabd89bf3feff7f5449fc981d.pdf + /// (Sec. 6., pp. 8) + Tangent res; + Vector2 const theta_sigma = rxso2().log(); + Scalar const theta = theta_sigma[0]; + Scalar const sigma = theta_sigma[1]; + Matrix2 const Omega = SO2::hat(theta); + Matrix2 const W_inv = + details::calcWInv(Omega, theta, sigma, scale()); + + res.segment(0, 2) = W_inv * translation(); + res[2] = theta; + res[3] = sigma; + return res; + } + + /// Returns 3x3 matrix representation of the instance. + /// + /// It has the following form: + /// + /// | s*R t | + /// | o 1 | + /// + /// where ``R`` is a 2x2 rotation matrix, ``s`` a scale factor, ``t`` a + /// translation 2-vector and ``o`` a 2-column vector of zeros. + /// + SOPHUS_FUNC Transformation matrix() const { + Transformation homogenious_matrix; + homogenious_matrix.template topLeftCorner<2, 3>() = matrix2x3(); + homogenious_matrix.row(2) = + Matrix(Scalar(0), Scalar(0), Scalar(1)); + return homogenious_matrix; + } + + /// Returns the significant first two rows of the matrix above. + /// + SOPHUS_FUNC Matrix matrix2x3() const { + Matrix matrix; + matrix.template topLeftCorner<2, 2>() = rxso2().matrix(); + matrix.col(2) = translation(); + return matrix; + } + + /// Assignment-like operator from OtherDerived. + /// + template + SOPHUS_FUNC Sim2Base& operator=( + Sim2Base const& other) { + rxso2() = other.rxso2(); + translation() = other.translation(); + return *this; + } + + /// Group multiplication, which is rotation plus scaling concatenation. + /// + /// Note: That scaling is calculated with saturation. See RxSO2 for + /// details. + /// + template + SOPHUS_FUNC Sim2Product operator*( + Sim2Base const& other) const { + return Sim2Product( + rxso2() * other.rxso2(), translation() + rxso2() * other.translation()); + } + + /// Group action on 2-points. + /// + /// This function rotates, scales and translates a two dimensional point + /// ``p`` by the Sim(2) element ``(bar_sR_foo, t_bar)`` (= similarity + /// transformation): + /// + /// ``p_bar = bar_sR_foo * p_foo + t_bar``. + /// + template ::value>::type> + SOPHUS_FUNC PointProduct operator*( + Eigen::MatrixBase const& p) const { + return rxso2() * p + translation(); + } + + /// Group action on homogeneous 2-points. See above for more details. + /// + template ::value>::type> + SOPHUS_FUNC HomogeneousPointProduct operator*( + Eigen::MatrixBase const& p) const { + const PointProduct tp = + rxso2() * p.template head<2>() + p(2) * translation(); + return HomogeneousPointProduct(tp(0), tp(1), p(2)); + } + + /// Group action on lines. + /// + /// This function rotates, scales and translates a parametrized line + /// ``l(t) = o + t * d`` by the Sim(2) element: + /// + /// Origin ``o`` is rotated, scaled and translated + /// Direction ``d`` is rotated + /// + SOPHUS_FUNC Line operator*(Line const& l) const { + Line rotatedLine = rxso2() * l; + return Line(rotatedLine.origin() + translation(), rotatedLine.direction()); + } + + /// Returns internal parameters of Sim(2). + /// + /// It returns (c[0], c[1], t[0], t[1]), + /// with c being the complex number, t the translation 3-vector. + /// + SOPHUS_FUNC Sophus::Vector params() const { + Sophus::Vector p; + p << rxso2().params(), translation(); + return p; + } + + /// In-place group multiplication. This method is only valid if the return + /// type of the multiplication is compatible with this SO2's Scalar type. + /// + template >::value>::type> + SOPHUS_FUNC Sim2Base& operator*=( + Sim2Base const& other) { + *static_cast(this) = *this * other; + return *this; + } + + /// Setter of non-zero complex number. + /// + /// Precondition: ``z`` must not be close to zero. + /// + SOPHUS_FUNC void setComplex(Vector2 const& z) { + rxso2().setComplex(z); + } + + /// Accessor of complex number. + /// + SOPHUS_FUNC + typename Eigen::internal::traits::RxSO2Type::ComplexType const& + complex() const { + return rxso2().complex(); + } + + /// Returns Rotation matrix + /// + SOPHUS_FUNC Matrix2 rotationMatrix() const { + return rxso2().rotationMatrix(); + } + + /// Mutator of SO2 group. + /// + SOPHUS_FUNC RxSO2Type& rxso2() { + return static_cast(this)->rxso2(); + } + + /// Accessor of SO2 group. + /// + SOPHUS_FUNC RxSO2Type const& rxso2() const { + return static_cast(this)->rxso2(); + } + + /// Returns scale. + /// + SOPHUS_FUNC Scalar scale() const { return rxso2().scale(); } + + /// Setter of complex number using rotation matrix ``R``, leaves scale as is. + /// + SOPHUS_FUNC void setRotationMatrix(Matrix2& R) { + rxso2().setRotationMatrix(R); + } + + /// Sets scale and leaves rotation as is. + /// + /// Note: This function as a significant computational cost, since it has to + /// call the square root twice. + /// + SOPHUS_FUNC void setScale(Scalar const& scale) { rxso2().setScale(scale); } + + /// Setter of complexnumber using scaled rotation matrix ``sR``. + /// + /// Precondition: The 2x2 matrix must be "scaled orthogonal" + /// and have a positive determinant. + /// + SOPHUS_FUNC void setScaledRotationMatrix(Matrix2 const& sR) { + rxso2().setScaledRotationMatrix(sR); + } + + /// Mutator of translation vector + /// + SOPHUS_FUNC TranslationType& translation() { + return static_cast(this)->translation(); + } + + /// Accessor of translation vector + /// + SOPHUS_FUNC TranslationType const& translation() const { + return static_cast(this)->translation(); + } +}; + +/// Sim2 using default storage; derived from Sim2Base. +template +class Sim2 : public Sim2Base> { + public: + using Base = Sim2Base>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + using RxSo2Member = RxSO2; + using TranslationMember = Vector2; + + using Base::operator=; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// Default constructor initializes similarity transform to the identity. + /// + SOPHUS_FUNC Sim2(); + + /// Copy constructor + /// + SOPHUS_FUNC Sim2(Sim2 const& other) = default; + + /// Copy-like constructor from OtherDerived. + /// + template + SOPHUS_FUNC Sim2(Sim2Base const& other) + : rxso2_(other.rxso2()), translation_(other.translation()) { + static_assert(std::is_same::value, + "must be same Scalar type"); + } + + /// Constructor from RxSO2 and translation vector + /// + template + SOPHUS_FUNC Sim2(RxSO2Base const& rxso2, + Eigen::MatrixBase const& translation) + : rxso2_(rxso2), translation_(translation) { + static_assert(std::is_same::value, + "must be same Scalar type"); + static_assert(std::is_same::value, + "must be same Scalar type"); + } + + /// Constructor from complex number and translation vector. + /// + /// Precondition: complex number must not be close to zero. + /// + template + SOPHUS_FUNC Sim2(Vector2 const& complex_number, + Eigen::MatrixBase const& translation) + : rxso2_(complex_number), translation_(translation) { + static_assert(std::is_same::value, + "must be same Scalar type"); + } + + /// Constructor from 3x3 matrix + /// + /// Precondition: Top-left 2x2 matrix needs to be "scaled-orthogonal" with + /// positive determinant. The last row must be ``(0, 0, 1)``. + /// + SOPHUS_FUNC explicit Sim2(Matrix const& T) + : rxso2_((T.template topLeftCorner<2, 2>()).eval()), + translation_(T.template block<2, 1>(0, 2)) {} + + /// This provides unsafe read/write access to internal data. Sim(2) is + /// represented by a complex number (two parameters) and a 2-vector. When + /// using direct write access, the user needs to take care of that the + /// complex number is not set close to zero. + /// + SOPHUS_FUNC Scalar* data() { + // rxso2_ and translation_ are laid out sequentially with no padding + return rxso2_.data(); + } + + /// Const version of data() above. + /// + SOPHUS_FUNC Scalar const* data() const { + // rxso2_ and translation_ are laid out sequentially with no padding + return rxso2_.data(); + } + + /// Accessor of RxSO2 + /// + SOPHUS_FUNC RxSo2Member& rxso2() { return rxso2_; } + + /// Mutator of RxSO2 + /// + SOPHUS_FUNC RxSo2Member const& rxso2() const { return rxso2_; } + + /// Mutator of translation vector + /// + SOPHUS_FUNC TranslationMember& translation() { return translation_; } + + /// Accessor of translation vector + /// + SOPHUS_FUNC TranslationMember const& translation() const { + return translation_; + } + + /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. + /// + SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { + return generator(i); + } + + /// Derivative of Lie bracket with respect to first element. + /// + /// This function returns ``D_a [a, b]`` with ``D_a`` being the + /// differential operator with respect to ``a``, ``[a, b]`` being the lie + /// bracket of the Lie algebra sim(2). + /// See ``lieBracket()`` below. + /// + + /// Group exponential + /// + /// This functions takes in an element of tangent space and returns the + /// corresponding element of the group Sim(2). + /// + /// The first two components of ``a`` represent the translational part + /// ``upsilon`` in the tangent space of Sim(2), the following two components + /// of ``a`` represents the rotation ``theta`` and the final component + /// represents the logarithm of the scaling factor ``sigma``. + /// To be more specific, this function computes ``expmat(hat(a))`` with + /// ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator + /// of Sim(2), see below. + /// + SOPHUS_FUNC static Sim2 exp(Tangent const& a) { + // For the derivation of the exponential map of Sim(N) see + // H. Strasdat, "Local Accuracy and Global Consistency for Efficient Visual + // SLAM", PhD thesis, 2012. + // http:///hauke.strasdat.net/files/strasdat_thesis_2012.pdf (A.5, pp. 186) + Vector2 const upsilon = a.segment(0, 2); + Scalar const theta = a[2]; + Scalar const sigma = a[3]; + RxSO2 rxso2 = RxSO2::exp(a.template tail<2>()); + Matrix2 const Omega = SO2::hat(theta); + Matrix2 const W = details::calcW(Omega, theta, sigma); + return Sim2(rxso2, W * upsilon); + } + + /// Returns the ith infinitesimal generators of Sim(2). + /// + /// The infinitesimal generators of Sim(2) are: + /// + /// ``` + /// | 0 0 1 | + /// G_0 = | 0 0 0 | + /// | 0 0 0 | + /// + /// | 0 0 0 | + /// G_1 = | 0 0 1 | + /// | 0 0 0 | + /// + /// | 0 -1 0 | + /// G_2 = | 1 0 0 | + /// | 0 0 0 | + /// + /// | 1 0 0 | + /// G_3 = | 0 1 0 | + /// | 0 0 0 | + /// ``` + /// + /// Precondition: ``i`` must be in [0, 3]. + /// + SOPHUS_FUNC static Transformation generator(int i) { + SOPHUS_ENSURE(i >= 0 || i <= 3, "i should be in range [0,3]."); + Tangent e; + e.setZero(); + e[i] = Scalar(1); + return hat(e); + } + + /// hat-operator + /// + /// It takes in the 4-vector representation and returns the corresponding + /// matrix representation of Lie algebra element. + /// + /// Formally, the hat()-operator of Sim(2) is defined as + /// + /// ``hat(.): R^4 -> R^{3x3}, hat(a) = sum_i a_i * G_i`` (for i=0,...,6) + /// + /// with ``G_i`` being the ith infinitesimal generator of Sim(2). + /// + /// The corresponding inverse is the vee()-operator, see below. + /// + SOPHUS_FUNC static Transformation hat(Tangent const& a) { + Transformation Omega; + Omega.template topLeftCorner<2, 2>() = + RxSO2::hat(a.template tail<2>()); + Omega.col(2).template head<2>() = a.template head<2>(); + Omega.row(2).setZero(); + return Omega; + } + + /// Lie bracket + /// + /// It computes the Lie bracket of Sim(2). To be more specific, it computes + /// + /// ``[omega_1, omega_2]_sim2 := vee([hat(omega_1), hat(omega_2)])`` + /// + /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the + /// hat()-operator and ``vee(.)`` the vee()-operator of Sim(2). + /// + SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) { + Vector2 const upsilon1 = a.template head<2>(); + Vector2 const upsilon2 = b.template head<2>(); + Scalar const theta1 = a[2]; + Scalar const theta2 = b[2]; + Scalar const sigma1 = a[3]; + Scalar const sigma2 = b[3]; + + Tangent res; + res[0] = -theta1 * upsilon2[1] + theta2 * upsilon1[1] + + sigma1 * upsilon2[0] - sigma2 * upsilon1[0]; + res[1] = theta1 * upsilon2[0] - theta2 * upsilon1[0] + + sigma1 * upsilon2[1] - sigma2 * upsilon1[1]; + res[2] = Scalar(0); + res[3] = Scalar(0); + + return res; + } + + /// Draw uniform sample from Sim(2) manifold. + /// + /// Translations are drawn component-wise from the range [-1, 1]. + /// The scale factor is drawn uniformly in log2-space from [-1, 1], + /// hence the scale is in [0.5, 2]. + /// + template + static Sim2 sampleUniform(UniformRandomBitGenerator& generator) { + std::uniform_real_distribution uniform(Scalar(-1), Scalar(1)); + return Sim2(RxSO2::sampleUniform(generator), + Vector2(uniform(generator), uniform(generator))); + } + + /// vee-operator + /// + /// It takes the 3x3-matrix representation ``Omega`` and maps it to the + /// corresponding 4-vector representation of Lie algebra. + /// + /// This is the inverse of the hat()-operator, see above. + /// + /// Precondition: ``Omega`` must have the following structure: + /// + /// | d -c a | + /// | c d b | + /// | 0 0 0 | + /// + SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { + Tangent upsilon_omega_sigma; + upsilon_omega_sigma.template head<2>() = Omega.col(2).template head<2>(); + upsilon_omega_sigma.template tail<2>() = + RxSO2::vee(Omega.template topLeftCorner<2, 2>()); + return upsilon_omega_sigma; + } + + protected: + RxSo2Member rxso2_; + TranslationMember translation_; +}; + +template +Sim2::Sim2() : translation_(TranslationMember::Zero()) { + static_assert(std::is_standard_layout::value, + "Assume standard layout for the use of offsetof check below."); + static_assert( + offsetof(Sim2, rxso2_) + sizeof(Scalar) * RxSO2::num_parameters == + offsetof(Sim2, translation_), + "This class assumes packed storage and hence will only work " + "correctly depending on the compiler (options) - in " + "particular when using [this->data(), this-data() + " + "num_parameters] to access the raw data in a contiguous fashion."); +} + +} // namespace Sophus + +namespace Eigen { + +/// Specialization of Eigen::Map for ``Sim2``; derived from Sim2Base. +/// +/// Allows us to wrap Sim2 objects around POD array. +template +class Map, Options> + : public Sophus::Sim2Base, Options>> { + public: + using Base = Sophus::Sim2Base, Options>>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + + using Base::operator=; + using Base::operator*=; + using Base::operator*; + + SOPHUS_FUNC Map(Scalar* coeffs) + : rxso2_(coeffs), + translation_(coeffs + Sophus::RxSO2::num_parameters) {} + + /// Mutator of RxSO2 + /// + SOPHUS_FUNC Map, Options>& rxso2() { return rxso2_; } + + /// Accessor of RxSO2 + /// + SOPHUS_FUNC Map, Options> const& rxso2() const { + return rxso2_; + } + + /// Mutator of translation vector + /// + SOPHUS_FUNC Map, Options>& translation() { + return translation_; + } + + /// Accessor of translation vector + SOPHUS_FUNC Map, Options> const& translation() const { + return translation_; + } + + protected: + Map, Options> rxso2_; + Map, Options> translation_; +}; + +/// Specialization of Eigen::Map for ``Sim2 const``; derived from Sim2Base. +/// +/// Allows us to wrap RxSO2 objects around POD array. +template +class Map const, Options> + : public Sophus::Sim2Base const, Options>> { + public: + using Base = Sophus::Sim2Base const, Options>>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + + using Base::operator*=; + using Base::operator*; + + SOPHUS_FUNC Map(Scalar const* coeffs) + : rxso2_(coeffs), + translation_(coeffs + Sophus::RxSO2::num_parameters) {} + + /// Accessor of RxSO2 + /// + SOPHUS_FUNC Map const, Options> const& rxso2() const { + return rxso2_; + } + + /// Accessor of translation vector + /// + SOPHUS_FUNC Map const, Options> const& translation() + const { + return translation_; + } + + protected: + Map const, Options> const rxso2_; + Map const, Options> const translation_; +}; +} // namespace Eigen + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/sim3.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/sim3.hpp new file mode 100644 index 0000000..5e51052 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/sim3.hpp @@ -0,0 +1,740 @@ +/// @file +/// Similarity group Sim(3) - scaling, rotation and translation in 3d. + +#ifndef SOPHUS_SIM3_HPP +#define SOPHUS_SIM3_HPP + +#include "rxso3.hpp" +#include "sim_details.hpp" + +namespace Sophus { +template +class Sim3; +using Sim3d = Sim3; +using Sim3f = Sim3; +} // namespace Sophus + +namespace Eigen { +namespace internal { + +template +struct traits> { + using Scalar = Scalar_; + using TranslationType = Sophus::Vector3; + using RxSO3Type = Sophus::RxSO3; +}; + +template +struct traits, Options>> + : traits> { + using Scalar = Scalar_; + using TranslationType = Map, Options>; + using RxSO3Type = Map, Options>; +}; + +template +struct traits const, Options>> + : traits const> { + using Scalar = Scalar_; + using TranslationType = Map const, Options>; + using RxSO3Type = Map const, Options>; +}; +} // namespace internal +} // namespace Eigen + +namespace Sophus { + +/// Sim3 base type - implements Sim3 class but is storage agnostic. +/// +/// Sim(3) is the group of rotations and translation and scaling in 3d. It is +/// the semi-direct product of R+xSO(3) and the 3d Euclidean vector space. The +/// class is represented using a composition of RxSO3 for scaling plus +/// rotation and a 3-vector for translation. +/// +/// Sim(3) is neither compact, nor a commutative group. +/// +/// See RxSO3 for more details of the scaling + rotation representation in 3d. +/// +template +class Sim3Base { + public: + using Scalar = typename Eigen::internal::traits::Scalar; + using TranslationType = + typename Eigen::internal::traits::TranslationType; + using RxSO3Type = typename Eigen::internal::traits::RxSO3Type; + using QuaternionType = typename RxSO3Type::QuaternionType; + + /// Degrees of freedom of manifold, number of dimensions in tangent space + /// (three for translation, three for rotation and one for scaling). + static int constexpr DoF = 7; + /// Number of internal parameters used (4-tuple for quaternion, three for + /// translation). + static int constexpr num_parameters = 7; + /// Group transformations are 4x4 matrices. + static int constexpr N = 4; + using Transformation = Matrix; + using Point = Vector3; + using HomogeneousPoint = Vector4; + using Line = ParametrizedLine3; + using Tangent = Vector; + using Adjoint = Matrix; + + /// For binary operations the return type is determined with the + /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map + /// types, as well as other compatible scalar types such as Ceres::Jet and + /// double scalars with Sim3 operations. + template + using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< + Scalar, typename OtherDerived::Scalar>::ReturnType; + + template + using Sim3Product = Sim3>; + + template + using PointProduct = Vector3>; + + template + using HomogeneousPointProduct = Vector4>; + + /// Adjoint transformation + /// + /// This function return the adjoint transformation ``Ad`` of the group + /// element ``A`` such that for all ``x`` it holds that + /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. + /// + SOPHUS_FUNC Adjoint Adj() const { + Matrix3 const R = rxso3().rotationMatrix(); + Adjoint res; + res.setZero(); + res.template block<3, 3>(0, 0) = rxso3().matrix(); + res.template block<3, 3>(0, 3) = SO3::hat(translation()) * R; + res.template block<3, 1>(0, 6) = -translation(); + + res.template block<3, 3>(3, 3) = R; + + res(6, 6) = Scalar(1); + return res; + } + + /// Returns copy of instance casted to NewScalarType. + /// + template + SOPHUS_FUNC Sim3 cast() const { + return Sim3(rxso3().template cast(), + translation().template cast()); + } + + /// Returns group inverse. + /// + SOPHUS_FUNC Sim3 inverse() const { + RxSO3 invR = rxso3().inverse(); + return Sim3(invR, invR * (translation() * Scalar(-1))); + } + + /// Logarithmic map + /// + /// Computes the logarithm, the inverse of the group exponential which maps + /// element of the group (rigid body transformations) to elements of the + /// tangent space (twist). + /// + /// To be specific, this function computes ``vee(logmat(.))`` with + /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator + /// of Sim(3). + /// + SOPHUS_FUNC Tangent log() const { + // The derivation of the closed-form Sim(3) logarithm for is done + // analogously to the closed-form solution of the SE(3) logarithm, see + // J. Gallier, D. Xu, "Computing exponentials of skew symmetric matrices + // and logarithms of orthogonal matrices", IJRA 2002. + // https:///pdfs.semanticscholar.org/cfe3/e4b39de63c8cabd89bf3feff7f5449fc981d.pdf + // (Sec. 6., pp. 8) + Tangent res; + auto omega_sigma_and_theta = rxso3().logAndTheta(); + Vector3 const omega = + omega_sigma_and_theta.tangent.template head<3>(); + Scalar sigma = omega_sigma_and_theta.tangent[3]; + Matrix3 const Omega = SO3::hat(omega); + Matrix3 const W_inv = details::calcWInv( + Omega, omega_sigma_and_theta.theta, sigma, scale()); + + res.segment(0, 3) = W_inv * translation(); + res.segment(3, 3) = omega; + res[6] = sigma; + return res; + } + + /// Returns 4x4 matrix representation of the instance. + /// + /// It has the following form: + /// + /// | s*R t | + /// | o 1 | + /// + /// where ``R`` is a 3x3 rotation matrix, ``s`` a scale factor, ``t`` a + /// translation 3-vector and ``o`` a 3-column vector of zeros. + /// + SOPHUS_FUNC Transformation matrix() const { + Transformation homogenious_matrix; + homogenious_matrix.template topLeftCorner<3, 4>() = matrix3x4(); + homogenious_matrix.row(3) = + Matrix(Scalar(0), Scalar(0), Scalar(0), Scalar(1)); + return homogenious_matrix; + } + + /// Returns the significant first three rows of the matrix above. + /// + SOPHUS_FUNC Matrix matrix3x4() const { + Matrix matrix; + matrix.template topLeftCorner<3, 3>() = rxso3().matrix(); + matrix.col(3) = translation(); + return matrix; + } + + /// Assignment-like operator from OtherDerived. + /// + template + SOPHUS_FUNC Sim3Base& operator=( + Sim3Base const& other) { + rxso3() = other.rxso3(); + translation() = other.translation(); + return *this; + } + + /// Group multiplication, which is rotation plus scaling concatenation. + /// + /// Note: That scaling is calculated with saturation. See RxSO3 for + /// details. + /// + template + SOPHUS_FUNC Sim3Product operator*( + Sim3Base const& other) const { + return Sim3Product( + rxso3() * other.rxso3(), translation() + rxso3() * other.translation()); + } + + /// Group action on 3-points. + /// + /// This function rotates, scales and translates a three dimensional point + /// ``p`` by the Sim(3) element ``(bar_sR_foo, t_bar)`` (= similarity + /// transformation): + /// + /// ``p_bar = bar_sR_foo * p_foo + t_bar``. + /// + template ::value>::type> + SOPHUS_FUNC PointProduct operator*( + Eigen::MatrixBase const& p) const { + return rxso3() * p + translation(); + } + + /// Group action on homogeneous 3-points. See above for more details. + /// + template ::value>::type> + SOPHUS_FUNC HomogeneousPointProduct operator*( + Eigen::MatrixBase const& p) const { + const PointProduct tp = + rxso3() * p.template head<3>() + p(3) * translation(); + return HomogeneousPointProduct(tp(0), tp(1), tp(2), p(3)); + } + + /// Group action on lines. + /// + /// This function rotates, scales and translates a parametrized line + /// ``l(t) = o + t * d`` by the Sim(3) element: + /// + /// Origin ``o`` is rotated, scaled and translated + /// Direction ``d`` is rotated + /// + SOPHUS_FUNC Line operator*(Line const& l) const { + Line rotatedLine = rxso3() * l; + return Line(rotatedLine.origin() + translation(), rotatedLine.direction()); + } + + /// In-place group multiplication. This method is only valid if the return + /// type of the multiplication is compatible with this SO3's Scalar type. + /// + template >::value>::type> + SOPHUS_FUNC Sim3Base& operator*=( + Sim3Base const& other) { + *static_cast(this) = *this * other; + return *this; + } + + /// Returns internal parameters of Sim(3). + /// + /// It returns (q.imag[0], q.imag[1], q.imag[2], q.real, t[0], t[1], t[2]), + /// with q being the quaternion, t the translation 3-vector. + /// + SOPHUS_FUNC Sophus::Vector params() const { + Sophus::Vector p; + p << rxso3().params(), translation(); + return p; + } + + /// Setter of non-zero quaternion. + /// + /// Precondition: ``quat`` must not be close to zero. + /// + SOPHUS_FUNC void setQuaternion(Eigen::Quaternion const& quat) { + rxso3().setQuaternion(quat); + } + + /// Accessor of quaternion. + /// + SOPHUS_FUNC QuaternionType const& quaternion() const { + return rxso3().quaternion(); + } + + /// Returns Rotation matrix + /// + SOPHUS_FUNC Matrix3 rotationMatrix() const { + return rxso3().rotationMatrix(); + } + + /// Mutator of SO3 group. + /// + SOPHUS_FUNC RxSO3Type& rxso3() { + return static_cast(this)->rxso3(); + } + + /// Accessor of SO3 group. + /// + SOPHUS_FUNC RxSO3Type const& rxso3() const { + return static_cast(this)->rxso3(); + } + + /// Returns scale. + /// + SOPHUS_FUNC Scalar scale() const { return rxso3().scale(); } + + /// Setter of quaternion using rotation matrix ``R``, leaves scale as is. + /// + SOPHUS_FUNC void setRotationMatrix(Matrix3& R) { + rxso3().setRotationMatrix(R); + } + + /// Sets scale and leaves rotation as is. + /// + /// Note: This function as a significant computational cost, since it has to + /// call the square root twice. + /// + SOPHUS_FUNC void setScale(Scalar const& scale) { rxso3().setScale(scale); } + + /// Setter of quaternion using scaled rotation matrix ``sR``. + /// + /// Precondition: The 3x3 matrix must be "scaled orthogonal" + /// and have a positive determinant. + /// + SOPHUS_FUNC void setScaledRotationMatrix(Matrix3 const& sR) { + rxso3().setScaledRotationMatrix(sR); + } + + /// Mutator of translation vector + /// + SOPHUS_FUNC TranslationType& translation() { + return static_cast(this)->translation(); + } + + /// Accessor of translation vector + /// + SOPHUS_FUNC TranslationType const& translation() const { + return static_cast(this)->translation(); + } +}; + +/// Sim3 using default storage; derived from Sim3Base. +template +class Sim3 : public Sim3Base> { + public: + using Base = Sim3Base>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + using RxSo3Member = RxSO3; + using TranslationMember = Vector3; + + using Base::operator=; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// Default constructor initializes similarity transform to the identity. + /// + SOPHUS_FUNC Sim3(); + + /// Copy constructor + /// + SOPHUS_FUNC Sim3(Sim3 const& other) = default; + + /// Copy-like constructor from OtherDerived. + /// + template + SOPHUS_FUNC Sim3(Sim3Base const& other) + : rxso3_(other.rxso3()), translation_(other.translation()) { + static_assert(std::is_same::value, + "must be same Scalar type"); + } + + /// Constructor from RxSO3 and translation vector + /// + template + SOPHUS_FUNC Sim3(RxSO3Base const& rxso3, + Eigen::MatrixBase const& translation) + : rxso3_(rxso3), translation_(translation) { + static_assert(std::is_same::value, + "must be same Scalar type"); + static_assert(std::is_same::value, + "must be same Scalar type"); + } + + /// Constructor from quaternion and translation vector. + /// + /// Precondition: quaternion must not be close to zero. + /// + template + SOPHUS_FUNC Sim3(Eigen::QuaternionBase const& quaternion, + Eigen::MatrixBase const& translation) + : rxso3_(quaternion), translation_(translation) { + static_assert(std::is_same::value, + "must be same Scalar type"); + static_assert(std::is_same::value, + "must be same Scalar type"); + } + + /// Constructor from 4x4 matrix + /// + /// Precondition: Top-left 3x3 matrix needs to be "scaled-orthogonal" with + /// positive determinant. The last row must be ``(0, 0, 0, 1)``. + /// + SOPHUS_FUNC explicit Sim3(Matrix const& T) + : rxso3_(T.template topLeftCorner<3, 3>()), + translation_(T.template block<3, 1>(0, 3)) {} + + /// This provides unsafe read/write access to internal data. Sim(3) is + /// represented by an Eigen::Quaternion (four parameters) and a 3-vector. When + /// using direct write access, the user needs to take care of that the + /// quaternion is not set close to zero. + /// + SOPHUS_FUNC Scalar* data() { + // rxso3_ and translation_ are laid out sequentially with no padding + return rxso3_.data(); + } + + /// Const version of data() above. + /// + SOPHUS_FUNC Scalar const* data() const { + // rxso3_ and translation_ are laid out sequentially with no padding + return rxso3_.data(); + } + + /// Accessor of RxSO3 + /// + SOPHUS_FUNC RxSo3Member& rxso3() { return rxso3_; } + + /// Mutator of RxSO3 + /// + SOPHUS_FUNC RxSo3Member const& rxso3() const { return rxso3_; } + + /// Mutator of translation vector + /// + SOPHUS_FUNC TranslationMember& translation() { return translation_; } + + /// Accessor of translation vector + /// + SOPHUS_FUNC TranslationMember const& translation() const { + return translation_; + } + + /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. + /// + SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { + return generator(i); + } + + /// Group exponential + /// + /// This functions takes in an element of tangent space and returns the + /// corresponding element of the group Sim(3). + /// + /// The first three components of ``a`` represent the translational part + /// ``upsilon`` in the tangent space of Sim(3), the following three components + /// of ``a`` represents the rotation vector ``omega`` and the final component + /// represents the logarithm of the scaling factor ``sigma``. + /// To be more specific, this function computes ``expmat(hat(a))`` with + /// ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator + /// of Sim(3), see below. + /// + SOPHUS_FUNC static Sim3 exp(Tangent const& a) { + // For the derivation of the exponential map of Sim(3) see + // H. Strasdat, "Local Accuracy and Global Consistency for Efficient Visual + // SLAM", PhD thesis, 2012. + // http:///hauke.strasdat.net/files/strasdat_thesis_2012.pdf (A.5, pp. 186) + Vector3 const upsilon = a.segment(0, 3); + Vector3 const omega = a.segment(3, 3); + Scalar const sigma = a[6]; + Scalar theta; + RxSO3 rxso3 = + RxSO3::expAndTheta(a.template tail<4>(), &theta); + Matrix3 const Omega = SO3::hat(omega); + + Matrix3 const W = details::calcW(Omega, theta, sigma); + return Sim3(rxso3, W * upsilon); + } + + /// Returns the ith infinitesimal generators of Sim(3). + /// + /// The infinitesimal generators of Sim(3) are: + /// + /// ``` + /// | 0 0 0 1 | + /// G_0 = | 0 0 0 0 | + /// | 0 0 0 0 | + /// | 0 0 0 0 | + /// + /// | 0 0 0 0 | + /// G_1 = | 0 0 0 1 | + /// | 0 0 0 0 | + /// | 0 0 0 0 | + /// + /// | 0 0 0 0 | + /// G_2 = | 0 0 0 0 | + /// | 0 0 0 1 | + /// | 0 0 0 0 | + /// + /// | 0 0 0 0 | + /// G_3 = | 0 0 -1 0 | + /// | 0 1 0 0 | + /// | 0 0 0 0 | + /// + /// | 0 0 1 0 | + /// G_4 = | 0 0 0 0 | + /// | -1 0 0 0 | + /// | 0 0 0 0 | + /// + /// | 0 -1 0 0 | + /// G_5 = | 1 0 0 0 | + /// | 0 0 0 0 | + /// | 0 0 0 0 | + /// + /// | 1 0 0 0 | + /// G_6 = | 0 1 0 0 | + /// | 0 0 1 0 | + /// | 0 0 0 0 | + /// ``` + /// + /// Precondition: ``i`` must be in [0, 6]. + /// + SOPHUS_FUNC static Transformation generator(int i) { + SOPHUS_ENSURE(i >= 0 || i <= 6, "i should be in range [0,6]."); + Tangent e; + e.setZero(); + e[i] = Scalar(1); + return hat(e); + } + + /// hat-operator + /// + /// It takes in the 7-vector representation and returns the corresponding + /// matrix representation of Lie algebra element. + /// + /// Formally, the hat()-operator of Sim(3) is defined as + /// + /// ``hat(.): R^7 -> R^{4x4}, hat(a) = sum_i a_i * G_i`` (for i=0,...,6) + /// + /// with ``G_i`` being the ith infinitesimal generator of Sim(3). + /// + /// The corresponding inverse is the vee()-operator, see below. + /// + SOPHUS_FUNC static Transformation hat(Tangent const& a) { + Transformation Omega; + Omega.template topLeftCorner<3, 3>() = + RxSO3::hat(a.template tail<4>()); + Omega.col(3).template head<3>() = a.template head<3>(); + Omega.row(3).setZero(); + return Omega; + } + + /// Lie bracket + /// + /// It computes the Lie bracket of Sim(3). To be more specific, it computes + /// + /// ``[omega_1, omega_2]_sim3 := vee([hat(omega_1), hat(omega_2)])`` + /// + /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the + /// hat()-operator and ``vee(.)`` the vee()-operator of Sim(3). + /// + SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) { + Vector3 const upsilon1 = a.template head<3>(); + Vector3 const upsilon2 = b.template head<3>(); + Vector3 const omega1 = a.template segment<3>(3); + Vector3 const omega2 = b.template segment<3>(3); + Scalar sigma1 = a[6]; + Scalar sigma2 = b[6]; + + Tangent res; + res.template head<3>() = SO3::hat(omega1) * upsilon2 + + SO3::hat(upsilon1) * omega2 + + sigma1 * upsilon2 - sigma2 * upsilon1; + res.template segment<3>(3) = omega1.cross(omega2); + res[6] = Scalar(0); + + return res; + } + + /// Draw uniform sample from Sim(3) manifold. + /// + /// Translations are drawn component-wise from the range [-1, 1]. + /// The scale factor is drawn uniformly in log2-space from [-1, 1], + /// hence the scale is in [0.5, 2]. + /// + template + static Sim3 sampleUniform(UniformRandomBitGenerator& generator) { + std::uniform_real_distribution uniform(Scalar(-1), Scalar(1)); + return Sim3(RxSO3::sampleUniform(generator), + Vector3(uniform(generator), uniform(generator), + uniform(generator))); + } + + /// vee-operator + /// + /// It takes the 4x4-matrix representation ``Omega`` and maps it to the + /// corresponding 7-vector representation of Lie algebra. + /// + /// This is the inverse of the hat()-operator, see above. + /// + /// Precondition: ``Omega`` must have the following structure: + /// + /// | g -f e a | + /// | f g -d b | + /// | -e d g c | + /// | 0 0 0 0 | + /// + SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { + Tangent upsilon_omega_sigma; + upsilon_omega_sigma.template head<3>() = Omega.col(3).template head<3>(); + upsilon_omega_sigma.template tail<4>() = + RxSO3::vee(Omega.template topLeftCorner<3, 3>()); + return upsilon_omega_sigma; + } + + protected: + RxSo3Member rxso3_; + TranslationMember translation_; +}; + +template +Sim3::Sim3() : translation_(TranslationMember::Zero()) { + static_assert(std::is_standard_layout::value, + "Assume standard layout for the use of offsetof check below."); + static_assert( + offsetof(Sim3, rxso3_) + sizeof(Scalar) * RxSO3::num_parameters == + offsetof(Sim3, translation_), + "This class assumes packed storage and hence will only work " + "correctly depending on the compiler (options) - in " + "particular when using [this->data(), this-data() + " + "num_parameters] to access the raw data in a contiguous fashion."); +} + +} // namespace Sophus + +namespace Eigen { + +/// Specialization of Eigen::Map for ``Sim3``; derived from Sim3Base. +/// +/// Allows us to wrap Sim3 objects around POD array. +template +class Map, Options> + : public Sophus::Sim3Base, Options>> { + public: + using Base = Sophus::Sim3Base, Options>>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + + using Base::operator=; + using Base::operator*=; + using Base::operator*; + + SOPHUS_FUNC Map(Scalar* coeffs) + : rxso3_(coeffs), + translation_(coeffs + Sophus::RxSO3::num_parameters) {} + + /// Mutator of RxSO3 + /// + SOPHUS_FUNC Map, Options>& rxso3() { return rxso3_; } + + /// Accessor of RxSO3 + /// + SOPHUS_FUNC Map, Options> const& rxso3() const { + return rxso3_; + } + + /// Mutator of translation vector + /// + SOPHUS_FUNC Map, Options>& translation() { + return translation_; + } + + /// Accessor of translation vector + SOPHUS_FUNC Map, Options> const& translation() const { + return translation_; + } + + protected: + Map, Options> rxso3_; + Map, Options> translation_; +}; + +/// Specialization of Eigen::Map for ``Sim3 const``; derived from Sim3Base. +/// +/// Allows us to wrap RxSO3 objects around POD array. +template +class Map const, Options> + : public Sophus::Sim3Base const, Options>> { + using Base = Sophus::Sim3Base const, Options>>; + + public: + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + + using Base::operator*=; + using Base::operator*; + + SOPHUS_FUNC Map(Scalar const* coeffs) + : rxso3_(coeffs), + translation_(coeffs + Sophus::RxSO3::num_parameters) {} + + /// Accessor of RxSO3 + /// + SOPHUS_FUNC Map const, Options> const& rxso3() const { + return rxso3_; + } + + /// Accessor of translation vector + /// + SOPHUS_FUNC Map const, Options> const& translation() + const { + return translation_; + } + + protected: + Map const, Options> const rxso3_; + Map const, Options> const translation_; +}; +} // namespace Eigen + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/sim_details.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/sim_details.hpp new file mode 100644 index 0000000..9b9f0ba --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/sim_details.hpp @@ -0,0 +1,103 @@ +#ifndef SOPHUS_SIM_DETAILS_HPP +#define SOPHUS_SIM_DETAILS_HPP + +#include "types.hpp" + +namespace Sophus { +namespace details { + +template +Matrix calcW(Matrix const& Omega, + Scalar const theta, Scalar const sigma) { + using std::abs; + using std::cos; + using std::exp; + using std::sin; + static Matrix const I = Matrix::Identity(); + static Scalar const one(1); + static Scalar const half(0.5); + Matrix const Omega2 = Omega * Omega; + Scalar const scale = exp(sigma); + Scalar A, B, C; + if (abs(sigma) < Constants::epsilon()) { + C = one; + if (abs(theta) < Constants::epsilon()) { + A = half; + B = Scalar(1. / 6.); + } else { + Scalar theta_sq = theta * theta; + A = (one - cos(theta)) / theta_sq; + B = (theta - sin(theta)) / (theta_sq * theta); + } + } else { + C = (scale - one) / sigma; + if (abs(theta) < Constants::epsilon()) { + Scalar sigma_sq = sigma * sigma; + A = ((sigma - one) * scale + one) / sigma_sq; + B = (scale * half * sigma_sq + scale - one - sigma * scale) / + (sigma_sq * sigma); + } else { + Scalar theta_sq = theta * theta; + Scalar a = scale * sin(theta); + Scalar b = scale * cos(theta); + Scalar c = theta_sq + sigma * sigma; + A = (a * sigma + (one - b) * theta) / (theta * c); + B = (C - ((b - one) * sigma + a * theta) / (c)) * one / (theta_sq); + } + } + return A * Omega + B * Omega2 + C * I; +} + +template +Matrix calcWInv(Matrix const& Omega, + Scalar const theta, Scalar const sigma, + Scalar const scale) { + using std::abs; + using std::cos; + using std::sin; + static Matrix const I = Matrix::Identity(); + static Scalar const half(0.5); + static Scalar const one(1); + static Scalar const two(2); + Matrix const Omega2 = Omega * Omega; + Scalar const scale_sq = scale * scale; + Scalar const theta_sq = theta * theta; + Scalar const sin_theta = sin(theta); + Scalar const cos_theta = cos(theta); + + Scalar a, b, c; + if (abs(sigma * sigma) < Constants::epsilon()) { + c = one - half * sigma; + a = -half; + if (abs(theta_sq) < Constants::epsilon()) { + b = Scalar(1. / 12.); + } else { + b = (theta * sin_theta + two * cos_theta - two) / + (two * theta_sq * (cos_theta - one)); + } + } else { + Scalar const scale_cu = scale_sq * scale; + c = sigma / (scale - one); + if (abs(theta_sq) < Constants::epsilon()) { + a = (-sigma * scale + scale - one) / ((scale - one) * (scale - one)); + b = (scale_sq * sigma - two * scale_sq + scale * sigma + two * scale) / + (two * scale_cu - Scalar(6) * scale_sq + Scalar(6) * scale - two); + } else { + Scalar const s_sin_theta = scale * sin_theta; + Scalar const s_cos_theta = scale * cos_theta; + a = (theta * s_cos_theta - theta - sigma * s_sin_theta) / + (theta * (scale_sq - two * s_cos_theta + one)); + b = -scale * + (theta * s_sin_theta - theta * sin_theta + sigma * s_cos_theta - + scale * sigma + sigma * cos_theta - sigma) / + (theta_sq * (scale_cu - two * scale * s_cos_theta - scale_sq + + two * s_cos_theta + scale - one)); + } + } + return a * Omega + b * Omega2 + c * I; +} + +} // namespace details +} // namespace Sophus + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/so2.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/so2.hpp new file mode 100644 index 0000000..2645449 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/so2.hpp @@ -0,0 +1,621 @@ +/// @file +/// Special orthogonal group SO(2) - rotation in 2d. + +#ifndef SOPHUS_SO2_HPP +#define SOPHUS_SO2_HPP + +#include +#include + +// Include only the selective set of Eigen headers that we need. +// This helps when using Sophus with unusual compilers, like nvcc. +#include + +#include "rotation_matrix.hpp" +#include "types.hpp" + +namespace Sophus { +template +class SO2; +using SO2d = SO2; +using SO2f = SO2; +} // namespace Sophus + +namespace Eigen { +namespace internal { + +template +struct traits> { + static constexpr int Options = Options_; + using Scalar = Scalar_; + using ComplexType = Sophus::Vector2; +}; + +template +struct traits, Options_>> + : traits> { + static constexpr int Options = Options_; + using Scalar = Scalar_; + using ComplexType = Map, Options>; +}; + +template +struct traits const, Options_>> + : traits const> { + static constexpr int Options = Options_; + using Scalar = Scalar_; + using ComplexType = Map const, Options>; +}; +} // namespace internal +} // namespace Eigen + +namespace Sophus { + +/// SO2 base type - implements SO2 class but is storage agnostic. +/// +/// SO(2) is the group of rotations in 2d. As a matrix group, it is the set of +/// matrices which are orthogonal such that ``R * R' = I`` (with ``R'`` being +/// the transpose of ``R``) and have a positive determinant. In particular, the +/// determinant is 1. Let ``theta`` be the rotation angle, the rotation matrix +/// can be written in close form: +/// +/// | cos(theta) -sin(theta) | +/// | sin(theta) cos(theta) | +/// +/// As a matter of fact, the first column of those matrices is isomorph to the +/// set of unit complex numbers U(1). Thus, internally, SO2 is represented as +/// complex number with length 1. +/// +/// SO(2) is a compact and commutative group. First it is compact since the set +/// of rotation matrices is a closed and bounded set. Second it is commutative +/// since ``R(alpha) * R(beta) = R(beta) * R(alpha)``, simply because ``alpha + +/// beta = beta + alpha`` with ``alpha`` and ``beta`` being rotation angles +/// (about the same axis). +/// +/// Class invariant: The 2-norm of ``unit_complex`` must be close to 1. +/// Technically speaking, it must hold that: +/// +/// ``|unit_complex().squaredNorm() - 1| <= Constants::epsilon()``. +template +class SO2Base { + public: + static constexpr int Options = Eigen::internal::traits::Options; + using Scalar = typename Eigen::internal::traits::Scalar; + using ComplexT = typename Eigen::internal::traits::ComplexType; + using ComplexTemporaryType = Sophus::Vector2; + + /// Degrees of freedom of manifold, number of dimensions in tangent space (one + /// since we only have in-plane rotations). + static int constexpr DoF = 1; + /// Number of internal parameters used (complex numbers are a tuples). + static int constexpr num_parameters = 2; + /// Group transformations are 2x2 matrices. + static int constexpr N = 2; + using Transformation = Matrix; + using Point = Vector2; + using HomogeneousPoint = Vector3; + using Line = ParametrizedLine2; + using Tangent = Scalar; + using Adjoint = Scalar; + + /// For binary operations the return type is determined with the + /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map + /// types, as well as other compatible scalar types such as Ceres::Jet and + /// double scalars with SO2 operations. + template + using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< + Scalar, typename OtherDerived::Scalar>::ReturnType; + + template + using SO2Product = SO2>; + + template + using PointProduct = Vector2>; + + template + using HomogeneousPointProduct = Vector3>; + + /// Adjoint transformation + /// + /// This function return the adjoint transformation ``Ad`` of the group + /// element ``A`` such that for all ``x`` it holds that + /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. + /// + /// It simply ``1``, since ``SO(2)`` is a commutative group. + /// + SOPHUS_FUNC Adjoint Adj() const { return Scalar(1); } + + /// Returns copy of instance casted to NewScalarType. + /// + template + SOPHUS_FUNC SO2 cast() const { + return SO2(unit_complex().template cast()); + } + + /// This provides unsafe read/write access to internal data. SO(2) is + /// represented by a unit complex number (two parameters). When using direct + /// write access, the user needs to take care of that the complex number stays + /// normalized. + /// + SOPHUS_FUNC Scalar* data() { return unit_complex_nonconst().data(); } + + /// Const version of data() above. + /// + SOPHUS_FUNC Scalar const* data() const { return unit_complex().data(); } + + /// Returns group inverse. + /// + SOPHUS_FUNC SO2 inverse() const { + return SO2(unit_complex().x(), -unit_complex().y()); + } + + /// Logarithmic map + /// + /// Computes the logarithm, the inverse of the group exponential which maps + /// element of the group (rotation matrices) to elements of the tangent space + /// (rotation angles). + /// + /// To be specific, this function computes ``vee(logmat(.))`` with + /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator + /// of SO(2). + /// + SOPHUS_FUNC Scalar log() const { + using std::atan2; + return atan2(unit_complex().y(), unit_complex().x()); + } + + /// It re-normalizes ``unit_complex`` to unit length. + /// + /// Note: Because of the class invariant, there is typically no need to call + /// this function directly. + /// + SOPHUS_FUNC void normalize() { + using std::sqrt; + Scalar length = sqrt(unit_complex().x() * unit_complex().x() + + unit_complex().y() * unit_complex().y()); + SOPHUS_ENSURE(length >= Constants::epsilon(), + "Complex number should not be close to zero!"); + unit_complex_nonconst().x() /= length; + unit_complex_nonconst().y() /= length; + } + + /// Returns 2x2 matrix representation of the instance. + /// + /// For SO(2), the matrix representation is an orthogonal matrix ``R`` with + /// ``det(R)=1``, thus the so-called "rotation matrix". + /// + SOPHUS_FUNC Transformation matrix() const { + Scalar const& real = unit_complex().x(); + Scalar const& imag = unit_complex().y(); + Transformation R; + // clang-format off + R << + real, -imag, + imag, real; + // clang-format on + return R; + } + + /// Assignment-like operator from OtherDerived. + /// + template + SOPHUS_FUNC SO2Base& operator=(SO2Base const& other) { + unit_complex_nonconst() = other.unit_complex(); + return *this; + } + + /// Group multiplication, which is rotation concatenation. + /// + template + SOPHUS_FUNC SO2Product operator*( + SO2Base const& other) const { + using ResultT = ReturnScalar; + Scalar const lhs_real = unit_complex().x(); + Scalar const lhs_imag = unit_complex().y(); + typename OtherDerived::Scalar const& rhs_real = other.unit_complex().x(); + typename OtherDerived::Scalar const& rhs_imag = other.unit_complex().y(); + // complex multiplication + ResultT const result_real = lhs_real * rhs_real - lhs_imag * rhs_imag; + ResultT const result_imag = lhs_real * rhs_imag + lhs_imag * rhs_real; + + ResultT const squared_norm = + result_real * result_real + result_imag * result_imag; + // We can assume that the squared-norm is close to 1 since we deal with a + // unit complex number. Due to numerical precision issues, there might + // be a small drift after pose concatenation. Hence, we need to renormalizes + // the complex number here. + // Since squared-norm is close to 1, we do not need to calculate the costly + // square-root, but can use an approximation around 1 (see + // http://stackoverflow.com/a/12934750 for details). + if (squared_norm != ResultT(1.0)) { + ResultT const scale = ResultT(2.0) / (ResultT(1.0) + squared_norm); + return SO2Product(result_real * scale, result_imag * scale); + } + return SO2Product(result_real, result_imag); + } + + /// Group action on 2-points. + /// + /// This function rotates a 2 dimensional point ``p`` by the SO2 element + /// ``bar_R_foo`` (= rotation matrix): ``p_bar = bar_R_foo * p_foo``. + /// + template ::value>::type> + SOPHUS_FUNC PointProduct operator*( + Eigen::MatrixBase const& p) const { + Scalar const& real = unit_complex().x(); + Scalar const& imag = unit_complex().y(); + return PointProduct(real * p[0] - imag * p[1], + imag * p[0] + real * p[1]); + } + + /// Group action on homogeneous 2-points. + /// + /// This function rotates a homogeneous 2 dimensional point ``p`` by the SO2 + /// element ``bar_R_foo`` (= rotation matrix): ``p_bar = bar_R_foo * p_foo``. + /// + template ::value>::type> + SOPHUS_FUNC HomogeneousPointProduct operator*( + Eigen::MatrixBase const& p) const { + Scalar const& real = unit_complex().x(); + Scalar const& imag = unit_complex().y(); + return HomogeneousPointProduct( + real * p[0] - imag * p[1], imag * p[0] + real * p[1], p[2]); + } + + /// Group action on lines. + /// + /// This function rotates a parametrized line ``l(t) = o + t * d`` by the SO2 + /// element: + /// + /// Both direction ``d`` and origin ``o`` are rotated as a 2 dimensional point + /// + SOPHUS_FUNC Line operator*(Line const& l) const { + return Line((*this) * l.origin(), (*this) * l.direction()); + } + + /// In-place group multiplication. This method is only valid if the return + /// type of the multiplication is compatible with this SO2's Scalar type. + /// + template >::value>::type> + SOPHUS_FUNC SO2Base operator*=(SO2Base const& other) { + *static_cast(this) = *this * other; + return *this; + } + + /// Returns derivative of this * SO2::exp(x) wrt. x at x=0. + /// + SOPHUS_FUNC Matrix Dx_this_mul_exp_x_at_0() + const { + return Matrix(-unit_complex()[1], + unit_complex()[0]); + } + + /// Returns internal parameters of SO(2). + /// + /// It returns (c[0], c[1]), with c being the unit complex number. + /// + SOPHUS_FUNC Sophus::Vector params() const { + return unit_complex(); + } + + /// Takes in complex number / tuple and normalizes it. + /// + /// Precondition: The complex number must not be close to zero. + /// + SOPHUS_FUNC void setComplex(Point const& complex) { + unit_complex_nonconst() = complex; + normalize(); + } + + /// Accessor of unit quaternion. + /// + SOPHUS_FUNC + ComplexT const& unit_complex() const { + return static_cast(this)->unit_complex(); + } + + private: + /// Mutator of unit_complex is private to ensure class invariant. That is + /// the complex number must stay close to unit length. + /// + SOPHUS_FUNC + ComplexT& unit_complex_nonconst() { + return static_cast(this)->unit_complex_nonconst(); + } +}; + +/// SO2 using default storage; derived from SO2Base. +template +class SO2 : public SO2Base> { + public: + using Base = SO2Base>; + static int constexpr DoF = Base::DoF; + static int constexpr num_parameters = Base::num_parameters; + + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + using ComplexMember = Vector2; + + /// ``Base`` is friend so unit_complex_nonconst can be accessed from ``Base``. + friend class SO2Base>; + + using Base::operator=; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// Default constructor initializes unit complex number to identity rotation. + /// + SOPHUS_FUNC SO2() : unit_complex_(Scalar(1), Scalar(0)) {} + + /// Copy constructor + /// + SOPHUS_FUNC SO2(SO2 const& other) = default; + + /// Copy-like constructor from OtherDerived. + /// + template + SOPHUS_FUNC SO2(SO2Base const& other) + : unit_complex_(other.unit_complex()) {} + + /// Constructor from rotation matrix + /// + /// Precondition: rotation matrix need to be orthogonal with determinant of 1. + /// + SOPHUS_FUNC explicit SO2(Transformation const& R) + : unit_complex_(Scalar(0.5) * (R(0, 0) + R(1, 1)), + Scalar(0.5) * (R(1, 0) - R(0, 1))) { + SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %", R); + SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: %", + R.determinant()); + } + + /// Constructor from pair of real and imaginary number. + /// + /// Precondition: The pair must not be close to zero. + /// + SOPHUS_FUNC SO2(Scalar const& real, Scalar const& imag) + : unit_complex_(real, imag) { + Base::normalize(); + } + + /// Constructor from 2-vector. + /// + /// Precondition: The vector must not be close to zero. + /// + template + SOPHUS_FUNC explicit SO2(Eigen::MatrixBase const& complex) + : unit_complex_(complex) { + static_assert(std::is_same::value, + "must be same Scalar type"); + Base::normalize(); + } + + /// Constructor from an rotation angle. + /// + SOPHUS_FUNC explicit SO2(Scalar theta) { + unit_complex_nonconst() = SO2::exp(theta).unit_complex(); + } + + /// Accessor of unit complex number + /// + SOPHUS_FUNC ComplexMember const& unit_complex() const { + return unit_complex_; + } + + /// Group exponential + /// + /// This functions takes in an element of tangent space (= rotation angle + /// ``theta``) and returns the corresponding element of the group SO(2). + /// + /// To be more specific, this function computes ``expmat(hat(omega))`` + /// with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the + /// hat()-operator of SO(2). + /// + SOPHUS_FUNC static SO2 exp(Tangent const& theta) { + using std::cos; + using std::sin; + return SO2(cos(theta), sin(theta)); + } + + /// Returns derivative of exp(x) wrt. x. + /// + SOPHUS_FUNC static Sophus::Matrix Dx_exp_x( + Tangent const& theta) { + using std::cos; + using std::sin; + return Sophus::Matrix(-sin(theta), cos(theta)); + } + + /// Returns derivative of exp(x) wrt. x_i at x=0. + /// + SOPHUS_FUNC static Sophus::Matrix + Dx_exp_x_at_0() { + return Sophus::Matrix(Scalar(0), Scalar(1)); + } + + /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. + /// + SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int) { + return generator(); + } + + /// Returns the infinitesimal generators of SO(2). + /// + /// The infinitesimal generators of SO(2) is: + /// + /// | 0 1 | + /// | -1 0 | + /// + SOPHUS_FUNC static Transformation generator() { return hat(Scalar(1)); } + + /// hat-operator + /// + /// It takes in the scalar representation ``theta`` (= rotation angle) and + /// returns the corresponding matrix representation of Lie algebra element. + /// + /// Formally, the hat()-operator of SO(2) is defined as + /// + /// ``hat(.): R^2 -> R^{2x2}, hat(theta) = theta * G`` + /// + /// with ``G`` being the infinitesimal generator of SO(2). + /// + /// The corresponding inverse is the vee()-operator, see below. + /// + SOPHUS_FUNC static Transformation hat(Tangent const& theta) { + Transformation Omega; + // clang-format off + Omega << + Scalar(0), -theta, + theta, Scalar(0); + // clang-format on + return Omega; + } + + /// Returns closed SO2 given arbitrary 2x2 matrix. + /// + template + static SOPHUS_FUNC enable_if_t::value, SO2> + fitToSO2(Transformation const& R) { + return SO2(makeRotationMatrix(R)); + } + + /// Lie bracket + /// + /// It returns the Lie bracket of SO(2). Since SO(2) is a commutative group, + /// the Lie bracket is simple ``0``. + /// + SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) { + return Scalar(0); + } + + /// Draw uniform sample from SO(2) manifold. + /// + template + static SO2 sampleUniform(UniformRandomBitGenerator& generator) { + static_assert(IsUniformRandomBitGenerator::value, + "generator must meet the UniformRandomBitGenerator concept"); + std::uniform_real_distribution uniform(-Constants::pi(), + Constants::pi()); + return SO2(uniform(generator)); + } + + /// vee-operator + /// + /// It takes the 2x2-matrix representation ``Omega`` and maps it to the + /// corresponding scalar representation of Lie algebra. + /// + /// This is the inverse of the hat()-operator, see above. + /// + /// Precondition: ``Omega`` must have the following structure: + /// + /// | 0 -a | + /// | a 0 | + /// + SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { + using std::abs; + return Omega(1, 0); + } + + protected: + /// Mutator of complex number is protected to ensure class invariant. + /// + SOPHUS_FUNC ComplexMember& unit_complex_nonconst() { return unit_complex_; } + + ComplexMember unit_complex_; +}; + +} // namespace Sophus + +namespace Eigen { + +/// Specialization of Eigen::Map for ``SO2``; derived from SO2Base. +/// +/// Allows us to wrap SO2 objects around POD array (e.g. external c style +/// complex number / tuple). +template +class Map, Options> + : public Sophus::SO2Base, Options>> { + public: + using Base = Sophus::SO2Base, Options>>; + using Scalar = Scalar_; + + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + + /// ``Base`` is friend so unit_complex_nonconst can be accessed from ``Base``. + friend class Sophus::SO2Base, Options>>; + + using Base::operator=; + using Base::operator*=; + using Base::operator*; + + SOPHUS_FUNC + Map(Scalar* coeffs) : unit_complex_(coeffs) {} + + /// Accessor of unit complex number. + /// + SOPHUS_FUNC + Map, Options> const& unit_complex() const { + return unit_complex_; + } + + protected: + /// Mutator of unit_complex is protected to ensure class invariant. + /// + SOPHUS_FUNC + Map, Options>& unit_complex_nonconst() { + return unit_complex_; + } + + Map, Options> unit_complex_; +}; + +/// Specialization of Eigen::Map for ``SO2 const``; derived from SO2Base. +/// +/// Allows us to wrap SO2 objects around POD array (e.g. external c style +/// complex number / tuple). +template +class Map const, Options> + : public Sophus::SO2Base const, Options>> { + public: + using Base = Sophus::SO2Base const, Options>>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + + using Base::operator*=; + using Base::operator*; + + SOPHUS_FUNC Map(Scalar const* coeffs) : unit_complex_(coeffs) {} + + /// Accessor of unit complex number. + /// + SOPHUS_FUNC Map const, Options> const& unit_complex() + const { + return unit_complex_; + } + + protected: + /// Mutator of unit_complex is protected to ensure class invariant. + /// + Map const, Options> const unit_complex_; +}; +} // namespace Eigen + +#endif // SOPHUS_SO2_HPP diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/so3.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/so3.hpp new file mode 100644 index 0000000..83563ef --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/so3.hpp @@ -0,0 +1,850 @@ +/// @file +/// Special orthogonal group SO(3) - rotation in 3d. + +#ifndef SOPHUS_SO3_HPP +#define SOPHUS_SO3_HPP + +#include "rotation_matrix.hpp" +#include "so2.hpp" +#include "types.hpp" + +// Include only the selective set of Eigen headers that we need. +// This helps when using Sophus with unusual compilers, like nvcc. +#include +#include +#include + +namespace Sophus { +template +class SO3; +using SO3d = SO3; +using SO3f = SO3; +} // namespace Sophus + +namespace Eigen { +namespace internal { + +template +struct traits> { + static constexpr int Options = Options_; + using Scalar = Scalar_; + using QuaternionType = Eigen::Quaternion; +}; + +template +struct traits, Options_>> + : traits> { + static constexpr int Options = Options_; + using Scalar = Scalar_; + using QuaternionType = Map, Options>; +}; + +template +struct traits const, Options_>> + : traits const> { + static constexpr int Options = Options_; + using Scalar = Scalar_; + using QuaternionType = Map const, Options>; +}; +} // namespace internal +} // namespace Eigen + +namespace Sophus { + +/// SO3 base type - implements SO3 class but is storage agnostic. +/// +/// SO(3) is the group of rotations in 3d. As a matrix group, it is the set of +/// matrices which are orthogonal such that ``R * R' = I`` (with ``R'`` being +/// the transpose of ``R``) and have a positive determinant. In particular, the +/// determinant is 1. Internally, the group is represented as a unit quaternion. +/// Unit quaternion can be seen as members of the special unitary group SU(2). +/// SU(2) is a double cover of SO(3). Hence, for every rotation matrix ``R``, +/// there exist two unit quaternions: ``(r, v)`` and ``(-r, -v)``, with ``r`` +/// the real part and ``v`` being the imaginary 3-vector part of the quaternion. +/// +/// SO(3) is a compact, but non-commutative group. First it is compact since the +/// set of rotation matrices is a closed and bounded set. Second it is +/// non-commutative since the equation ``R_1 * R_2 = R_2 * R_1`` does not hold +/// in general. For example rotating an object by some degrees about its +/// ``x``-axis and then by some degrees about its y axis, does not lead to the +/// same orientation when rotation first about ``y`` and then about ``x``. +/// +/// Class invariant: The 2-norm of ``unit_quaternion`` must be close to 1. +/// Technically speaking, it must hold that: +/// +/// ``|unit_quaternion().squaredNorm() - 1| <= Constants::epsilon()``. +template +class SO3Base { + public: + static constexpr int Options = Eigen::internal::traits::Options; + using Scalar = typename Eigen::internal::traits::Scalar; + using QuaternionType = + typename Eigen::internal::traits::QuaternionType; + using QuaternionTemporaryType = Eigen::Quaternion; + + /// Degrees of freedom of group, number of dimensions in tangent space. + static int constexpr DoF = 3; + /// Number of internal parameters used (quaternion is a 4-tuple). + static int constexpr num_parameters = 4; + /// Group transformations are 3x3 matrices. + static int constexpr N = 3; + using Transformation = Matrix; + using Point = Vector3; + using HomogeneousPoint = Vector4; + using Line = ParametrizedLine3; + using Tangent = Vector; + using Adjoint = Matrix; + + struct TangentAndTheta { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Tangent tangent; + Scalar theta; + }; + + /// For binary operations the return type is determined with the + /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map + /// types, as well as other compatible scalar types such as Ceres::Jet and + /// double scalars with SO3 operations. + template + using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< + Scalar, typename OtherDerived::Scalar>::ReturnType; + + template + using SO3Product = SO3>; + + template + using PointProduct = Vector3>; + + template + using HomogeneousPointProduct = Vector4>; + + /// Adjoint transformation + // + /// This function return the adjoint transformation ``Ad`` of the group + /// element ``A`` such that for all ``x`` it holds that + /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. + // + /// For SO(3), it simply returns the rotation matrix corresponding to ``A``. + /// + SOPHUS_FUNC Adjoint Adj() const { return matrix(); } + + /// Extract rotation angle about canonical X-axis + /// + template + SOPHUS_FUNC enable_if_t::value, S> angleX() const { + Sophus::Matrix3 R = matrix(); + Sophus::Matrix2 Rx = R.template block<2, 2>(1, 1); + return SO2(makeRotationMatrix(Rx)).log(); + } + + /// Extract rotation angle about canonical Y-axis + /// + template + SOPHUS_FUNC enable_if_t::value, S> angleY() const { + Sophus::Matrix3 R = matrix(); + Sophus::Matrix2 Ry; + // clang-format off + Ry << + R(0, 0), R(2, 0), + R(0, 2), R(2, 2); + // clang-format on + return SO2(makeRotationMatrix(Ry)).log(); + } + + /// Extract rotation angle about canonical Z-axis + /// + template + SOPHUS_FUNC enable_if_t::value, S> angleZ() const { + Sophus::Matrix3 R = matrix(); + Sophus::Matrix2 Rz = R.template block<2, 2>(0, 0); + return SO2(makeRotationMatrix(Rz)).log(); + } + + /// Returns copy of instance casted to NewScalarType. + /// + template + SOPHUS_FUNC SO3 cast() const { + return SO3(unit_quaternion().template cast()); + } + + /// This provides unsafe read/write access to internal data. SO(3) is + /// represented by an Eigen::Quaternion (four parameters). When using direct + /// write access, the user needs to take care of that the quaternion stays + /// normalized. + /// + /// Note: The first three Scalars represent the imaginary parts, while the + /// forth Scalar represent the real part. + /// + SOPHUS_FUNC Scalar* data() { + return unit_quaternion_nonconst().coeffs().data(); + } + + /// Const version of data() above. + /// + SOPHUS_FUNC Scalar const* data() const { + return unit_quaternion().coeffs().data(); + } + + /// Returns derivative of this * SO3::exp(x) wrt. x at x=0. + /// + SOPHUS_FUNC Matrix Dx_this_mul_exp_x_at_0() + const { + Matrix J; + Eigen::Quaternion const q = unit_quaternion(); + Scalar const c0 = Scalar(0.5) * q.w(); + Scalar const c1 = Scalar(0.5) * q.z(); + Scalar const c2 = -c1; + Scalar const c3 = Scalar(0.5) * q.y(); + Scalar const c4 = Scalar(0.5) * q.x(); + Scalar const c5 = -c4; + Scalar const c6 = -c3; + J(0, 0) = c0; + J(0, 1) = c2; + J(0, 2) = c3; + J(1, 0) = c1; + J(1, 1) = c0; + J(1, 2) = c5; + J(2, 0) = c6; + J(2, 1) = c4; + J(2, 2) = c0; + J(3, 0) = c5; + J(3, 1) = c6; + J(3, 2) = c2; + + return J; + } + + /// Returns internal parameters of SO(3). + /// + /// It returns (q.imag[0], q.imag[1], q.imag[2], q.real), with q being the + /// unit quaternion. + /// + SOPHUS_FUNC Sophus::Vector params() const { + return unit_quaternion().coeffs(); + } + + /// Returns group inverse. + /// + SOPHUS_FUNC SO3 inverse() const { + return SO3(unit_quaternion().conjugate()); + } + + /// Logarithmic map + /// + /// Computes the logarithm, the inverse of the group exponential which maps + /// element of the group (rotation matrices) to elements of the tangent space + /// (rotation-vector). + /// + /// To be specific, this function computes ``vee(logmat(.))`` with + /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator + /// of SO(3). + /// + SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; } + + /// As above, but also returns ``theta = |omega|``. + /// + SOPHUS_FUNC TangentAndTheta logAndTheta() const { + TangentAndTheta J; + using std::abs; + using std::atan; + using std::sqrt; + Scalar squared_n = unit_quaternion().vec().squaredNorm(); + Scalar w = unit_quaternion().w(); + + Scalar two_atan_nbyw_by_n; + + /// Atan-based log thanks to + /// + /// C. Hertzberg et al.: + /// "Integrating Generic Sensor Fusion Algorithms with Sound State + /// Representation through Encapsulation of Manifolds" + /// Information Fusion, 2011 + + if (squared_n < Constants::epsilon() * Constants::epsilon()) { + // If quaternion is normalized and n=0, then w should be 1; + // w=0 should never happen here! + SOPHUS_ENSURE(abs(w) >= Constants::epsilon(), + "Quaternion (%) should be normalized!", + unit_quaternion().coeffs().transpose()); + Scalar squared_w = w * w; + two_atan_nbyw_by_n = + Scalar(2) / w - Scalar(2.0/3.0) * (squared_n) / (w * squared_w); + J.theta = Scalar(2) * squared_n / w; + } else { + Scalar n = sqrt(squared_n); + if (abs(w) < Constants::epsilon()) { + if (w > Scalar(0)) { + two_atan_nbyw_by_n = Constants::pi() / n; + } else { + two_atan_nbyw_by_n = -Constants::pi() / n; + } + } else { + two_atan_nbyw_by_n = Scalar(2) * atan(n / w) / n; + } + J.theta = two_atan_nbyw_by_n * n; + } + + J.tangent = two_atan_nbyw_by_n * unit_quaternion().vec(); + return J; + } + + /// It re-normalizes ``unit_quaternion`` to unit length. + /// + /// Note: Because of the class invariant, there is typically no need to call + /// this function directly. + /// + SOPHUS_FUNC void normalize() { + Scalar length = unit_quaternion_nonconst().norm(); + SOPHUS_ENSURE(length >= Constants::epsilon(), + "Quaternion (%) should not be close to zero!", + unit_quaternion_nonconst().coeffs().transpose()); + unit_quaternion_nonconst().coeffs() /= length; + } + + /// Returns 3x3 matrix representation of the instance. + /// + /// For SO(3), the matrix representation is an orthogonal matrix ``R`` with + /// ``det(R)=1``, thus the so-called "rotation matrix". + /// + SOPHUS_FUNC Transformation matrix() const { + return unit_quaternion().toRotationMatrix(); + } + + /// Assignment-like operator from OtherDerived. + /// + template + SOPHUS_FUNC SO3Base& operator=(SO3Base const& other) { + unit_quaternion_nonconst() = other.unit_quaternion(); + return *this; + } + + /// Group multiplication, which is rotation concatenation. + /// + template + SOPHUS_FUNC SO3Product operator*( + SO3Base const& other) const { + using QuaternionProductType = + typename SO3Product::QuaternionType; + const QuaternionType& a = unit_quaternion(); + const typename OtherDerived::QuaternionType& b = other.unit_quaternion(); + /// NOTE: We cannot use Eigen's Quaternion multiplication because it always + /// returns a Quaternion of the same Scalar as this object, so it is not + /// able to multiple Jets and doubles correctly. + return SO3Product(QuaternionProductType( + a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(), + a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(), + a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(), + a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x())); + } + + /// Group action on 3-points. + /// + /// This function rotates a 3 dimensional point ``p`` by the SO3 element + /// ``bar_R_foo`` (= rotation matrix): ``p_bar = bar_R_foo * p_foo``. + /// + /// Since SO3 is internally represented by a unit quaternion ``q``, it is + /// implemented as ``p_bar = q * p_foo * q^{*}`` + /// with ``q^{*}`` being the quaternion conjugate of ``q``. + /// + /// Geometrically, ``p`` is rotated by angle ``|omega|`` around the + /// axis ``omega/|omega|`` with ``omega := vee(log(bar_R_foo))``. + /// + /// For ``vee``-operator, see below. + /// + template ::value>::type> + SOPHUS_FUNC PointProduct operator*( + Eigen::MatrixBase const& p) const { + /// NOTE: We cannot use Eigen's Quaternion transformVector because it always + /// returns a Vector3 of the same Scalar as this quaternion, so it is not + /// able to be applied to Jets and doubles correctly. + const QuaternionType& q = unit_quaternion(); + PointProduct uv = q.vec().cross(p); + uv += uv; + return p + q.w() * uv + q.vec().cross(uv); + } + + /// Group action on homogeneous 3-points. See above for more details. + template ::value>::type> + SOPHUS_FUNC HomogeneousPointProduct operator*( + Eigen::MatrixBase const& p) const { + const auto rp = *this * p.template head<3>(); + return HomogeneousPointProduct(rp(0), rp(1), rp(2), p(3)); + } + + /// Group action on lines. + /// + /// This function rotates a parametrized line ``l(t) = o + t * d`` by the SO3 + /// element: + /// + /// Both direction ``d`` and origin ``o`` are rotated as a 3 dimensional point + /// + SOPHUS_FUNC Line operator*(Line const& l) const { + return Line((*this) * l.origin(), (*this) * l.direction()); + } + + /// In-place group multiplication. This method is only valid if the return + /// type of the multiplication is compatible with this SO3's Scalar type. + /// + template >::value>::type> + SOPHUS_FUNC SO3Base& operator*=(SO3Base const& other) { + *static_cast(this) = *this * other; + return *this; + } + + /// Takes in quaternion, and normalizes it. + /// + /// Precondition: The quaternion must not be close to zero. + /// + SOPHUS_FUNC void setQuaternion(Eigen::Quaternion const& quaternion) { + unit_quaternion_nonconst() = quaternion; + normalize(); + } + + /// Accessor of unit quaternion. + /// + SOPHUS_FUNC QuaternionType const& unit_quaternion() const { + return static_cast(this)->unit_quaternion(); + } + + private: + /// Mutator of unit_quaternion is private to ensure class invariant. That is + /// the quaternion must stay close to unit length. + /// + SOPHUS_FUNC QuaternionType& unit_quaternion_nonconst() { + return static_cast(this)->unit_quaternion_nonconst(); + } +}; + +/// SO3 using default storage; derived from SO3Base. +template +class SO3 : public SO3Base> { + public: + using Base = SO3Base>; + static int constexpr DoF = Base::DoF; + static int constexpr num_parameters = Base::num_parameters; + + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + using QuaternionMember = Eigen::Quaternion; + + /// ``Base`` is friend so unit_quaternion_nonconst can be accessed from + /// ``Base``. + friend class SO3Base>; + + using Base::operator=; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// Default constructor initializes unit quaternion to identity rotation. + /// + SOPHUS_FUNC SO3() + : unit_quaternion_(Scalar(1), Scalar(0), Scalar(0), Scalar(0)) {} + + /// Copy constructor + /// + SOPHUS_FUNC SO3(SO3 const& other) = default; + + /// Copy-like constructor from OtherDerived. + /// + template + SOPHUS_FUNC SO3(SO3Base const& other) + : unit_quaternion_(other.unit_quaternion()) {} + + /// Constructor from rotation matrix + /// + /// Precondition: rotation matrix needs to be orthogonal with determinant + /// of 1. + /// + SOPHUS_FUNC SO3(Transformation const& R) : unit_quaternion_(R) { + SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %", + R * R.transpose()); + SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: %", + R.determinant()); + } + + /// Constructor from quaternion + /// + /// Precondition: quaternion must not be close to zero. + /// + template + SOPHUS_FUNC explicit SO3(Eigen::QuaternionBase const& quat) + : unit_quaternion_(quat) { + static_assert( + std::is_same::Scalar, Scalar>::value, + "Input must be of same scalar type"); + Base::normalize(); + } + + /// Accessor of unit quaternion. + /// + SOPHUS_FUNC QuaternionMember const& unit_quaternion() const { + return unit_quaternion_; + } + + /// Returns derivative of exp(x) wrt. x. + /// + SOPHUS_FUNC static Sophus::Matrix Dx_exp_x( + Tangent const& omega) { + using std::cos; + using std::exp; + using std::sin; + using std::sqrt; + Scalar const c0 = omega[0] * omega[0]; + Scalar const c1 = omega[1] * omega[1]; + Scalar const c2 = omega[2] * omega[2]; + Scalar const c3 = c0 + c1 + c2; + + if (c3 < Constants::epsilon()) { + return Dx_exp_x_at_0(); + } + + Scalar const c4 = sqrt(c3); + Scalar const c5 = 1.0 / c4; + Scalar const c6 = 0.5 * c4; + Scalar const c7 = sin(c6); + Scalar const c8 = c5 * c7; + Scalar const c9 = pow(c3, -3.0L / 2.0L); + Scalar const c10 = c7 * c9; + Scalar const c11 = Scalar(1.0) / c3; + Scalar const c12 = cos(c6); + Scalar const c13 = Scalar(0.5) * c11 * c12; + Scalar const c14 = c7 * c9 * omega[0]; + Scalar const c15 = Scalar(0.5) * c11 * c12 * omega[0]; + Scalar const c16 = -c14 * omega[1] + c15 * omega[1]; + Scalar const c17 = -c14 * omega[2] + c15 * omega[2]; + Scalar const c18 = omega[1] * omega[2]; + Scalar const c19 = -c10 * c18 + c13 * c18; + Scalar const c20 = Scalar(0.5) * c5 * c7; + Sophus::Matrix J; + J(0, 0) = -c0 * c10 + c0 * c13 + c8; + J(0, 1) = c16; + J(0, 2) = c17; + J(1, 0) = c16; + J(1, 1) = -c1 * c10 + c1 * c13 + c8; + J(1, 2) = c19; + J(2, 0) = c17; + J(2, 1) = c19; + J(2, 2) = -c10 * c2 + c13 * c2 + c8; + J(3, 0) = -c20 * omega[0]; + J(3, 1) = -c20 * omega[1]; + J(3, 2) = -c20 * omega[2]; + return J; + } + + /// Returns derivative of exp(x) wrt. x_i at x=0. + /// + SOPHUS_FUNC static Sophus::Matrix + Dx_exp_x_at_0() { + Sophus::Matrix J; + // clang-format off + J << Scalar(0.5), Scalar(0), Scalar(0), + Scalar(0), Scalar(0.5), Scalar(0), + Scalar(0), Scalar(0), Scalar(0.5), + Scalar(0), Scalar(0), Scalar(0); + // clang-format on + return J; + } + + /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. + /// + SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { + return generator(i); + } + + /// Group exponential + /// + /// This functions takes in an element of tangent space (= rotation vector + /// ``omega``) and returns the corresponding element of the group SO(3). + /// + /// To be more specific, this function computes ``expmat(hat(omega))`` + /// with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the + /// hat()-operator of SO(3). + /// + SOPHUS_FUNC static SO3 exp(Tangent const& omega) { + Scalar theta; + return expAndTheta(omega, &theta); + } + + /// As above, but also returns ``theta = |omega|`` as out-parameter. + /// + /// Precondition: ``theta`` must not be ``nullptr``. + /// + SOPHUS_FUNC static SO3 expAndTheta(Tangent const& omega, + Scalar* theta) { + SOPHUS_ENSURE(theta != nullptr, "must not be nullptr."); + using std::abs; + using std::cos; + using std::sin; + using std::sqrt; + Scalar theta_sq = omega.squaredNorm(); + + Scalar imag_factor; + Scalar real_factor; + if (theta_sq < + Constants::epsilon() * Constants::epsilon()) { + *theta = Scalar(0); + Scalar theta_po4 = theta_sq * theta_sq; + imag_factor = Scalar(0.5) - Scalar(1.0 / 48.0) * theta_sq + + Scalar(1.0 / 3840.0) * theta_po4; + real_factor = Scalar(1) - Scalar(1.0 / 8.0) * theta_sq + + Scalar(1.0 / 384.0) * theta_po4; + } else { + *theta = sqrt(theta_sq); + Scalar half_theta = Scalar(0.5) * (*theta); + Scalar sin_half_theta = sin(half_theta); + imag_factor = sin_half_theta / (*theta); + real_factor = cos(half_theta); + } + + SO3 q; + q.unit_quaternion_nonconst() = + QuaternionMember(real_factor, imag_factor * omega.x(), + imag_factor * omega.y(), imag_factor * omega.z()); + SOPHUS_ENSURE(abs(q.unit_quaternion().squaredNorm() - Scalar(1)) < + Sophus::Constants::epsilon(), + "SO3::exp failed! omega: %, real: %, img: %", + omega.transpose(), real_factor, imag_factor); + return q; + } + + /// Returns closest SO3 given arbitrary 3x3 matrix. + /// + template + static SOPHUS_FUNC enable_if_t::value, SO3> + fitToSO3(Transformation const& R) { + return SO3(::Sophus::makeRotationMatrix(R)); + } + + /// Returns the ith infinitesimal generators of SO(3). + /// + /// The infinitesimal generators of SO(3) are: + /// + /// ``` + /// | 0 0 0 | + /// G_0 = | 0 0 -1 | + /// | 0 1 0 | + /// + /// | 0 0 1 | + /// G_1 = | 0 0 0 | + /// | -1 0 0 | + /// + /// | 0 -1 0 | + /// G_2 = | 1 0 0 | + /// | 0 0 0 | + /// ``` + /// + /// Precondition: ``i`` must be 0, 1 or 2. + /// + SOPHUS_FUNC static Transformation generator(int i) { + SOPHUS_ENSURE(i >= 0 && i <= 2, "i should be in range [0,2]."); + Tangent e; + e.setZero(); + e[i] = Scalar(1); + return hat(e); + } + + /// hat-operator + /// + /// It takes in the 3-vector representation ``omega`` (= rotation vector) and + /// returns the corresponding matrix representation of Lie algebra element. + /// + /// Formally, the hat()-operator of SO(3) is defined as + /// + /// ``hat(.): R^3 -> R^{3x3}, hat(omega) = sum_i omega_i * G_i`` + /// (for i=0,1,2) + /// + /// with ``G_i`` being the ith infinitesimal generator of SO(3). + /// + /// The corresponding inverse is the vee()-operator, see below. + /// + SOPHUS_FUNC static Transformation hat(Tangent const& omega) { + Transformation Omega; + // clang-format off + Omega << + Scalar(0), -omega(2), omega(1), + omega(2), Scalar(0), -omega(0), + -omega(1), omega(0), Scalar(0); + // clang-format on + return Omega; + } + + /// Lie bracket + /// + /// It computes the Lie bracket of SO(3). To be more specific, it computes + /// + /// ``[omega_1, omega_2]_so3 := vee([hat(omega_1), hat(omega_2)])`` + /// + /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the + /// hat()-operator and ``vee(.)`` the vee()-operator of SO3. + /// + /// For the Lie algebra so3, the Lie bracket is simply the cross product: + /// + /// ``[omega_1, omega_2]_so3 = omega_1 x omega_2.`` + /// + SOPHUS_FUNC static Tangent lieBracket(Tangent const& omega1, + Tangent const& omega2) { + return omega1.cross(omega2); + } + + /// Construct x-axis rotation. + /// + static SOPHUS_FUNC SO3 rotX(Scalar const& x) { + return SO3::exp(Sophus::Vector3(x, Scalar(0), Scalar(0))); + } + + /// Construct y-axis rotation. + /// + static SOPHUS_FUNC SO3 rotY(Scalar const& y) { + return SO3::exp(Sophus::Vector3(Scalar(0), y, Scalar(0))); + } + + /// Construct z-axis rotation. + /// + static SOPHUS_FUNC SO3 rotZ(Scalar const& z) { + return SO3::exp(Sophus::Vector3(Scalar(0), Scalar(0), z)); + } + + /// Draw uniform sample from SO(3) manifold. + /// Based on: http://planning.cs.uiuc.edu/node198.html + /// + template + static SO3 sampleUniform(UniformRandomBitGenerator& generator) { + static_assert(IsUniformRandomBitGenerator::value, + "generator must meet the UniformRandomBitGenerator concept"); + + std::uniform_real_distribution uniform(Scalar(0), Scalar(1)); + std::uniform_real_distribution uniform_twopi( + Scalar(0), 2 * Constants::pi()); + + const Scalar u1 = uniform(generator); + const Scalar u2 = uniform_twopi(generator); + const Scalar u3 = uniform_twopi(generator); + + const Scalar a = sqrt(1 - u1); + const Scalar b = sqrt(u1); + + return SO3( + QuaternionMember(a * sin(u2), a * cos(u2), b * sin(u3), b * cos(u3))); + } + + /// vee-operator + /// + /// It takes the 3x3-matrix representation ``Omega`` and maps it to the + /// corresponding vector representation of Lie algebra. + /// + /// This is the inverse of the hat()-operator, see above. + /// + /// Precondition: ``Omega`` must have the following structure: + /// + /// | 0 -c b | + /// | c 0 -a | + /// | -b a 0 | + /// + SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { + return Tangent(Omega(2, 1), Omega(0, 2), Omega(1, 0)); + } + + protected: + /// Mutator of unit_quaternion is protected to ensure class invariant. + /// + SOPHUS_FUNC QuaternionMember& unit_quaternion_nonconst() { + return unit_quaternion_; + } + + QuaternionMember unit_quaternion_; +}; + +} // namespace Sophus + +namespace Eigen { +/// Specialization of Eigen::Map for ``SO3``; derived from SO3Base. +/// +/// Allows us to wrap SO3 objects around POD array (e.g. external c style +/// quaternion). +template +class Map, Options> + : public Sophus::SO3Base, Options>> { + public: + using Base = Sophus::SO3Base, Options>>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + + /// ``Base`` is friend so unit_quaternion_nonconst can be accessed from + /// ``Base``. + friend class Sophus::SO3Base, Options>>; + + using Base::operator=; + using Base::operator*=; + using Base::operator*; + + SOPHUS_FUNC Map(Scalar* coeffs) : unit_quaternion_(coeffs) {} + + /// Accessor of unit quaternion. + /// + SOPHUS_FUNC Map, Options> const& unit_quaternion() + const { + return unit_quaternion_; + } + + protected: + /// Mutator of unit_quaternion is protected to ensure class invariant. + /// + SOPHUS_FUNC Map, Options>& + unit_quaternion_nonconst() { + return unit_quaternion_; + } + + Map, Options> unit_quaternion_; +}; + +/// Specialization of Eigen::Map for ``SO3 const``; derived from SO3Base. +/// +/// Allows us to wrap SO3 objects around POD array (e.g. external c style +/// quaternion). +template +class Map const, Options> + : public Sophus::SO3Base const, Options>> { + public: + using Base = Sophus::SO3Base const, Options>>; + using Scalar = Scalar_; + using Transformation = typename Base::Transformation; + using Point = typename Base::Point; + using HomogeneousPoint = typename Base::HomogeneousPoint; + using Tangent = typename Base::Tangent; + using Adjoint = typename Base::Adjoint; + + using Base::operator*=; + using Base::operator*; + + SOPHUS_FUNC Map(Scalar const* coeffs) : unit_quaternion_(coeffs) {} + + /// Accessor of unit quaternion. + /// + SOPHUS_FUNC Map const, Options> const& + unit_quaternion() const { + return unit_quaternion_; + } + + protected: + /// Mutator of unit_quaternion is protected to ensure class invariant. + /// + Map const, Options> const unit_quaternion_; +}; +} // namespace Eigen + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/test_macros.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/test_macros.hpp new file mode 100644 index 0000000..fd13b0b --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/test_macros.hpp @@ -0,0 +1,129 @@ +#ifndef SOPUHS_TESTS_MACROS_HPP +#define SOPUHS_TESTS_MACROS_HPP + +#include +#include + +namespace Sophus { +namespace details { + +template +class Pretty { + public: + static std::string impl(Scalar s) { return FormatString("%", s); } +}; + +template +class Pretty> { + public: + static std::string impl(Matrix const& v) { + return FormatString("\n%\n", v); + } +}; + +template +std::string pretty(T const& v) { + return Pretty::impl(v); +} + +template +void testFailed(bool& passed, char const* func, char const* file, int line, + std::string const& msg) { + std::cerr << FormatString("Test failed in function %, file %, line %\n", func, + file, line); + std::cerr << msg << "\n\n"; + passed = false; +} +} // namespace details + +void processTestResult(bool passed) { + if (!passed) { + // LCOV_EXCL_START + std::cerr << "failed!" << std::endl << std::endl; + exit(-1); + // LCOV_EXCL_STOP + } + std::cerr << "passed." << std::endl << std::endl; +} +} // namespace Sophus + +#define SOPHUS_STRINGIFY(x) #x + +/// GenericTests whether condition is true. +/// The in-out parameter passed will be set to false if test fails. +#define SOPHUS_TEST(passed, condition, ...) \ + do { \ + if (!(condition)) { \ + std::string msg = Sophus::details::FormatString( \ + "condition ``%`` is false\n", SOPHUS_STRINGIFY(condition)); \ + msg += Sophus::details::FormatString(__VA_ARGS__); \ + Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ + msg); \ + } \ + } while (false) + +/// GenericTests whether left is equal to right given a threshold. +/// The in-out parameter passed will be set to false if test fails. +#define SOPHUS_TEST_EQUAL(passed, left, right, ...) \ + do { \ + if (left != right) { \ + std::string msg = Sophus::details::FormatString( \ + "% (=%) is not equal to % (=%)\n", SOPHUS_STRINGIFY(left), \ + Sophus::details::pretty(left), SOPHUS_STRINGIFY(right), \ + Sophus::details::pretty(right)); \ + msg += Sophus::details::FormatString(__VA_ARGS__); \ + Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ + msg); \ + } \ + } while (false) + +/// GenericTests whether left is equal to right given a threshold. +/// The in-out parameter passed will be set to false if test fails. +#define SOPHUS_TEST_NEQ(passed, left, right, ...) \ + do { \ + if (left == right) { \ + std::string msg = Sophus::details::FormatString( \ + "% (=%) shoudl not be equal to % (=%)\n", SOPHUS_STRINGIFY(left), \ + Sophus::details::pretty(left), SOPHUS_STRINGIFY(right), \ + Sophus::details::pretty(right)); \ + msg += Sophus::details::FormatString(__VA_ARGS__); \ + Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ + msg); \ + } \ + } while (false) + +/// GenericTests whether left is approximatly equal to right given a threshold. +/// The in-out parameter passed will be set to false if test fails. +#define SOPHUS_TEST_APPROX(passed, left, right, thr, ...) \ + do { \ + auto nrm = Sophus::maxMetric((left), (right)); \ + if (!(nrm < (thr))) { \ + std::string msg = Sophus::details::FormatString( \ + "% (=%) is not approx % (=%); % is %; nrm is %\n", \ + SOPHUS_STRINGIFY(left), Sophus::details::pretty(left), \ + SOPHUS_STRINGIFY(right), Sophus::details::pretty(right), \ + SOPHUS_STRINGIFY(thr), Sophus::details::pretty(thr), nrm); \ + msg += Sophus::details::FormatString(__VA_ARGS__); \ + Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ + msg); \ + } \ + } while (false) + +/// GenericTests whether left is NOT approximatly equal to right given a +/// threshold. The in-out parameter passed will be set to false if test fails. +#define SOPHUS_TEST_NOT_APPROX(passed, left, right, thr, ...) \ + do { \ + auto nrm = Sophus::maxMetric((left), (right)); \ + if (nrm < (thr)) { \ + std::string msg = Sophus::details::FormatString( \ + "% (=%) is approx % (=%), but it should not!\n % is %; nrm is %\n", \ + SOPHUS_STRINGIFY(left), Sophus::details::pretty(left), \ + SOPHUS_STRINGIFY(right), Sophus::details::pretty(right), \ + SOPHUS_STRINGIFY(thr), Sophus::details::pretty(thr), nrm); \ + msg += Sophus::details::FormatString(__VA_ARGS__); \ + Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ + msg); \ + } \ + } while (false) + +#endif // SOPUHS_TESTS_MACROS_HPP diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/types.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/types.hpp new file mode 100644 index 0000000..6ce0324 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/types.hpp @@ -0,0 +1,241 @@ +/// @file +/// Common type aliases. + +#ifndef SOPHUS_TYPES_HPP +#define SOPHUS_TYPES_HPP + +#include +#include "common.hpp" + +namespace Sophus { + +template +using Vector = Eigen::Matrix; + +template +using Vector2 = Vector; +using Vector2f = Vector2; +using Vector2d = Vector2; + +template +using Vector3 = Vector; +using Vector3f = Vector3; +using Vector3d = Vector3; + +template +using Vector4 = Vector; +using Vector4f = Vector4; +using Vector4d = Vector4; + +template +using Vector6 = Vector; +using Vector6f = Vector6; +using Vector6d = Vector6; + +template +using Vector7 = Vector; +using Vector7f = Vector7; +using Vector7d = Vector7; + +template +using Matrix = Eigen::Matrix; + +template +using Matrix2 = Matrix; +using Matrix2f = Matrix2; +using Matrix2d = Matrix2; + +template +using Matrix3 = Matrix; +using Matrix3f = Matrix3; +using Matrix3d = Matrix3; + +template +using Matrix4 = Matrix; +using Matrix4f = Matrix4; +using Matrix4d = Matrix4; + +template +using Matrix6 = Matrix; +using Matrix6f = Matrix6; +using Matrix6d = Matrix6; + +template +using Matrix7 = Matrix; +using Matrix7f = Matrix7; +using Matrix7d = Matrix7; + +template +using ParametrizedLine = Eigen::ParametrizedLine; + +template +using ParametrizedLine3 = ParametrizedLine; +using ParametrizedLine3f = ParametrizedLine3; +using ParametrizedLine3d = ParametrizedLine3; + +template +using ParametrizedLine2 = ParametrizedLine; +using ParametrizedLine2f = ParametrizedLine2; +using ParametrizedLine2d = ParametrizedLine2; + +namespace details { +template +class MaxMetric { + public: + static Scalar impl(Scalar s0, Scalar s1) { + using std::abs; + return abs(s0 - s1); + } +}; + +template +class MaxMetric> { + public: + static Scalar impl(Matrix const& p0, + Matrix const& p1) { + return (p0 - p1).template lpNorm(); + } +}; + +template +class SetToZero { + public: + static void impl(Scalar& s) { s = Scalar(0); } +}; + +template +class SetToZero> { + public: + static void impl(Matrix& v) { v.setZero(); } +}; + +template +class SetElementAt; + +template +class SetElementAt { + public: + static void impl(Scalar& s, Scalar value, int at) { + SOPHUS_ENSURE(at == 0, "is %", at); + s = value; + } +}; + +template +class SetElementAt, Scalar> { + public: + static void impl(Vector& v, Scalar value, int at) { + SOPHUS_ENSURE(at >= 0 && at < N, "is %", at); + v[at] = value; + } +}; + +template +class SquaredNorm { + public: + static Scalar impl(Scalar const& s) { return s * s; } +}; + +template +class SquaredNorm> { + public: + static Scalar impl(Matrix const& s) { return s.squaredNorm(); } +}; + +template +class Transpose { + public: + static Scalar impl(Scalar const& s) { return s; } +}; + +template +class Transpose> { + public: + static Matrix impl(Matrix const& s) { + return s.transpose(); + } +}; +} // namespace details + +/// Returns maximum metric between two points ``p0`` and ``p1``, with ``p0, p1`` +/// being matrices or a scalars. +/// +template +auto maxMetric(T const& p0, T const& p1) + -> decltype(details::MaxMetric::impl(p0, p1)) { + return details::MaxMetric::impl(p0, p1); +} + +/// Sets point ``p`` to zero, with ``p`` being a matrix or a scalar. +/// +template +void setToZero(T& p) { + return details::SetToZero::impl(p); +} + +/// Sets ``i``th component of ``p`` to ``value``, with ``p`` being a +/// matrix or a scalar. If ``p`` is a scalar, ``i`` must be ``0``. +/// +template +void setElementAt(T& p, Scalar value, int i) { + return details::SetElementAt::impl(p, value, i); +} + +/// Returns the squared 2-norm of ``p``, with ``p`` being a vector or a scalar. +/// +template +auto squaredNorm(T const& p) -> decltype(details::SquaredNorm::impl(p)) { + return details::SquaredNorm::impl(p); +} + +/// Returns ``p.transpose()`` if ``p`` is a matrix, and simply ``p`` if m is a +/// scalar. +/// +template +auto transpose(T const& p) -> decltype(details::Transpose::impl(T())) { + return details::Transpose::impl(p); +} + +template +struct IsFloatingPoint { + static bool const value = std::is_floating_point::value; +}; + +template +struct IsFloatingPoint> { + static bool const value = std::is_floating_point::value; +}; + +template +struct GetScalar { + using Scalar = Scalar_; +}; + +template +struct GetScalar> { + using Scalar = Scalar_; +}; + +/// If the Vector type is of fixed size, then IsFixedSizeVector::value will be +/// true. +template ::type> +struct IsFixedSizeVector : std::true_type {}; + +/// Planes in 3d are hyperplanes. +template +using Plane3 = Eigen::Hyperplane; +using Plane3d = Plane3; +using Plane3f = Plane3; + +/// Lines in 2d are hyperplanes. +template +using Line2 = Eigen::Hyperplane; +using Line2d = Line2; +using Line2f = Line2; + +} // namespace Sophus + +#endif // SOPHUS_TYPES_HPP diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/velocities.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/velocities.hpp new file mode 100644 index 0000000..ee900f6 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/sophus/velocities.hpp @@ -0,0 +1,74 @@ +#ifndef SOPHUS_VELOCITIES_HPP +#define SOPHUS_VELOCITIES_HPP + +#include + +#include "num_diff.hpp" +#include "se3.hpp" + +namespace Sophus { +namespace experimental { +// Experimental since the API will certainly change drastically in the future. + +// Transforms velocity vector by rotation ``foo_R_bar``. +// +// Note: vel_bar can be either a linear or a rotational velocity vector. +// +template +Vector3 transformVelocity(SO3 const& foo_R_bar, + Vector3 const& vel_bar) { + // For rotational velocities note that: + // + // vel_bar = vee(foo_R_bar * hat(vel_bar) * foo_R_bar^T) + // = vee(hat(Adj(foo_R_bar) * vel_bar)) + // = Adj(foo_R_bar) * vel_bar + // = foo_R_bar * vel_bar. + // + return foo_R_bar * vel_bar; +} + +// Transforms velocity vector by pose ``foo_T_bar``. +// +// Note: vel_bar can be either a linear or a rotational velocity vector. +// +template +Vector3 transformVelocity(SE3 const& foo_T_bar, + Vector3 const& vel_bar) { + return transformVelocity(foo_T_bar.so3(), vel_bar); +} + +// finite difference approximation of instantanious velocity in frame foo +// +template +Vector3 finiteDifferenceRotationalVelocity( + std::function(Scalar)> const& foo_R_bar, Scalar t, + Scalar h = Constants::epsilon()) { + // https://en.wikipedia.org/w/index.php?title=Angular_velocity&oldid=791867792#Angular_velocity_tensor + // + // W = dR(t)/dt * R^{-1}(t) + Matrix3 dR_dt_in_frame_foo = curveNumDiff( + [&foo_R_bar](Scalar t0) -> Matrix3 { + return foo_R_bar(t0).matrix(); + }, + t, h); + // velocity tensor + Matrix3 W_in_frame_foo = + dR_dt_in_frame_foo * (foo_R_bar(t)).inverse().matrix(); + return SO3::vee(W_in_frame_foo); +} + +// finite difference approximation of instantanious velocity in frame foo +// +template +Vector3 finiteDifferenceRotationalVelocity( + std::function(Scalar)> const& foo_T_bar, Scalar t, + Scalar h = Constants::epsilon()) { + return finiteDifferenceRotationalVelocity( + [&foo_T_bar](Scalar t) -> SO3 { return foo_T_bar(t).so3(); }, t, + h); +} + +} // namespace experimental +} // namespace Sophus + +#endif // SOPHUS_VELOCITIES_HPP diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/CMakeLists.txt b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/CMakeLists.txt new file mode 100644 index 0000000..6c869ff --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/CMakeLists.txt @@ -0,0 +1,2 @@ +ADD_SUBDIRECTORY(core) +ADD_SUBDIRECTORY(ceres) diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/ceres/CMakeLists.txt b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/ceres/CMakeLists.txt new file mode 100644 index 0000000..eabf9c3 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/ceres/CMakeLists.txt @@ -0,0 +1,20 @@ +# Make sure Ceres knows where to find Eigen +list(APPEND SEARCH_HEADERS ${EIGEN3_INCLUDE_DIR}) + +# git clone https://ceres-solver.googlesource.com/ceres-solver +find_package( Ceres 1.6.0 QUIET ) + +if( Ceres_FOUND ) + MESSAGE(STATUS "CERES found") + + # Tests to run + SET( TEST_SOURCES test_ceres_se3 ) + + FOREACH(test_src ${TEST_SOURCES}) + ADD_EXECUTABLE( ${test_src} ${test_src}.cpp local_parameterization_se3) + TARGET_LINK_LIBRARIES( ${test_src} sophus ${CERES_LIBRARIES} ) + TARGET_INCLUDE_DIRECTORIES( ${test_src} SYSTEM PRIVATE ${CERES_INCLUDE_DIRS}) + ADD_TEST( ${test_src} ${test_src} ) + ENDFOREACH(test_src) + +endif( Ceres_FOUND ) diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/ceres/local_parameterization_se3.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/ceres/local_parameterization_se3.hpp new file mode 100644 index 0000000..de9d83f --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/ceres/local_parameterization_se3.hpp @@ -0,0 +1,47 @@ +#ifndef SOPHUS_TEST_LOCAL_PARAMETERIZATION_SE3_HPP +#define SOPHUS_TEST_LOCAL_PARAMETERIZATION_SE3_HPP + +#include +#include + +namespace Sophus { +namespace test { + +class LocalParameterizationSE3 : public ceres::LocalParameterization { + public: + virtual ~LocalParameterizationSE3() {} + + // SE3 plus operation for Ceres + // + // T * exp(x) + // + virtual bool Plus(double const* T_raw, double const* delta_raw, + double* T_plus_delta_raw) const { + Eigen::Map const T(T_raw); + Eigen::Map const delta(delta_raw); + Eigen::Map T_plus_delta(T_plus_delta_raw); + T_plus_delta = T * SE3d::exp(delta); + return true; + } + + // Jacobian of SE3 plus operation for Ceres + // + // Dx T * exp(x) with x=0 + // + virtual bool ComputeJacobian(double const* T_raw, + double* jacobian_raw) const { + Eigen::Map T(T_raw); + Eigen::Map> jacobian( + jacobian_raw); + jacobian = T.Dx_this_mul_exp_x_at_0(); + return true; + } + + virtual int GlobalSize() const { return SE3d::num_parameters; } + + virtual int LocalSize() const { return SE3d::DoF; } +}; +} // namespace test +} // namespace Sophus + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/ceres/test_ceres_se3.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/ceres/test_ceres_se3.cpp new file mode 100644 index 0000000..00650b2 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/ceres/test_ceres_se3.cpp @@ -0,0 +1,192 @@ +#include +#include +#include + +#include "local_parameterization_se3.hpp" + +// Eigen's ostream operator is not compatible with ceres::Jet types. +// In particular, Eigen assumes that the scalar type (here Jet) can be +// casted to an arithmetic type, which is not true for ceres::Jet. +// Unfortunately, the ceres::Jet class does not define a conversion +// operator (http://en.cppreference.com/w/cpp/language/cast_operator). +// +// This workaround creates a template specialization for Eigen's cast_impl, +// when casting from a ceres::Jet type. It relies on Eigen's internal API and +// might break with future versions of Eigen. +namespace Eigen { +namespace internal { + +template +struct cast_impl, NewType> { + EIGEN_DEVICE_FUNC + static inline NewType run(ceres::Jet const& x) { + return static_cast(x.a); + } +}; + +} // namespace internal +} // namespace Eigen + +struct TestSE3CostFunctor { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + TestSE3CostFunctor(Sophus::SE3d T_aw) : T_aw(T_aw) {} + + template + bool operator()(T const* const sT_wa, T* sResiduals) const { + Eigen::Map const> const T_wa(sT_wa); + Eigen::Map > residuals(sResiduals); + + // We are able to mix Sophus types with doubles and Jet types without + // needing to cast to T. + residuals = (T_aw * T_wa).log(); + // Reverse order of multiplication. This forces the compiler to verify that + // (Jet, double) and (double, Jet) SE3 multiplication work correctly. + residuals = (T_wa * T_aw).log(); + // Finally, ensure that Jet-to-Jet multiplication works. + residuals = (T_wa * T_aw.cast()).log(); + return true; + } + + Sophus::SE3d T_aw; +}; + +struct TestPointCostFunctor { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + TestPointCostFunctor(Sophus::SE3d T_aw, Eigen::Vector3d point_a) + : T_aw(T_aw), point_a(point_a) {} + + template + bool operator()(T const* const sT_wa, T const* const spoint_b, + T* sResiduals) const { + using Vector3T = Eigen::Matrix; + Eigen::Map const> const T_wa(sT_wa); + Eigen::Map point_b(spoint_b); + Eigen::Map residuals(sResiduals); + + // Multiply SE3d by Jet Vector3. + Vector3T point_b_prime = T_aw * point_b; + // Ensure Jet SE3 multiplication with Jet Vector3. + point_b_prime = T_aw.cast() * point_b; + + // Multiply Jet SE3 with Vector3d. + Vector3T point_a_prime = T_wa * point_a; + // Ensure Jet SE3 multiplication with Jet Vector3. + point_a_prime = T_wa * point_a.cast(); + + residuals = point_b_prime - point_a_prime; + return true; + } + + Sophus::SE3d T_aw; + Eigen::Vector3d point_a; +}; + +bool test(Sophus::SE3d const& T_w_targ, Sophus::SE3d const& T_w_init, + Sophus::SE3d::Point const& point_a_init, + Sophus::SE3d::Point const& point_b) { + static constexpr int kNumPointParameters = 3; + + // Optimisation parameters. + Sophus::SE3d T_wr = T_w_init; + Sophus::SE3d::Point point_a = point_a_init; + + // Build the problem. + ceres::Problem problem; + + // Specify local update rule for our parameter + problem.AddParameterBlock(T_wr.data(), Sophus::SE3d::num_parameters, + new Sophus::test::LocalParameterizationSE3); + + // Create and add cost functions. Derivatives will be evaluated via + // automatic differentiation + ceres::CostFunction* cost_function1 = + new ceres::AutoDiffCostFunction( + new TestSE3CostFunctor(T_w_targ.inverse())); + problem.AddResidualBlock(cost_function1, NULL, T_wr.data()); + ceres::CostFunction* cost_function2 = + new ceres::AutoDiffCostFunction( + new TestPointCostFunctor(T_w_targ.inverse(), point_b)); + problem.AddResidualBlock(cost_function2, NULL, T_wr.data(), point_a.data()); + + // Set solver options (precision / method) + ceres::Solver::Options options; + options.gradient_tolerance = 0.01 * Sophus::Constants::epsilon(); + options.function_tolerance = 0.01 * Sophus::Constants::epsilon(); + options.linear_solver_type = ceres::DENSE_QR; + + // Solve + ceres::Solver::Summary summary; + Solve(options, &problem, &summary); + std::cout << summary.BriefReport() << std::endl; + + // Difference between target and parameter + double const mse = (T_w_targ.inverse() * T_wr).log().squaredNorm(); + bool const passed = mse < 10. * Sophus::Constants::epsilon(); + return passed; +} + +template +bool CreateSE3FromMatrix(const Eigen::Matrix& mat) { + Sophus::SE3 se3 = Sophus::SE3(mat); + std::cout << se3.translation().x() << std::endl; + return true; +} + +int main(int, char**) { + using SE3Type = Sophus::SE3; + using SO3Type = Sophus::SO3; + using Point = SE3Type::Point; + double const kPi = Sophus::Constants::pi(); + + std::vector se3_vec; + se3_vec.push_back( + SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), Point(0, 0, 0))); + se3_vec.push_back( + SE3Type(SO3Type::exp(Point(0.2, 0.5, -1.0)), Point(10, 0, 0))); + se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.)), Point(0, 100, 5))); + se3_vec.push_back( + SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), Point(0, 0, 0))); + se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), + Point(0, -0.00000001, 0.0000000001))); + se3_vec.push_back( + SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), Point(0.01, 0, 0))); + se3_vec.push_back(SE3Type(SO3Type::exp(Point(kPi, 0, 0)), Point(4, -5, 0))); + se3_vec.push_back( + SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), Point(0, 0, 0)) * + SE3Type(SO3Type::exp(Point(kPi, 0, 0)), Point(0, 0, 0)) * + SE3Type(SO3Type::exp(Point(-0.2, -0.5, -0.0)), Point(0, 0, 0))); + se3_vec.push_back( + SE3Type(SO3Type::exp(Point(0.3, 0.5, 0.1)), Point(2, 0, -7)) * + SE3Type(SO3Type::exp(Point(kPi, 0, 0)), Point(0, 0, 0)) * + SE3Type(SO3Type::exp(Point(-0.3, -0.5, -0.1)), Point(0, 6, 0))); + + std::vector point_vec; + point_vec.emplace_back(1.012, 2.73, -1.4); + point_vec.emplace_back(9.2, -7.3, -4.4); + point_vec.emplace_back(2.5, 0.1, 9.1); + point_vec.emplace_back(12.3, 1.9, 3.8); + point_vec.emplace_back(-3.21, 3.42, 2.3); + point_vec.emplace_back(-8.0, 6.1, -1.1); + point_vec.emplace_back(0.0, 2.5, 5.9); + point_vec.emplace_back(7.1, 7.8, -14); + point_vec.emplace_back(5.8, 9.2, 0.0); + + for (size_t i = 0; i < se3_vec.size(); ++i) { + const int other_index = (i + 3) % se3_vec.size(); + bool const passed = test(se3_vec[i], se3_vec[other_index], point_vec[i], + point_vec[other_index]); + if (!passed) { + std::cerr << "failed!" << std::endl << std::endl; + exit(-1); + } + } + + Eigen::Matrix, 4, 4> mat; + mat.setIdentity(); + std::cout << CreateSE3FromMatrix(mat) << std::endl; + + return 0; +} diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/CMakeLists.txt b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/CMakeLists.txt new file mode 100644 index 0000000..9c7624d --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/CMakeLists.txt @@ -0,0 +1,15 @@ +# Tests to run +SET( TEST_SOURCES test_common test_so2 test_se2 test_rxso2 test_sim2 test_so3 test_se3 test_rxso3 test_sim3 test_velocities test_geometry) +find_package( Ceres 1.6.0 QUIET ) + +FOREACH(test_src ${TEST_SOURCES}) + ADD_EXECUTABLE( ${test_src} ${test_src}.cpp tests.hpp ../../sophus/test_macros.hpp) + TARGET_LINK_LIBRARIES( ${test_src} sophus ) + if( Ceres_FOUND ) + TARGET_INCLUDE_DIRECTORIES( ${test_src} SYSTEM PRIVATE ${CERES_INCLUDE_DIRS}) + ADD_DEFINITIONS(-DSOPHUS_CERES) + endif(Ceres_FOUND) + ADD_TEST( ${test_src} ${test_src} ) +ENDFOREACH(test_src) + + diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_common.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_common.cpp new file mode 100644 index 0000000..1d0a158 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_common.cpp @@ -0,0 +1,61 @@ +#include + +#include +#include + +namespace Sophus { + +namespace { + +bool testFormatString() { + bool passed = true; + SOPHUS_TEST_EQUAL(passed, details::FormatString(), std::string()); + std::string test_str = "Hello World!"; + SOPHUS_TEST_EQUAL(passed, details::FormatString(test_str.c_str()), test_str); + SOPHUS_TEST_EQUAL(passed, details::FormatString("Number: %", 5), + std::string("Number: 5")); + SOPHUS_TEST_EQUAL(passed, + details::FormatString("Real: % msg %", 1.5, test_str), + std::string("Real: 1.5 msg Hello World!")); + SOPHUS_TEST_EQUAL(passed, + details::FormatString( + "vec: %", Eigen::Vector3f(0.f, 1.f, 1.5f).transpose()), + std::string("vec: 0 1 1.5")); + SOPHUS_TEST_EQUAL( + passed, details::FormatString("Number: %", 1, 2), + std::string("Number: 1\nFormat-Warning: There are 1 args unused.")); + return passed; +} + +bool testSmokeDetails() { + bool passed = true; + std::cout << details::pretty(4.2) << std::endl; + std::cout << details::pretty(Vector2f(1, 2)) << std::endl; + bool dummy = true; + details::testFailed(dummy, "dummyFunc", "dummyFile", 99, + "This is just a pratice alarm!"); + SOPHUS_TEST_EQUAL(passed, dummy, false); + + double val = transpose(42.0); + SOPHUS_TEST_EQUAL(passed, val, 42.0); + Matrix row = transpose(Vector2f(1, 7)); + Matrix expected_row(1, 7); + SOPHUS_TEST_EQUAL(passed, row, expected_row); + + optional opt(nullopt); + SOPHUS_TEST(passed, !opt); + + return passed; +} + +void runAll() { + std::cerr << "Common tests:" << std::endl; + bool passed = testFormatString(); + passed &= testSmokeDetails(); + processTestResult(passed); +} + +} // namespace +} // namespace Sophus + +int main() { Sophus::runAll(); } diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_geometry.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_geometry.cpp new file mode 100644 index 0000000..4b432bd --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_geometry.cpp @@ -0,0 +1,130 @@ +#include + +#include +#include +#include "tests.hpp" + +namespace Sophus { + +namespace { + +template +bool test2dGeometry() { + bool passed = true; + T const eps = Constants::epsilon(); + + for (int i = 0; i < 20; ++i) { + // Roundtrip test: + Vector2 normal_foo = Vector2::Random().normalized(); + Sophus::SO2 R_foo_plane = SO2FromNormal(normal_foo); + Vector2 resultNormal_foo = normalFromSO2(R_foo_plane); + SOPHUS_TEST_APPROX(passed, normal_foo, resultNormal_foo, eps); + } + + for (int i = 0; i < 20; ++i) { + // Roundtrip test: + Line2 line_foo = makeHyperplaneUnique( + Line2(Vector2::Random().normalized(), Vector2::Random())); + Sophus::SE2 T_foo_plane = SE2FromLine(line_foo); + Line2 resultPlane_foo = lineFromSE2(T_foo_plane); + SOPHUS_TEST_APPROX(passed, line_foo.normal().eval(), + resultPlane_foo.normal().eval(), eps); + SOPHUS_TEST_APPROX(passed, line_foo.offset(), resultPlane_foo.offset(), + eps); + } + + std::vector, Eigen::aligned_allocator>> Ts_foo_line = + getTestSE2s(); + + for (SE2 const& T_foo_line : Ts_foo_line) { + Line2 line_foo = lineFromSE2(T_foo_line); + SE2 T2_foo_line = SE2FromLine(line_foo); + Line2 line2_foo = lineFromSE2(T2_foo_line); + SOPHUS_TEST_APPROX(passed, line_foo.normal().eval(), + line2_foo.normal().eval(), eps); + SOPHUS_TEST_APPROX(passed, line_foo.offset(), line2_foo.offset(), eps); + } + + return passed; +} + +template +bool test3dGeometry() { + bool passed = true; + T const eps = Constants::epsilon(); + + Vector3 normal_foo = Vector3(1, 2, 0).normalized(); + Matrix3 R_foo_plane = rotationFromNormal(normal_foo); + SOPHUS_TEST_APPROX(passed, normal_foo, R_foo_plane.col(2).eval(), eps); + // Just testing that the function normalizes the input normal and hint + // direction correctly: + Matrix3 R2_foo_plane = rotationFromNormal((T(0.9) * normal_foo).eval()); + SOPHUS_TEST_APPROX(passed, normal_foo, R2_foo_plane.col(2).eval(), eps); + Matrix3 R3_foo_plane = + rotationFromNormal(normal_foo, Vector3(T(0.9), T(0), T(0)), + Vector3(T(0), T(1.1), T(0))); + SOPHUS_TEST_APPROX(passed, normal_foo, R3_foo_plane.col(2).eval(), eps); + + normal_foo = Vector3(1, 0, 0); + R_foo_plane = rotationFromNormal(normal_foo); + SOPHUS_TEST_APPROX(passed, normal_foo, R_foo_plane.col(2).eval(), eps); + SOPHUS_TEST_APPROX(passed, Vector3(0, 1, 0), R_foo_plane.col(1).eval(), + eps); + + normal_foo = Vector3(0, 1, 0); + R_foo_plane = rotationFromNormal(normal_foo); + SOPHUS_TEST_APPROX(passed, normal_foo, R_foo_plane.col(2).eval(), eps); + SOPHUS_TEST_APPROX(passed, Vector3(1, 0, 0), R_foo_plane.col(0).eval(), + eps); + + for (int i = 0; i < 20; ++i) { + // Roundtrip test: + Vector3 normal_foo = Vector3::Random().normalized(); + Sophus::SO3 R_foo_plane = SO3FromNormal(normal_foo); + Vector3 resultNormal_foo = normalFromSO3(R_foo_plane); + SOPHUS_TEST_APPROX(passed, normal_foo, resultNormal_foo, eps); + } + + for (int i = 0; i < 20; ++i) { + // Roundtrip test: + Plane3 plane_foo = makeHyperplaneUnique( + Plane3(Vector3::Random().normalized(), Vector3::Random())); + Sophus::SE3 T_foo_plane = SE3FromPlane(plane_foo); + Plane3 resultPlane_foo = planeFromSE3(T_foo_plane); + SOPHUS_TEST_APPROX(passed, plane_foo.normal().eval(), + resultPlane_foo.normal().eval(), eps); + SOPHUS_TEST_APPROX(passed, plane_foo.offset(), resultPlane_foo.offset(), + eps); + } + + std::vector, Eigen::aligned_allocator>> Ts_foo_plane = + getTestSE3s(); + + for (SE3 const& T_foo_plane : Ts_foo_plane) { + Plane3 plane_foo = planeFromSE3(T_foo_plane); + SE3 T2_foo_plane = SE3FromPlane(plane_foo); + Plane3 plane2_foo = planeFromSE3(T2_foo_plane); + SOPHUS_TEST_APPROX(passed, plane_foo.normal().eval(), + plane2_foo.normal().eval(), eps); + SOPHUS_TEST_APPROX(passed, plane_foo.offset(), plane2_foo.offset(), eps); + } + + return passed; +} + +void runAll() { + std::cerr << "Geometry (Lines/Planes) tests:" << std::endl; + std::cerr << "Double tests: " << std::endl; + bool passed = test2dGeometry(); + passed = test3dGeometry(); + processTestResult(passed); + std::cerr << "Float tests: " << std::endl; + passed = test2dGeometry(); + passed = test3dGeometry(); + processTestResult(passed); +} + +} // namespace +} // namespace Sophus + +int main() { Sophus::runAll(); } diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_rxso2.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_rxso2.cpp new file mode 100644 index 0000000..e12cea0 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_rxso2.cpp @@ -0,0 +1,261 @@ +#include + +#include +#include "tests.hpp" + +// Explicit instantiate all class templates so that all member methods +// get compiled and for code coverage analysis. +namespace Eigen { +template class Map>; +template class Map const>; +} // namespace Eigen + +namespace Sophus { + +template class RxSO2; +template class RxSO2; +#if SOPHUS_CERES +template class RxSO2>; +#endif + +template +class Tests { + public: + using SO2Type = SO2; + using RxSO2Type = RxSO2; + using RotationMatrixType = typename SO2::Transformation; + using Point = typename RxSO2::Point; + using Tangent = typename RxSO2::Tangent; + Scalar const kPi = Constants::pi(); + + Tests() { + rxso2_vec_.push_back(RxSO2Type::exp(Tangent(0.2, 1.))); + rxso2_vec_.push_back(RxSO2Type::exp(Tangent(0.2, 1.1))); + rxso2_vec_.push_back(RxSO2Type::exp(Tangent(0., 1.1))); + rxso2_vec_.push_back(RxSO2Type::exp(Tangent(0.00001, 0.))); + rxso2_vec_.push_back(RxSO2Type::exp(Tangent(0.00001, 0.00001))); + rxso2_vec_.push_back(RxSO2Type::exp(Tangent(kPi, 0.9))); + rxso2_vec_.push_back(RxSO2Type::exp(Tangent(0.2, 0)) * + RxSO2Type::exp(Tangent(kPi, 0.0)) * + RxSO2Type::exp(Tangent(-0.2, 0))); + rxso2_vec_.push_back(RxSO2Type::exp(Tangent(0.3, 0)) * + RxSO2Type::exp(Tangent(kPi, 0.001)) * + RxSO2Type::exp(Tangent(-0.3, 0))); + + Tangent tmp; + tmp << Scalar(0), Scalar(0); + tangent_vec_.push_back(tmp); + tmp << Scalar(1), Scalar(0); + tangent_vec_.push_back(tmp); + tmp << Scalar(1), Scalar(0.1); + tangent_vec_.push_back(tmp); + tmp << Scalar(0), Scalar(0.1); + tangent_vec_.push_back(tmp); + tmp << Scalar(0), Scalar(-0.1); + tangent_vec_.push_back(tmp); + tmp << Scalar(-1), Scalar(-0.1); + tangent_vec_.push_back(tmp); + tmp << Scalar(20), Scalar(2); + tangent_vec_.push_back(tmp); + + point_vec_.push_back(Point(Scalar(1), Scalar(4))); + point_vec_.push_back(Point(Scalar(1), Scalar(-3))); + } + + template + enable_if_t::value, bool> testFit() { + bool passed = true; + for (int i = 0; i < 10; ++i) { + Matrix2 M = Matrix2::Random(); + for (Scalar scale : {Scalar(0.01), Scalar(0.99), Scalar(1), Scalar(10)}) { + Matrix2 R = makeRotationMatrix(M); + Matrix2 sR = scale * R; + SOPHUS_TEST(passed, isScaledOrthogonalAndPositive(sR), + "isScaledOrthogonalAndPositive(sR): % *\n%", scale, R); + Matrix2 sR_cols_swapped; + sR_cols_swapped << sR.col(1), sR.col(0); + SOPHUS_TEST(passed, !isScaledOrthogonalAndPositive(sR_cols_swapped), + "isScaledOrthogonalAndPositive(-sR): % *\n%", scale, R); + } + } + return passed; + } + + template + enable_if_t::value, bool> testFit() { + return true; + } + + void runAll() { + bool passed = testLieProperties(); + passed &= testSaturation(); + passed &= testRawDataAcces(); + passed &= testConstructors(); + passed &= testFit(); + processTestResult(passed); + } + + private: + bool testLieProperties() { + LieGroupTests tests(rxso2_vec_, tangent_vec_, point_vec_); + return tests.doAllTestsPass(); + } + + bool testSaturation() { + bool passed = true; + RxSO2Type small1(Scalar(1.1) * Constants::epsilon(), SO2Type()); + RxSO2Type small2(Scalar(1.1) * Constants::epsilon(), + SO2Type::exp(Constants::pi())); + RxSO2Type saturated_product = small1 * small2; + SOPHUS_TEST_APPROX(passed, saturated_product.scale(), + Constants::epsilon(), + Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, saturated_product.so2().matrix(), + (small1.so2() * small2.so2()).matrix(), + Constants::epsilon()); + return passed; + } + + bool testRawDataAcces() { + bool passed = true; + Eigen::Matrix raw = {0, 1}; + Eigen::Map map_of_const_rxso2(raw.data()); + SOPHUS_TEST_APPROX(passed, map_of_const_rxso2.complex().eval(), raw, + Constants::epsilon()); + SOPHUS_TEST_EQUAL(passed, map_of_const_rxso2.complex().data(), raw.data()); + Eigen::Map const_shallow_copy = map_of_const_rxso2; + SOPHUS_TEST_EQUAL(passed, const_shallow_copy.complex().eval(), + map_of_const_rxso2.complex().eval()); + + Eigen::Matrix raw2 = {1, 0}; + Eigen::Map map_of_rxso2(raw2.data()); + SOPHUS_TEST_APPROX(passed, map_of_rxso2.complex().eval(), raw2, + Constants::epsilon()); + SOPHUS_TEST_EQUAL(passed, map_of_rxso2.complex().data(), raw2.data()); + Eigen::Map shallow_copy = map_of_rxso2; + SOPHUS_TEST_EQUAL(passed, shallow_copy.complex().eval(), + map_of_rxso2.complex().eval()); + + RxSO2Type const const_so2(raw2); + for (int i = 0; i < 2; ++i) { + SOPHUS_TEST_EQUAL(passed, const_so2.data()[i], raw2.data()[i]); + } + + RxSO2Type so2(raw2); + for (int i = 0; i < 2; ++i) { + so2.data()[i] = raw[i]; + } + + for (int i = 0; i < 2; ++i) { + SOPHUS_TEST_EQUAL(passed, so2.data()[i], raw.data()[i]); + } + + // regression: test that rotationMatrix API doesn't change underlying value + // for non-const-map and compiles at all for const-map + Eigen::Matrix raw3 = {Scalar(2), Scalar(0)}; + Eigen::Map map_of_rxso2_3(raw3.data()); + Eigen::Map const_map_of_rxso2_3(raw3.data()); + RxSO2Type rxso2_copy3 = map_of_rxso2_3; + const RotationMatrixType r_ref = map_of_rxso2_3.so2().matrix(); + + const RotationMatrixType r = map_of_rxso2_3.rotationMatrix(); + SOPHUS_TEST_APPROX(passed, r_ref, r, Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, map_of_rxso2_3.complex().eval(), + rxso2_copy3.complex().eval(), + Constants::epsilon()); + + const RotationMatrixType r_const = const_map_of_rxso2_3.rotationMatrix(); + SOPHUS_TEST_APPROX(passed, r_ref, r_const, Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, const_map_of_rxso2_3.complex().eval(), + rxso2_copy3.complex().eval(), + Constants::epsilon()); + + Eigen::Matrix data1, data2; + data1 << Scalar(.1), Scalar(.2); + data2 << Scalar(.5), Scalar(.4); + + Eigen::Map map1(data1.data()), map2(data2.data()); + + // map -> map assignment + map2 = map1; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), map2.matrix()); + + // map -> type assignment + RxSO2Type copy; + copy = map1; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), copy.matrix()); + + // type -> map assignment + copy = RxSO2Type::exp(Tangent(Scalar(0.2), Scalar(0.5))); + map1 = copy; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), copy.matrix()); + + return passed; + } + + bool testConstructors() { + bool passed = true; + RxSO2Type rxso2; + Scalar scale(1.2); + rxso2.setScale(scale); + SOPHUS_TEST_APPROX(passed, scale, rxso2.scale(), + Constants::epsilon(), "setScale"); + Scalar angle(0.2); + rxso2.setAngle(angle); + SOPHUS_TEST_APPROX(passed, angle, rxso2.angle(), + Constants::epsilon(), "setAngle"); + SOPHUS_TEST_APPROX(passed, scale, rxso2.scale(), + Constants::epsilon(), + "setAngle leaves scale as is"); + + auto so2 = rxso2_vec_[0].so2(); + rxso2.setSO2(so2); + SOPHUS_TEST_APPROX(passed, scale, rxso2.scale(), + Constants::epsilon(), "setSO2"); + SOPHUS_TEST_APPROX(passed, RxSO2Type(scale, so2).matrix(), rxso2.matrix(), + Constants::epsilon(), "RxSO2(scale, SO2)"); + SOPHUS_TEST_APPROX(passed, RxSO2Type(scale, so2.matrix()).matrix(), + rxso2.matrix(), Constants::epsilon(), + "RxSO2(scale, SO2)"); + Matrix2 R = SO2::exp(Scalar(0.2)).matrix(); + Matrix2 sR = R * Scalar(1.3); + SOPHUS_TEST_APPROX(passed, RxSO2Type(sR).matrix(), sR, + Constants::epsilon(), "RxSO2(sR)"); + rxso2.setScaledRotationMatrix(sR); + SOPHUS_TEST_APPROX(passed, sR, rxso2.matrix(), Constants::epsilon(), + "setScaleRotationMatrix"); + rxso2.setScale(scale); + rxso2.setRotationMatrix(R); + SOPHUS_TEST_APPROX(passed, R, rxso2.rotationMatrix(), + Constants::epsilon(), "setRotationMatrix"); + SOPHUS_TEST_APPROX(passed, scale, rxso2.scale(), + Constants::epsilon(), "setScale"); + + return passed; + } + + std::vector> rxso2_vec_; + std::vector> tangent_vec_; + std::vector> point_vec_; +}; + +int test_rxso2() { + using std::cerr; + using std::endl; + + cerr << "Test RxSO2" << endl << endl; + cerr << "Double tests: " << endl; + Tests().runAll(); + cerr << "Float tests: " << endl; + Tests().runAll(); + +#if SOPHUS_CERES + cerr << "ceres::Jet tests: " << endl; + Tests>().runAll(); +#endif + return 0; +} + +} // namespace Sophus + +int main() { return Sophus::test_rxso2(); } diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_rxso3.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_rxso3.cpp new file mode 100644 index 0000000..c5eb6cb --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_rxso3.cpp @@ -0,0 +1,283 @@ +#include + +#include +#include "tests.hpp" + +// Explicit instantiate all class templates so that all member methods +// get compiled and for code coverage analysis. +namespace Eigen { +template class Map>; +template class Map const>; +} // namespace Eigen + +namespace Sophus { + +template class RxSO3; +template class RxSO3; +#if SOPHUS_CERES +template class RxSO3>; +#endif + +template +class Tests { + public: + using Scalar = Scalar_; + using SO3Type = SO3; + using RxSO3Type = RxSO3; + using RotationMatrixType = typename SO3::Transformation; + using Point = typename RxSO3::Point; + using Tangent = typename RxSO3::Tangent; + Scalar const kPi = Constants::pi(); + + Tests() { + rxso3_vec_.push_back(RxSO3Type::exp( + Tangent(Scalar(0.2), Scalar(0.5), Scalar(0.0), Scalar(1.)))); + rxso3_vec_.push_back(RxSO3Type::exp( + Tangent(Scalar(0.2), Scalar(0.5), Scalar(-1.0), Scalar(1.1)))); + rxso3_vec_.push_back(RxSO3Type::exp( + Tangent(Scalar(0.), Scalar(0.), Scalar(0.), Scalar(1.1)))); + rxso3_vec_.push_back(RxSO3Type::exp( + Tangent(Scalar(0.), Scalar(0.), Scalar(0.00001), Scalar(0.)))); + rxso3_vec_.push_back(RxSO3Type::exp( + Tangent(Scalar(0.), Scalar(0.), Scalar(0.00001), Scalar(0.00001)))); + rxso3_vec_.push_back(RxSO3Type::exp( + Tangent(Scalar(0.), Scalar(0.), Scalar(0.00001), Scalar(0)))); + rxso3_vec_.push_back( + RxSO3Type::exp(Tangent(kPi, Scalar(0), Scalar(0), Scalar(0.9)))); + rxso3_vec_.push_back( + RxSO3Type::exp( + Tangent(Scalar(0.2), Scalar(-0.5), Scalar(0), Scalar(0))) * + RxSO3Type::exp(Tangent(kPi, Scalar(0), Scalar(0), Scalar(0))) * + RxSO3Type::exp( + Tangent(-Scalar(0.2), Scalar(-0.5), Scalar(0), Scalar(0)))); + rxso3_vec_.push_back( + RxSO3Type::exp( + Tangent(Scalar(0.3), Scalar(0.5), Scalar(0.1), Scalar(0))) * + RxSO3Type::exp(Tangent(kPi, Scalar(0), Scalar(0), Scalar(0))) * + RxSO3Type::exp( + Tangent(Scalar(-0.3), Scalar(-0.5), Scalar(-0.1), Scalar(0)))); + + Tangent tmp; + tmp << Scalar(0), Scalar(0), Scalar(0), Scalar(0); + tangent_vec_.push_back(tmp); + tmp << Scalar(1), Scalar(0), Scalar(0), Scalar(0); + tangent_vec_.push_back(tmp); + tmp << Scalar(1), Scalar(0), Scalar(0), Scalar(0.1); + tangent_vec_.push_back(tmp); + tmp << Scalar(0), Scalar(1), Scalar(0), Scalar(0.1); + tangent_vec_.push_back(tmp); + tmp << Scalar(0), Scalar(0), Scalar(1), Scalar(-0.1); + tangent_vec_.push_back(tmp); + tmp << Scalar(-1), Scalar(1), Scalar(0), Scalar(-0.1); + tangent_vec_.push_back(tmp); + tmp << Scalar(20), Scalar(-1), Scalar(0), Scalar(2); + tangent_vec_.push_back(tmp); + + point_vec_.push_back(Point(Scalar(1), Scalar(2), Scalar(4))); + point_vec_.push_back(Point(Scalar(1), Scalar(-3), Scalar(0.5))); + } + + void runAll() { + bool passed = testLieProperties(); + passed &= testSaturation(); + passed &= testRawDataAcces(); + passed &= testConstructors(); + passed &= testFit(); + processTestResult(passed); + } + + private: + bool testLieProperties() { + LieGroupTests tests(rxso3_vec_, tangent_vec_, point_vec_); + return tests.doAllTestsPass(); + } + + bool testSaturation() { + bool passed = true; + RxSO3Type small1(Constants::epsilon(), SO3Type()); + RxSO3Type small2(Constants::epsilon(), + SO3Type::exp(Vector3(Constants::pi(), + Scalar(0), Scalar(0)))); + RxSO3Type saturated_product = small1 * small2; + SOPHUS_TEST_APPROX(passed, saturated_product.scale(), + Constants::epsilon(), + Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, saturated_product.so3().matrix(), + (small1.so3() * small2.so3()).matrix(), + Constants::epsilon()); + return passed; + } + + bool testRawDataAcces() { + bool passed = true; + Eigen::Matrix raw = {Scalar(0), Scalar(1), Scalar(0), + Scalar(0)}; + Eigen::Map map_of_const_rxso3(raw.data()); + SOPHUS_TEST_APPROX(passed, map_of_const_rxso3.quaternion().coeffs().eval(), + raw, Constants::epsilon()); + SOPHUS_TEST_EQUAL(passed, map_of_const_rxso3.quaternion().coeffs().data(), + raw.data()); + Eigen::Map const_shallow_copy = map_of_const_rxso3; + SOPHUS_TEST_EQUAL(passed, const_shallow_copy.quaternion().coeffs().eval(), + map_of_const_rxso3.quaternion().coeffs().eval()); + + Eigen::Matrix raw2 = {Scalar(1), Scalar(0), Scalar(0), + Scalar(0)}; + Eigen::Map map_of_rxso3(raw.data()); + Eigen::Quaternion quat; + quat.coeffs() = raw2; + map_of_rxso3.setQuaternion(quat); + SOPHUS_TEST_APPROX(passed, map_of_rxso3.quaternion().coeffs().eval(), raw2, + Constants::epsilon()); + SOPHUS_TEST_EQUAL(passed, map_of_rxso3.quaternion().coeffs().data(), + raw.data()); + SOPHUS_TEST_NEQ(passed, map_of_rxso3.quaternion().coeffs().data(), + quat.coeffs().data()); + Eigen::Map shallow_copy = map_of_rxso3; + SOPHUS_TEST_EQUAL(passed, shallow_copy.quaternion().coeffs().eval(), + map_of_rxso3.quaternion().coeffs().eval()); + + RxSO3Type const const_so3(quat); + for (int i = 0; i < 4; ++i) { + SOPHUS_TEST_EQUAL(passed, const_so3.data()[i], raw2.data()[i]); + } + + RxSO3Type so3(quat); + for (int i = 0; i < 4; ++i) { + so3.data()[i] = raw[i]; + } + + for (int i = 0; i < 4; ++i) { + SOPHUS_TEST_EQUAL(passed, so3.data()[i], raw.data()[i]); + } + + // regression: test that rotationMatrix API doesn't change underlying value + // for non-const-map and compiles at all for const-map + Eigen::Matrix raw3 = {Scalar(2), Scalar(0), Scalar(0), + Scalar(0)}; + Eigen::Map map_of_rxso3_3(raw3.data()); + Eigen::Map const_map_of_rxso3_3(raw3.data()); + RxSO3Type rxso3_copy3 = map_of_rxso3_3; + const RotationMatrixType r_ref = map_of_rxso3_3.so3().matrix(); + + const RotationMatrixType r = map_of_rxso3_3.rotationMatrix(); + SOPHUS_TEST_APPROX(passed, r_ref, r, Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, map_of_rxso3_3.quaternion().coeffs().eval(), + rxso3_copy3.quaternion().coeffs().eval(), + Constants::epsilon()); + + const RotationMatrixType r_const = const_map_of_rxso3_3.rotationMatrix(); + SOPHUS_TEST_APPROX(passed, r_ref, r_const, Constants::epsilon()); + SOPHUS_TEST_APPROX( + passed, const_map_of_rxso3_3.quaternion().coeffs().eval(), + rxso3_copy3.quaternion().coeffs().eval(), Constants::epsilon()); + + Eigen::Matrix data1, data2; + data1 << Scalar(.1), Scalar(.2), Scalar(.3), Scalar(.4); + data2 << Scalar(.5), Scalar(.4), Scalar(.3), Scalar(.2); + + Eigen::Map map1(data1.data()), map2(data2.data()); + + // map -> map assignment + map2 = map1; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), map2.matrix()); + + // map -> type assignment + RxSO3Type copy; + copy = map1; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), copy.matrix()); + + // type -> map assignment + copy = RxSO3Type::exp(Tangent(Scalar(0.2), Scalar(0.5), + Scalar(-1.0), Scalar(1.1))); + map1 = copy; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), copy.matrix()); + + return passed; + } + + bool testConstructors() { + bool passed = true; + RxSO3Type rxso3; + Scalar scale(1.2); + rxso3.setScale(scale); + SOPHUS_TEST_APPROX(passed, scale, rxso3.scale(), + Constants::epsilon(), "setScale"); + auto so3 = rxso3_vec_[0].so3(); + rxso3.setSO3(so3); + SOPHUS_TEST_APPROX(passed, scale, rxso3.scale(), + Constants::epsilon(), "setScale"); + SOPHUS_TEST_APPROX(passed, RxSO3Type(scale, so3).matrix(), rxso3.matrix(), + Constants::epsilon(), "RxSO3(scale, SO3)"); + SOPHUS_TEST_APPROX(passed, RxSO3Type(scale, so3.matrix()).matrix(), + rxso3.matrix(), Constants::epsilon(), + "RxSO3(scale, SO3)"); + Matrix3 R = + SO3::exp(Point(Scalar(0.2), Scalar(0.5), Scalar(-1.0))) + .matrix(); + Matrix3 sR = R * Scalar(1.3); + SOPHUS_TEST_APPROX(passed, RxSO3Type(sR).matrix(), sR, + Constants::epsilon(), "RxSO3(sR)"); + rxso3.setScaledRotationMatrix(sR); + SOPHUS_TEST_APPROX(passed, sR, rxso3.matrix(), Constants::epsilon(), + "setScaleRotationMatrix"); + rxso3.setScale(scale); + rxso3.setRotationMatrix(R); + SOPHUS_TEST_APPROX(passed, R, rxso3.rotationMatrix(), + Constants::epsilon(), "setRotationMatrix"); + SOPHUS_TEST_APPROX(passed, scale, rxso3.scale(), + Constants::epsilon(), "setScale"); + + return passed; + } + + template + enable_if_t::value, bool> testFit() { + bool passed = true; + for (int i = 0; i < 10; ++i) { + Matrix3 M = Matrix3::Random(); + for (Scalar scale : {Scalar(0.01), Scalar(0.99), Scalar(1), Scalar(10)}) { + Matrix3 R = makeRotationMatrix(M); + Matrix3 sR = scale * R; + SOPHUS_TEST(passed, isScaledOrthogonalAndPositive(sR), + "isScaledOrthogonalAndPositive(sR): % *\n%", scale, R); + Matrix3 sR_cols_swapped; + sR_cols_swapped << sR.col(1), sR.col(0), sR.col(2); + SOPHUS_TEST(passed, !isScaledOrthogonalAndPositive(sR_cols_swapped), + "isScaledOrthogonalAndPositive(-sR): % *\n%", scale, R); + } + } + return passed; + } + + template + enable_if_t::value, bool> testFit() { + return true; + } + + std::vector> rxso3_vec_; + std::vector> tangent_vec_; + std::vector> point_vec_; +}; + +int test_rxso3() { + using std::cerr; + using std::endl; + + cerr << "Test RxSO3" << endl << endl; + cerr << "Double tests: " << endl; + Tests().runAll(); + cerr << "Float tests: " << endl; + Tests().runAll(); + +#if SOPHUS_CERES + cerr << "ceres::Jet tests: " << endl; + Tests>().runAll(); +#endif + + return 0; +} + +} // namespace Sophus + +int main() { return Sophus::test_rxso3(); } diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_se2.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_se2.cpp new file mode 100644 index 0000000..e743379 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_se2.cpp @@ -0,0 +1,260 @@ +#include + +#include +#include +#include "tests.hpp" + +// Explicit instantiate all class templates so that all member methods +// get compiled and for code coverage analysis. +namespace Eigen { +template class Map>; +template class Map const>; +} // namespace Eigen + +namespace Sophus { + +template class SE2; +template class SE2; +#if SOPHUS_CERES +template class SE2>; +#endif + +template +class Tests { + public: + using SE2Type = SE2; + using SO2Type = SO2; + using Point = typename SE2::Point; + using Tangent = typename SE2::Tangent; + Scalar const kPi = Constants::pi(); + + Tests() { + se2_vec_.push_back( + SE2Type(SO2Type(Scalar(0.0)), Point(Scalar(0), Scalar(0)))); + se2_vec_.push_back( + SE2Type(SO2Type(Scalar(0.2)), Point(Scalar(10), Scalar(0)))); + se2_vec_.push_back( + SE2Type(SO2Type(Scalar(0.)), Point(Scalar(0), Scalar(100)))); + se2_vec_.push_back( + SE2Type(SO2Type(Scalar(-1.)), Point(Scalar(20), -Scalar(1)))); + se2_vec_.push_back( + SE2Type(SO2Type(Scalar(0.00001)), + Point(Scalar(-0.00000001), Scalar(0.0000000001)))); + se2_vec_.push_back( + SE2Type(SO2Type(Scalar(0.2)), Point(Scalar(0), Scalar(0))) * + SE2Type(SO2Type(kPi), Point(Scalar(0), Scalar(0))) * + SE2Type(SO2Type(Scalar(-0.2)), Point(Scalar(0), Scalar(0)))); + se2_vec_.push_back( + SE2Type(SO2Type(Scalar(0.3)), Point(Scalar(2), Scalar(0))) * + SE2Type(SO2Type(kPi), Point(Scalar(0), Scalar(0))) * + SE2Type(SO2Type(Scalar(-0.3)), Point(Scalar(0), Scalar(6)))); + + Tangent tmp; + tmp << Scalar(0), Scalar(0), Scalar(0); + tangent_vec_.push_back(tmp); + tmp << Scalar(1), Scalar(0), Scalar(0); + tangent_vec_.push_back(tmp); + tmp << Scalar(0), Scalar(1), Scalar(1); + tangent_vec_.push_back(tmp); + tmp << Scalar(-1), Scalar(1), Scalar(0); + tangent_vec_.push_back(tmp); + tmp << Scalar(20), Scalar(-1), Scalar(-1); + tangent_vec_.push_back(tmp); + tmp << Scalar(30), Scalar(5), Scalar(20); + tangent_vec_.push_back(tmp); + + point_vec_.push_back(Point(1, 2)); + point_vec_.push_back(Point(1, -3)); + } + + void runAll() { + bool passed = testLieProperties(); + passed &= testRawDataAcces(); + passed &= testMutatingAccessors(); + passed &= testConstructors(); + passed &= testFit(); + processTestResult(passed); + } + + private: + bool testLieProperties() { + LieGroupTests tests(se2_vec_, tangent_vec_, point_vec_); + return tests.doAllTestsPass(); + } + + bool testRawDataAcces() { + bool passed = true; + Eigen::Matrix raw; + raw << Scalar(0), Scalar(1), Scalar(0), Scalar(3); + Eigen::Map const_se2_map(raw.data()); + SOPHUS_TEST_APPROX(passed, const_se2_map.unit_complex().eval(), + raw.template head<2>().eval(), + Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, const_se2_map.translation().eval(), + raw.template tail<2>().eval(), + Constants::epsilon()); + SOPHUS_TEST_EQUAL(passed, const_se2_map.unit_complex().data(), raw.data()); + SOPHUS_TEST_EQUAL(passed, const_se2_map.translation().data(), + raw.data() + 2); + Eigen::Map const_shallow_copy = const_se2_map; + SOPHUS_TEST_EQUAL(passed, const_shallow_copy.unit_complex().eval(), + const_se2_map.unit_complex().eval()); + SOPHUS_TEST_EQUAL(passed, const_shallow_copy.translation().eval(), + const_se2_map.translation().eval()); + + Eigen::Matrix raw2; + raw2 << Scalar(1), Scalar(0), Scalar(3), Scalar(1); + Eigen::Map map_of_se3(raw.data()); + map_of_se3.setComplex(raw2.template head<2>()); + map_of_se3.translation() = raw2.template tail<2>(); + SOPHUS_TEST_APPROX(passed, map_of_se3.unit_complex().eval(), + raw2.template head<2>().eval(), + Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, map_of_se3.translation().eval(), + raw2.template tail<2>().eval(), + Constants::epsilon()); + SOPHUS_TEST_EQUAL(passed, map_of_se3.unit_complex().data(), raw.data()); + SOPHUS_TEST_EQUAL(passed, map_of_se3.translation().data(), raw.data() + 2); + SOPHUS_TEST_NEQ(passed, map_of_se3.unit_complex().data(), raw2.data()); + Eigen::Map shallow_copy = map_of_se3; + SOPHUS_TEST_EQUAL(passed, shallow_copy.unit_complex().eval(), + map_of_se3.unit_complex().eval()); + SOPHUS_TEST_EQUAL(passed, shallow_copy.translation().eval(), + map_of_se3.translation().eval()); + Eigen::Map const const_map_of_se2 = map_of_se3; + SOPHUS_TEST_EQUAL(passed, const_map_of_se2.unit_complex().eval(), + map_of_se3.unit_complex().eval()); + SOPHUS_TEST_EQUAL(passed, const_map_of_se2.translation().eval(), + map_of_se3.translation().eval()); + + SE2Type const const_se2(raw2.template head<2>().eval(), + raw2.template tail<2>().eval()); + for (int i = 0; i < 4; ++i) { + SOPHUS_TEST_EQUAL(passed, const_se2.data()[i], raw2.data()[i]); + } + + SE2Type se2(raw2.template head<2>().eval(), raw2.template tail<2>().eval()); + for (int i = 0; i < 4; ++i) { + SOPHUS_TEST_EQUAL(passed, se2.data()[i], raw2.data()[i]); + } + + for (int i = 0; i < 4; ++i) { + SOPHUS_TEST_EQUAL(passed, se2.data()[i], raw.data()[i]); + } + + SE2Type trans = SE2Type::transX(Scalar(0.2)); + SOPHUS_TEST_APPROX(passed, trans.translation().x(), Scalar(0.2), + Constants::epsilon()); + trans = SE2Type::transY(Scalar(0.7)); + SOPHUS_TEST_APPROX(passed, trans.translation().y(), Scalar(0.7), + Constants::epsilon()); + + Eigen::Matrix data1, data2; + data1 << Scalar(0), Scalar(1), Scalar(1), Scalar(2); + data1 << Scalar(1), Scalar(0), Scalar(2), Scalar(1); + + Eigen::Map map1(data1.data()), map2(data2.data()); + + // map -> map assignment + map2 = map1; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), map2.matrix()); + + // map -> type assignment + SE2Type copy; + copy = map1; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), copy.matrix()); + + // type -> map assignment + copy = SE2Type::trans(Scalar(4), Scalar(5)) * SE2Type::rot(Scalar(0.5)); + map1 = copy; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), copy.matrix()); + + return passed; + } + + bool testMutatingAccessors() { + bool passed = true; + SE2Type se2; + SO2Type R(Scalar(0.2)); + se2.setRotationMatrix(R.matrix()); + SOPHUS_TEST_APPROX(passed, se2.rotationMatrix(), R.matrix(), + Constants::epsilon()); + + Eigen::Matrix raw; + raw << Scalar(1), Scalar(0), Scalar(3), Scalar(1); + Eigen::Map map_of_se2(raw.data()); + map_of_se2.setRotationMatrix(R.matrix()); + SOPHUS_TEST_APPROX(passed, map_of_se2.rotationMatrix(), R.matrix(), + Constants::epsilon()); + + return passed; + } + + bool testConstructors() { + bool passed = true; + Matrix3 I = Matrix3::Identity(); + SOPHUS_TEST_EQUAL(passed, SE2Type().matrix(), I); + + SE2Type se2 = se2_vec_.front(); + Point translation = se2.translation(); + SO2Type so2 = se2.so2(); + + SOPHUS_TEST_APPROX(passed, SE2Type(so2.log(), translation).matrix(), + se2.matrix(), Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, SE2Type(so2, translation).matrix(), se2.matrix(), + Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, SE2Type(so2.matrix(), translation).matrix(), + se2.matrix(), Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, + SE2Type(so2.unit_complex(), translation).matrix(), + se2.matrix(), Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, SE2Type(se2.matrix()).matrix(), se2.matrix(), + Constants::epsilon()); + + return passed; + } + + template + enable_if_t::value, bool> testFit() { + bool passed = true; + for (int i = 0; i < 100; ++i) { + Matrix3 T = Matrix3::Random(); + SE2Type se2 = SE2Type::fitToSE2(T); + SE2Type se2_2 = SE2Type::fitToSE2(se2.matrix()); + + SOPHUS_TEST_APPROX(passed, se2.matrix(), se2_2.matrix(), + Constants::epsilon()); + } + return passed; + } + + template + enable_if_t::value, bool> testFit() { + return true; + } + + std::vector> se2_vec_; + std::vector> tangent_vec_; + std::vector> point_vec_; +}; + +int test_se2() { + using std::cerr; + using std::endl; + + cerr << "Test SE2" << endl << endl; + cerr << "Double tests: " << endl; + Tests().runAll(); + cerr << "Float tests: " << endl; + Tests().runAll(); + +#if SOPHUS_CERES + cerr << "ceres::Jet tests: " << endl; + Tests>().runAll(); +#endif + + return 0; +} +} // namespace Sophus + +int main() { return Sophus::test_se2(); } diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_se3.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_se3.cpp new file mode 100644 index 0000000..6ce0e85 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_se3.cpp @@ -0,0 +1,267 @@ +#include + +#include +#include "tests.hpp" + +// Explicit instantiate all class templates so that all member methods +// get compiled and for code coverage analysis. +namespace Eigen { +template class Map>; +template class Map const>; +} // namespace Eigen + +namespace Sophus { + +template class SE3; +template class SE3; +#if SOPHUS_CERES +template class SE3>; +#endif + +template +class Tests { + public: + using SE3Type = SE3; + using SO3Type = SO3; + using Point = typename SE3::Point; + using Tangent = typename SE3::Tangent; + Scalar const kPi = Constants::pi(); + + Tests() { + se3_vec_ = getTestSE3s(); + + Tangent tmp; + tmp << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0); + tangent_vec_.push_back(tmp); + tmp << Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0); + tangent_vec_.push_back(tmp); + tmp << Scalar(0), Scalar(1), Scalar(0), Scalar(1), Scalar(0), Scalar(0); + tangent_vec_.push_back(tmp); + tmp << Scalar(0), Scalar(-5), Scalar(10), Scalar(0), Scalar(0), Scalar(0); + tangent_vec_.push_back(tmp); + tmp << Scalar(-1), Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(1); + tangent_vec_.push_back(tmp); + tmp << Scalar(20), Scalar(-1), Scalar(0), Scalar(-1), Scalar(1), Scalar(0); + tangent_vec_.push_back(tmp); + tmp << Scalar(30), Scalar(5), Scalar(-1), Scalar(20), Scalar(-1), Scalar(0); + tangent_vec_.push_back(tmp); + + point_vec_.push_back(Point(Scalar(1), Scalar(2), Scalar(4))); + point_vec_.push_back(Point(Scalar(1), Scalar(-3), Scalar(0.5))); + } + + void runAll() { + bool passed = testLieProperties(); + passed &= testRawDataAcces(); + passed &= testMutatingAccessors(); + passed &= testConstructors(); + passed &= testFit(); + processTestResult(passed); + } + + private: + bool testLieProperties() { + LieGroupTests tests(se3_vec_, tangent_vec_, point_vec_); + return tests.doAllTestsPass(); + } + + bool testRawDataAcces() { + bool passed = true; + Eigen::Matrix raw; + raw << Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1), Scalar(3), + Scalar(2); + Eigen::Map map_of_const_se3(raw.data()); + SOPHUS_TEST_APPROX( + passed, map_of_const_se3.unit_quaternion().coeffs().eval(), + raw.template head<4>().eval(), Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, map_of_const_se3.translation().eval(), + raw.template tail<3>().eval(), + Constants::epsilon()); + SOPHUS_TEST_EQUAL( + passed, map_of_const_se3.unit_quaternion().coeffs().data(), raw.data()); + SOPHUS_TEST_EQUAL(passed, map_of_const_se3.translation().data(), + raw.data() + 4); + Eigen::Map const_shallow_copy = map_of_const_se3; + SOPHUS_TEST_EQUAL(passed, + const_shallow_copy.unit_quaternion().coeffs().eval(), + map_of_const_se3.unit_quaternion().coeffs().eval()); + SOPHUS_TEST_EQUAL(passed, const_shallow_copy.translation().eval(), + map_of_const_se3.translation().eval()); + + Eigen::Matrix raw2; + raw2 << Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(3), Scalar(2), + Scalar(1); + Eigen::Map map_of_se3(raw.data()); + Eigen::Quaternion quat; + quat.coeffs() = raw2.template head<4>(); + map_of_se3.setQuaternion(quat); + map_of_se3.translation() = raw2.template tail<3>(); + SOPHUS_TEST_APPROX(passed, map_of_se3.unit_quaternion().coeffs().eval(), + raw2.template head<4>().eval(), + Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, map_of_se3.translation().eval(), + raw2.template tail<3>().eval(), + Constants::epsilon()); + SOPHUS_TEST_EQUAL(passed, map_of_se3.unit_quaternion().coeffs().data(), + raw.data()); + SOPHUS_TEST_EQUAL(passed, map_of_se3.translation().data(), raw.data() + 4); + SOPHUS_TEST_NEQ(passed, map_of_se3.unit_quaternion().coeffs().data(), + quat.coeffs().data()); + Eigen::Map shallow_copy = map_of_se3; + SOPHUS_TEST_EQUAL(passed, shallow_copy.unit_quaternion().coeffs().eval(), + map_of_se3.unit_quaternion().coeffs().eval()); + SOPHUS_TEST_EQUAL(passed, shallow_copy.translation().eval(), + map_of_se3.translation().eval()); + Eigen::Map const const_map_of_se3 = map_of_se3; + SOPHUS_TEST_EQUAL(passed, + const_map_of_se3.unit_quaternion().coeffs().eval(), + map_of_se3.unit_quaternion().coeffs().eval()); + SOPHUS_TEST_EQUAL(passed, const_map_of_se3.translation().eval(), + map_of_se3.translation().eval()); + + SE3Type const const_se3(quat, raw2.template tail<3>().eval()); + for (int i = 0; i < 7; ++i) { + SOPHUS_TEST_EQUAL(passed, const_se3.data()[i], raw2.data()[i]); + } + + SE3Type se3(quat, raw2.template tail<3>().eval()); + for (int i = 0; i < 7; ++i) { + SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw2.data()[i]); + } + + for (int i = 0; i < 7; ++i) { + SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw.data()[i]); + } + SE3Type trans = SE3Type::transX(Scalar(0.2)); + SOPHUS_TEST_APPROX(passed, trans.translation().x(), Scalar(0.2), + Constants::epsilon()); + trans = SE3Type::transY(Scalar(0.7)); + SOPHUS_TEST_APPROX(passed, trans.translation().y(), Scalar(0.7), + Constants::epsilon()); + trans = SE3Type::transZ(Scalar(-0.2)); + SOPHUS_TEST_APPROX(passed, trans.translation().z(), Scalar(-0.2), + Constants::epsilon()); + Tangent t; + t << Scalar(0), Scalar(0), Scalar(0), Scalar(0.2), Scalar(0), Scalar(0); + SOPHUS_TEST_EQUAL(passed, SE3Type::rotX(Scalar(0.2)).matrix(), + SE3Type::exp(t).matrix()); + t << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(-0.2), Scalar(0); + SOPHUS_TEST_EQUAL(passed, SE3Type::rotY(Scalar(-0.2)).matrix(), + SE3Type::exp(t).matrix()); + t << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1.1); + SOPHUS_TEST_EQUAL(passed, SE3Type::rotZ(Scalar(1.1)).matrix(), + SE3Type::exp(t).matrix()); + + Eigen::Matrix data1, data2; + data1 << Scalar(0), Scalar(1), Scalar(0), Scalar(0), + Scalar(1), Scalar(2), Scalar(3); + data1 << Scalar(0), Scalar(0), Scalar(1), Scalar(0), + Scalar(3), Scalar(2), Scalar(1); + + Eigen::Map map1(data1.data()), map2(data2.data()); + + // map -> map assignment + map2 = map1; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), map2.matrix()); + + // map -> type assignment + SE3Type copy; + copy = map1; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), copy.matrix()); + + // type -> map assignment + copy = SE3Type::trans(Scalar(4), Scalar(5), Scalar(6)) + * SE3Type::rotZ(Scalar(0.5)); + map1 = copy; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), copy.matrix()); + + return passed; + } + + bool testMutatingAccessors() { + bool passed = true; + SE3Type se3; + SO3Type R(SO3Type::exp(Point(Scalar(0.2), Scalar(0.5), Scalar(0.0)))); + se3.setRotationMatrix(R.matrix()); + SOPHUS_TEST_APPROX(passed, se3.rotationMatrix(), R.matrix(), + Constants::epsilon()); + + return passed; + } + + bool testConstructors() { + bool passed = true; + Eigen::Matrix I = Eigen::Matrix::Identity(); + SOPHUS_TEST_EQUAL(passed, SE3Type().matrix(), I); + + SE3Type se3 = se3_vec_.front(); + Point translation = se3.translation(); + SO3Type so3 = se3.so3(); + + SOPHUS_TEST_APPROX(passed, SE3Type(so3, translation).matrix(), se3.matrix(), + Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, SE3Type(so3.matrix(), translation).matrix(), + se3.matrix(), Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, + SE3Type(so3.unit_quaternion(), translation).matrix(), + se3.matrix(), Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, SE3Type(se3.matrix()).matrix(), se3.matrix(), + Constants::epsilon()); + + return passed; + } + + template + enable_if_t::value, bool> testFit() { + bool passed = true; + + for (int i = 0; i < 100; ++i) { + Matrix4 T = Matrix4::Random(); + SE3Type se3 = SE3Type::fitToSE3(T); + SE3Type se3_2 = SE3Type::fitToSE3(se3.matrix()); + + SOPHUS_TEST_APPROX(passed, se3.matrix(), se3_2.matrix(), + Constants::epsilon()); + } + for (Scalar const angle : + {Scalar(0.0), Scalar(0.1), Scalar(0.3), Scalar(-0.7)}) { + SOPHUS_TEST_APPROX(passed, SE3Type::rotX(angle).angleX(), angle, + Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, SE3Type::rotY(angle).angleY(), angle, + Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, SE3Type::rotZ(angle).angleZ(), angle, + Constants::epsilon()); + } + return passed; + } + + template + enable_if_t::value, bool> testFit() { + return true; + } + + std::vector> se3_vec_; + std::vector> tangent_vec_; + std::vector> point_vec_; +}; + +int test_se3() { + using std::cerr; + using std::endl; + + cerr << "Test SE3" << endl << endl; + cerr << "Double tests: " << endl; + Tests().runAll(); + cerr << "Float tests: " << endl; + Tests().runAll(); + +#if SOPHUS_CERES + cerr << "ceres::Jet tests: " << endl; + Tests>().runAll(); +#endif + + return 0; +} +} // namespace Sophus + +int main() { return Sophus::test_se3(); } diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_sim2.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_sim2.cpp new file mode 100644 index 0000000..1e1a6a7 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_sim2.cpp @@ -0,0 +1,225 @@ +#include + +#include + +#include +#include "tests.hpp" + +// Explicit instantiate all class templates so that all member methods +// get compiled and for code coverage analysis. +namespace Eigen { +template class Map>; +template class Map const>; +} // namespace Eigen + +namespace Sophus { + +template class Sim2; +template class Sim2; +#if SOPHUS_CERES +template class Sim2>; +#endif + +template +class Tests { + public: + using Sim2Type = Sim2; + using RxSO2Type = RxSO2; + using Point = typename Sim2::Point; + using Vector2Type = Vector2; + using Tangent = typename Sim2::Tangent; + Scalar const kPi = Constants::pi(); + + Tests() { + sim2_vec_.push_back( + Sim2Type(RxSO2Type::exp(Vector2Type(0.2, 1.)), Point(0, 0))); + sim2_vec_.push_back( + Sim2Type(RxSO2Type::exp(Vector2Type(0.2, 1.1)), Point(10, 0))); + sim2_vec_.push_back( + Sim2Type(RxSO2Type::exp(Vector2Type(0., 0.)), Point(0, 10))); + sim2_vec_.push_back( + Sim2Type(RxSO2Type::exp(Vector2Type(0.00001, 0.)), Point(0, 0))); + sim2_vec_.push_back( + Sim2Type(RxSO2Type::exp(Vector2Type(0.00001, 0.0000001)), + Point(1, -1.00000001))); + sim2_vec_.push_back( + Sim2Type(RxSO2Type::exp(Vector2Type(0., 0.)), Point(0.01, 0))); + sim2_vec_.push_back( + Sim2Type(RxSO2Type::exp(Vector2Type(kPi, 0.9)), Point(4, 0))); + sim2_vec_.push_back( + Sim2Type(RxSO2Type::exp(Vector2Type(0.2, 0)), Point(0, 0)) * + Sim2Type(RxSO2Type::exp(Vector2Type(kPi, 0)), Point(0, 0)) * + Sim2Type(RxSO2Type::exp(Vector2Type(-0.2, 0)), Point(0, 0))); + sim2_vec_.push_back( + Sim2Type(RxSO2Type::exp(Vector2Type(0.3, 0)), Point(2, -7)) * + Sim2Type(RxSO2Type::exp(Vector2Type(kPi, 0)), Point(0, 0)) * + Sim2Type(RxSO2Type::exp(Vector2Type(-0.3, 0)), Point(0, 6))); + Tangent tmp; + tmp << Scalar(0), Scalar(0), Scalar(0), Scalar(0); + tangent_vec_.push_back(tmp); + tmp << Scalar(1), Scalar(0), Scalar(0), Scalar(0); + tangent_vec_.push_back(tmp); + tmp << Scalar(0), Scalar(1), Scalar(0), Scalar(0.1); + tangent_vec_.push_back(tmp); + tmp << Scalar(-1), Scalar(1), Scalar(1), Scalar(-0.1); + tangent_vec_.push_back(tmp); + tmp << Scalar(20), Scalar(-1), Scalar(0), Scalar(-0.1); + tangent_vec_.push_back(tmp); + tmp << Scalar(30), Scalar(5), Scalar(-1), Scalar(1.5); + tangent_vec_.push_back(tmp); + + point_vec_.push_back(Point(Scalar(1), Scalar(4))); + point_vec_.push_back(Point(Scalar(1), Scalar(-3))); + } + + void runAll() { + bool passed = testLieProperties(); + passed &= testRawDataAcces(); + passed &= testConstructors(); + processTestResult(passed); + } + + private: + bool testLieProperties() { + LieGroupTests tests(sim2_vec_, tangent_vec_, point_vec_); + return tests.doAllTestsPass(); + } + + bool testRawDataAcces() { + bool passed = true; + Eigen::Matrix raw; + raw << Scalar(0), Scalar(1), Scalar(3), Scalar(2); + Eigen::Map map_of_const_sim2(raw.data()); + SOPHUS_TEST_APPROX(passed, map_of_const_sim2.complex().eval(), + raw.template head<2>().eval(), + Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, map_of_const_sim2.translation().eval(), + raw.template tail<2>().eval(), + Constants::epsilon()); + SOPHUS_TEST_EQUAL(passed, map_of_const_sim2.complex().data(), raw.data()); + SOPHUS_TEST_EQUAL(passed, map_of_const_sim2.translation().data(), + raw.data() + 2); + Eigen::Map const_shallow_copy = map_of_const_sim2; + SOPHUS_TEST_EQUAL(passed, const_shallow_copy.complex().eval(), + map_of_const_sim2.complex().eval()); + SOPHUS_TEST_EQUAL(passed, const_shallow_copy.translation().eval(), + map_of_const_sim2.translation().eval()); + + Eigen::Matrix raw2; + raw2 << Scalar(1), Scalar(0), Scalar(2), Scalar(1); + Eigen::Map map_of_sim2(raw.data()); + Vector2 z; + z = raw2.template head<2>(); + map_of_sim2.setComplex(z); + map_of_sim2.translation() = raw2.template tail<2>(); + SOPHUS_TEST_APPROX(passed, map_of_sim2.complex().eval(), + raw2.template head<2>().eval(), + Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, map_of_sim2.translation().eval(), + raw2.template tail<2>().eval(), + Constants::epsilon()); + SOPHUS_TEST_EQUAL(passed, map_of_sim2.complex().data(), raw.data()); + SOPHUS_TEST_EQUAL(passed, map_of_sim2.translation().data(), raw.data() + 2); + SOPHUS_TEST_NEQ(passed, map_of_sim2.complex().data(), z.data()); + Eigen::Map shallow_copy = map_of_sim2; + SOPHUS_TEST_EQUAL(passed, shallow_copy.complex().eval(), + map_of_sim2.complex().eval()); + SOPHUS_TEST_EQUAL(passed, shallow_copy.translation().eval(), + map_of_sim2.translation().eval()); + Eigen::Map const const_map_of_sim3 = map_of_sim2; + SOPHUS_TEST_EQUAL(passed, const_map_of_sim3.complex().eval(), + map_of_sim2.complex().eval()); + SOPHUS_TEST_EQUAL(passed, const_map_of_sim3.translation().eval(), + map_of_sim2.translation().eval()); + + Sim2Type const const_sim2(z, raw2.template tail<2>().eval()); + for (int i = 0; i < 4; ++i) { + SOPHUS_TEST_EQUAL(passed, const_sim2.data()[i], raw2.data()[i]); + } + + Sim2Type se3(z, raw2.template tail<2>().eval()); + for (int i = 0; i < 4; ++i) { + SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw2.data()[i]); + } + + for (int i = 0; i < 4; ++i) { + SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw.data()[i]); + } + + Eigen::Matrix data1, data2; + data1 << Scalar(0), Scalar(2), Scalar(1), Scalar(2); + data2 << Scalar(2), Scalar(0), Scalar(2), Scalar(1); + + Eigen::Map map1(data1.data()), map2(data2.data()); + + // map -> map assignment + map2 = map1; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), map2.matrix()); + + // map -> type assignment + Sim2Type copy; + copy = map1; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), copy.matrix()); + + // type -> map assignment + copy = Sim2Type(RxSO2Type::exp(Vector2Type(-1, 1)), + Point(Scalar(10), Scalar(0))); + map1 = copy; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), copy.matrix()); + + return passed; + } + + bool testConstructors() { + bool passed = true; + Eigen::Matrix I = Eigen::Matrix::Identity(); + SOPHUS_TEST_EQUAL(passed, Sim2Type().matrix(), I); + + Sim2Type sim2 = sim2_vec_.front(); + Point translation = sim2.translation(); + RxSO2Type rxso2 = sim2.rxso2(); + + SOPHUS_TEST_APPROX(passed, Sim2Type(rxso2, translation).matrix(), + sim2.matrix(), Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, Sim2Type(rxso2.complex(), translation).matrix(), + sim2.matrix(), Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, Sim2Type(sim2.matrix()).matrix(), sim2.matrix(), + Constants::epsilon()); + + Scalar scale(1.2); + sim2.setScale(scale); + SOPHUS_TEST_APPROX(passed, scale, sim2.scale(), + Constants::epsilon(), "setScale"); + + sim2.setComplex(sim2_vec_[0].rxso2().complex()); + SOPHUS_TEST_APPROX(passed, sim2_vec_[0].rxso2().complex(), + sim2_vec_[0].rxso2().complex(), + Constants::epsilon(), "setComplex"); + return passed; + } + + std::vector> sim2_vec_; + std::vector> tangent_vec_; + std::vector> point_vec_; +}; + +int test_sim3() { + using std::cerr; + using std::endl; + + cerr << "Test Sim2" << endl << endl; + cerr << "Double tests: " << endl; + Tests().runAll(); + cerr << "Float tests: " << endl; + Tests().runAll(); + +#if SOPHUS_CERES + cerr << "ceres::Jet tests: " << endl; + Tests>().runAll(); +#endif + + return 0; +} +} // namespace Sophus + +int main() { return Sophus::test_sim3(); } diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_sim3.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_sim3.cpp new file mode 100644 index 0000000..e4ec0b9 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_sim3.cpp @@ -0,0 +1,271 @@ +#include + +#include + +#include +#include "tests.hpp" + +// Explicit instantiate all class templates so that all member methods +// get compiled and for code coverage analysis. +namespace Eigen { +template class Map>; +template class Map const>; +} // namespace Eigen + +namespace Sophus { + +template class Sim3; +template class Sim3; +#if SOPHUS_CERES +template class Sim3>; +#endif + +template +class Tests { + public: + using Sim3Type = Sim3; + using RxSO3Type = RxSO3; + using Point = typename Sim3::Point; + using Vector4Type = Vector4; + using Tangent = typename Sim3::Tangent; + Scalar const kPi = Constants::pi(); + + Tests() { + sim3_vec_.push_back( + Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(0.2), Scalar(0.5), + Scalar(0.0), Scalar(1.))), + Point(Scalar(0), Scalar(0), Scalar(0)))); + sim3_vec_.push_back( + Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(0.2), Scalar(0.5), + Scalar(-1.0), Scalar(1.1))), + Point(Scalar(10), Scalar(0), Scalar(0)))); + sim3_vec_.push_back( + Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(0.), Scalar(0.), Scalar(0.), + Scalar(0.))), + Point(Scalar(0), Scalar(10), Scalar(5)))); + sim3_vec_.push_back( + Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(0.), Scalar(0.), Scalar(0.), + Scalar(1.1))), + Point(Scalar(0), Scalar(10), Scalar(5)))); + sim3_vec_.push_back( + Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(0.), Scalar(0.), + Scalar(0.00001), Scalar(0.))), + Point(Scalar(0), Scalar(0), Scalar(0)))); + sim3_vec_.push_back(Sim3Type( + RxSO3Type::exp(Vector4Type(Scalar(0.), Scalar(0.), Scalar(0.00001), + Scalar(0.0000001))), + Point(Scalar(1), Scalar(-1.00000001), Scalar(2.0000000001)))); + sim3_vec_.push_back( + Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(0.), Scalar(0.), + Scalar(0.00001), Scalar(0))), + Point(Scalar(0.01), Scalar(0), Scalar(0)))); + sim3_vec_.push_back(Sim3Type( + RxSO3Type::exp(Vector4Type(kPi, Scalar(0), Scalar(0), Scalar(0.9))), + Point(Scalar(4), Scalar(-5), Scalar(0)))); + sim3_vec_.push_back( + Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(0.2), Scalar(0.5), + Scalar(0.0), Scalar(0))), + Point(Scalar(0), Scalar(0), Scalar(0))) * + Sim3Type( + RxSO3Type::exp(Vector4Type(kPi, Scalar(0), Scalar(0), Scalar(0))), + Point(Scalar(0), Scalar(0), Scalar(0))) * + Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(-0.2), Scalar(-0.5), + Scalar(-0.0), Scalar(0))), + Point(Scalar(0), Scalar(0), Scalar(0)))); + sim3_vec_.push_back( + Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(0.3), Scalar(0.5), + Scalar(0.1), Scalar(0))), + Point(Scalar(2), Scalar(0), Scalar(-7))) * + Sim3Type( + RxSO3Type::exp(Vector4Type(kPi, Scalar(0), Scalar(0), Scalar(0))), + Point(Scalar(0), Scalar(0), Scalar(0))) * + Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(-0.3), Scalar(-0.5), + Scalar(-0.1), Scalar(0))), + Point(Scalar(0), Scalar(6), Scalar(0)))); + Tangent tmp; + tmp << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0); + tangent_vec_.push_back(tmp); + tmp << Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0); + tangent_vec_.push_back(tmp); + tmp << Scalar(0), Scalar(1), Scalar(0), Scalar(1), Scalar(0), Scalar(0), + Scalar(0.1); + tangent_vec_.push_back(tmp); + tmp << Scalar(0), Scalar(0), Scalar(1), Scalar(0), Scalar(1), Scalar(0), + Scalar(0.1); + tangent_vec_.push_back(tmp); + tmp << Scalar(-1), Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(1), + Scalar(-0.1); + tangent_vec_.push_back(tmp); + tmp << Scalar(20), Scalar(-1), Scalar(0), Scalar(-1), Scalar(1), Scalar(0), + Scalar(-0.1); + tangent_vec_.push_back(tmp); + tmp << Scalar(30), Scalar(5), Scalar(-1), Scalar(20), Scalar(-1), Scalar(0), + Scalar(1.5); + tangent_vec_.push_back(tmp); + + point_vec_.push_back(Point(Scalar(1), Scalar(2), Scalar(4))); + point_vec_.push_back(Point(Scalar(1), Scalar(-3), Scalar(0.5))); + } + + void runAll() { + bool passed = testLieProperties(); + passed &= testRawDataAcces(); + passed &= testConstructors(); + processTestResult(passed); + } + + private: + bool testLieProperties() { + LieGroupTests tests(sim3_vec_, tangent_vec_, point_vec_); + return tests.doAllTestsPass(); + } + + bool testRawDataAcces() { + bool passed = true; + Eigen::Matrix raw; + raw << Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1), Scalar(3), + Scalar(2); + Eigen::Map map_of_const_sim3(raw.data()); + SOPHUS_TEST_APPROX(passed, map_of_const_sim3.quaternion().coeffs().eval(), + raw.template head<4>().eval(), + Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, map_of_const_sim3.translation().eval(), + raw.template tail<3>().eval(), + Constants::epsilon()); + SOPHUS_TEST_EQUAL(passed, map_of_const_sim3.quaternion().coeffs().data(), + raw.data()); + SOPHUS_TEST_EQUAL(passed, map_of_const_sim3.translation().data(), + raw.data() + 4); + Eigen::Map const_shallow_copy = map_of_const_sim3; + SOPHUS_TEST_EQUAL(passed, const_shallow_copy.quaternion().coeffs().eval(), + map_of_const_sim3.quaternion().coeffs().eval()); + SOPHUS_TEST_EQUAL(passed, const_shallow_copy.translation().eval(), + map_of_const_sim3.translation().eval()); + + Eigen::Matrix raw2; + raw2 << Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(3), Scalar(2), + Scalar(1); + Eigen::Map map_of_sim3(raw.data()); + Eigen::Quaternion quat; + quat.coeffs() = raw2.template head<4>(); + map_of_sim3.setQuaternion(quat); + map_of_sim3.translation() = raw2.template tail<3>(); + SOPHUS_TEST_APPROX(passed, map_of_sim3.quaternion().coeffs().eval(), + raw2.template head<4>().eval(), + Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, map_of_sim3.translation().eval(), + raw2.template tail<3>().eval(), + Constants::epsilon()); + SOPHUS_TEST_EQUAL(passed, map_of_sim3.quaternion().coeffs().data(), + raw.data()); + SOPHUS_TEST_EQUAL(passed, map_of_sim3.translation().data(), raw.data() + 4); + SOPHUS_TEST_NEQ(passed, map_of_sim3.quaternion().coeffs().data(), + quat.coeffs().data()); + Eigen::Map shallow_copy = map_of_sim3; + SOPHUS_TEST_EQUAL(passed, shallow_copy.quaternion().coeffs().eval(), + map_of_sim3.quaternion().coeffs().eval()); + SOPHUS_TEST_EQUAL(passed, shallow_copy.translation().eval(), + map_of_sim3.translation().eval()); + Eigen::Map const const_map_of_sim3 = map_of_sim3; + SOPHUS_TEST_EQUAL(passed, const_map_of_sim3.quaternion().coeffs().eval(), + map_of_sim3.quaternion().coeffs().eval()); + SOPHUS_TEST_EQUAL(passed, const_map_of_sim3.translation().eval(), + map_of_sim3.translation().eval()); + + Sim3Type const const_sim3(quat, raw2.template tail<3>().eval()); + for (int i = 0; i < 7; ++i) { + SOPHUS_TEST_EQUAL(passed, const_sim3.data()[i], raw2.data()[i]); + } + + Sim3Type se3(quat, raw2.template tail<3>().eval()); + for (int i = 0; i < 7; ++i) { + SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw2.data()[i]); + } + + for (int i = 0; i < 7; ++i) { + SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw.data()[i]); + } + + Eigen::Matrix data1, data2; + data1 << Scalar(0), Scalar(2), Scalar(0), Scalar(0), + Scalar(1), Scalar(2), Scalar(3); + data2 << Scalar(0), Scalar(0), Scalar(2), Scalar(0), + Scalar(3), Scalar(2), Scalar(1); + + Eigen::Map map1(data1.data()), map2(data2.data()); + + // map -> map assignment + map2 = map1; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), map2.matrix()); + + // map -> type assignment + Sim3Type copy; + copy = map1; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), copy.matrix()); + + // type -> map assignment + copy = Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(0.2), Scalar(0.5), + Scalar(-1.0), Scalar(1.1))), + Point(Scalar(10), Scalar(0), Scalar(0))); + map1 = copy; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), copy.matrix()); + + return passed; + } + + bool testConstructors() { + bool passed = true; + Eigen::Matrix I = Eigen::Matrix::Identity(); + SOPHUS_TEST_EQUAL(passed, Sim3Type().matrix(), I); + + Sim3Type sim3 = sim3_vec_.front(); + Point translation = sim3.translation(); + RxSO3Type rxso3 = sim3.rxso3(); + + SOPHUS_TEST_APPROX(passed, Sim3Type(rxso3, translation).matrix(), + sim3.matrix(), Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, + Sim3Type(rxso3.quaternion(), translation).matrix(), + sim3.matrix(), Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, Sim3Type(sim3.matrix()).matrix(), sim3.matrix(), + Constants::epsilon()); + + Scalar scale(1.2); + sim3.setScale(scale); + SOPHUS_TEST_APPROX(passed, scale, sim3.scale(), + Constants::epsilon(), "setScale"); + + sim3.setQuaternion(sim3_vec_[0].rxso3().quaternion()); + SOPHUS_TEST_APPROX(passed, sim3_vec_[0].rxso3().quaternion().coeffs(), + sim3_vec_[0].rxso3().quaternion().coeffs(), + Constants::epsilon(), "setQuaternion"); + return passed; + } + + std::vector> sim3_vec_; + std::vector> tangent_vec_; + std::vector> point_vec_; +}; + +int test_sim3() { + using std::cerr; + using std::endl; + + cerr << "Test Sim3" << endl << endl; + cerr << "Double tests: " << endl; + Tests().runAll(); + cerr << "Float tests: " << endl; + Tests().runAll(); + +#if SOPHUS_CERES + cerr << "ceres::Jet tests: " << endl; + Tests>().runAll(); +#endif + + return 0; +} +} // namespace Sophus + +int main() { return Sophus::test_sim3(); } diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_so2.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_so2.cpp new file mode 100644 index 0000000..e581dce --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_so2.cpp @@ -0,0 +1,190 @@ +#include + +#include +#include "tests.hpp" + +// Explicit instantiate all class templates so that all member methods +// get compiled and for code coverage analysis. +namespace Eigen { +template class Map>; +template class Map const>; +} // namespace Eigen + +namespace Sophus { + +template class SO2; +template class SO2; +#if SOPHUS_CERES +template class SO2>; +#endif + +template +class Tests { + public: + using SO2Type = SO2; + using Point = typename SO2::Point; + using Tangent = typename SO2::Tangent; + Scalar const kPi = Constants::pi(); + + Tests() { + so2_vec_.push_back(SO2Type::exp(Scalar(0.0))); + so2_vec_.push_back(SO2Type::exp(Scalar(0.2))); + so2_vec_.push_back(SO2Type::exp(Scalar(10.))); + so2_vec_.push_back(SO2Type::exp(Scalar(0.00001))); + so2_vec_.push_back(SO2Type::exp(kPi)); + so2_vec_.push_back(SO2Type::exp(Scalar(0.2)) * SO2Type::exp(kPi) * + SO2Type::exp(Scalar(-0.2))); + so2_vec_.push_back(SO2Type::exp(Scalar(-0.3)) * SO2Type::exp(kPi) * + SO2Type::exp(Scalar(0.3))); + + tangent_vec_.push_back(Tangent(Scalar(0))); + tangent_vec_.push_back(Tangent(Scalar(1))); + tangent_vec_.push_back(Tangent(Scalar(kPi / 2.))); + tangent_vec_.push_back(Tangent(Scalar(-1))); + tangent_vec_.push_back(Tangent(Scalar(20))); + tangent_vec_.push_back(Tangent(Scalar(kPi / 2. + 0.0001))); + + point_vec_.push_back(Point(Scalar(1), Scalar(2))); + point_vec_.push_back(Point(Scalar(1), Scalar(-3))); + } + + void runAll() { + bool passed = testLieProperties(); + passed &= testUnity(); + passed &= testRawDataAcces(); + passed &= testConstructors(); + passed &= testFit(); + processTestResult(passed); + } + + private: + bool testLieProperties() { + LieGroupTests tests(so2_vec_, tangent_vec_, point_vec_); + return tests.doAllTestsPass(); + } + + bool testUnity() { + bool passed = true; + // Test that the complex number magnitude stays close to one. + SO2Type current_q; + for (std::size_t i = 0; i < 1000; ++i) { + for (SO2Type const& q : so2_vec_) { + current_q *= q; + } + } + SOPHUS_TEST_APPROX(passed, current_q.unit_complex().norm(), Scalar(1), + Constants::epsilon(), "Magnitude drift"); + return passed; + } + + bool testRawDataAcces() { + bool passed = true; + Vector2 raw = {0, 1}; + Eigen::Map map_of_const_so2(raw.data()); + SOPHUS_TEST_APPROX(passed, map_of_const_so2.unit_complex().eval(), raw, + Constants::epsilon()); + SOPHUS_TEST_EQUAL(passed, map_of_const_so2.unit_complex().data(), + raw.data()); + Eigen::Map const_shallow_copy = map_of_const_so2; + SOPHUS_TEST_EQUAL(passed, const_shallow_copy.unit_complex().eval(), + map_of_const_so2.unit_complex().eval()); + + Vector2 raw2 = {1, 0}; + Eigen::Map map_of_so2(raw.data()); + map_of_so2.setComplex(raw2); + SOPHUS_TEST_APPROX(passed, map_of_so2.unit_complex().eval(), raw2, + Constants::epsilon()); + SOPHUS_TEST_EQUAL(passed, map_of_so2.unit_complex().data(), raw.data()); + SOPHUS_TEST_NEQ(passed, map_of_so2.unit_complex().data(), raw2.data()); + Eigen::Map shallow_copy = map_of_so2; + SOPHUS_TEST_EQUAL(passed, shallow_copy.unit_complex().eval(), + map_of_so2.unit_complex().eval()); + + SO2Type const const_so2(raw2); + for (int i = 0; i < 2; ++i) { + SOPHUS_TEST_EQUAL(passed, const_so2.data()[i], raw2.data()[i]); + } + + SO2Type so2(raw2); + for (int i = 0; i < 2; ++i) { + so2.data()[i] = raw[i]; + } + + for (int i = 0; i < 2; ++i) { + SOPHUS_TEST_EQUAL(passed, so2.data()[i], raw.data()[i]); + } + + Vector2 data1 = {1, 0}, data2 = {0, 1}; + Eigen::Map map1(data1.data()), map2(data2.data()); + + // map -> map assignment + map2 = map1; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), map2.matrix()); + + // map -> type assignment + SO2Type copy; + copy = map1; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), copy.matrix()); + + // type -> map assignment + copy = SO2Type(Scalar(0.5)); + map1 = copy; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), copy.matrix()); + + return passed; + } + + bool testConstructors() { + bool passed = true; + Matrix2 R = so2_vec_.front().matrix(); + SO2Type so2(R); + SOPHUS_TEST_APPROX(passed, R, so2.matrix(), Constants::epsilon()); + + return passed; + } + + template + enable_if_t::value, bool> testFit() { + bool passed = true; + + for (int i = 0; i < 100; ++i) { + Matrix2 R = Matrix2::Random(); + SO2Type so2 = SO2Type::fitToSO2(R); + SO2Type so2_2 = SO2Type::fitToSO2(so2.matrix()); + + SOPHUS_TEST_APPROX(passed, so2.matrix(), so2_2.matrix(), + Constants::epsilon()); + } + return passed; + } + + template + enable_if_t::value, bool> testFit() { + return true; + } + + std::vector> so2_vec_; + std::vector> tangent_vec_; + std::vector> point_vec_; +}; + +int test_so2() { + using std::cerr; + using std::endl; + + cerr << "Test SO2" << endl << endl; + cerr << "Double tests: " << endl; + Tests().runAll(); + cerr << "Float tests: " << endl; + Tests().runAll(); + +#if SOPHUS_CERES + cerr << "ceres::Jet tests: " << endl; + Tests>().runAll(); +#endif + + return 0; +} +} // namespace Sophus + +int main() { return Sophus::test_so2(); } diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_so3.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_so3.cpp new file mode 100644 index 0000000..5c2af4a --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_so3.cpp @@ -0,0 +1,287 @@ +#include + +#include +#include +#include "tests.hpp" + +// Explicit instantiate all class templates so that all member methods +// get compiled and for code coverage analysis. +namespace Eigen { +template class Map>; +template class Map const>; +} // namespace Eigen + +namespace Sophus { + +template class SO3; +template class SO3; +#if SOPHUS_CERES +template class SO3>; +#endif + +template +class Tests { + public: + using SO3Type = SO3; + using Point = typename SO3::Point; + using Tangent = typename SO3::Tangent; + Scalar const kPi = Constants::pi(); + + Tests() { + so3_vec_.push_back(SO3Type(Eigen::Quaternion( + Scalar(0.1e-11), Scalar(0.), Scalar(1.), Scalar(0.)))); + so3_vec_.push_back(SO3Type(Eigen::Quaternion( + Scalar(-1), Scalar(0.00001), Scalar(0.0), Scalar(0.0)))); + so3_vec_.push_back( + SO3Type::exp(Point(Scalar(0.2), Scalar(0.5), Scalar(0.0)))); + so3_vec_.push_back( + SO3Type::exp(Point(Scalar(0.2), Scalar(0.5), Scalar(-1.0)))); + so3_vec_.push_back(SO3Type::exp(Point(Scalar(0.), Scalar(0.), Scalar(0.)))); + so3_vec_.push_back( + SO3Type::exp(Point(Scalar(0.), Scalar(0.), Scalar(0.00001)))); + so3_vec_.push_back(SO3Type::exp(Point(kPi, Scalar(0), Scalar(0)))); + so3_vec_.push_back( + SO3Type::exp(Point(Scalar(0.2), Scalar(0.5), Scalar(0.0))) * + SO3Type::exp(Point(kPi, Scalar(0), Scalar(0))) * + SO3Type::exp(Point(Scalar(-0.2), Scalar(-0.5), Scalar(-0.0)))); + so3_vec_.push_back( + SO3Type::exp(Point(Scalar(0.3), Scalar(0.5), Scalar(0.1))) * + SO3Type::exp(Point(kPi, Scalar(0), Scalar(0))) * + SO3Type::exp(Point(Scalar(-0.3), Scalar(-0.5), Scalar(-0.1)))); + tangent_vec_.push_back(Tangent(Scalar(0), Scalar(0), Scalar(0))); + tangent_vec_.push_back(Tangent(Scalar(1), Scalar(0), Scalar(0))); + tangent_vec_.push_back(Tangent(Scalar(0), Scalar(1), Scalar(0))); + tangent_vec_.push_back( + Tangent(Scalar(kPi / 2.), Scalar(kPi / 2.), Scalar(0))); + tangent_vec_.push_back(Tangent(Scalar(-1), Scalar(1), Scalar(0))); + tangent_vec_.push_back(Tangent(Scalar(20), Scalar(-1), Scalar(0))); + tangent_vec_.push_back(Tangent(Scalar(30), Scalar(5), Scalar(-1))); + + point_vec_.push_back(Point(Scalar(1), Scalar(2), Scalar(4))); + point_vec_.push_back(Point(Scalar(1), Scalar(-3), Scalar(0.5))); + } + + void runAll() { + bool passed = testLieProperties(); + passed &= testUnity(); + passed &= testRawDataAcces(); + passed &= testConstructors(); + passed &= testSampleUniformSymmetry(); + passed &= testFit(); + processTestResult(passed); + } + + private: + bool testLieProperties() { + LieGroupTests tests(so3_vec_, tangent_vec_, point_vec_); + return tests.doAllTestsPass(); + } + + bool testUnity() { + bool passed = true; + // Test that the complex number magnitude stays close to one. + SO3Type current_q; + for (size_t i = 0; i < 1000; ++i) { + for (SO3Type const& q : so3_vec_) { + current_q *= q; + } + } + SOPHUS_TEST_APPROX(passed, current_q.unit_quaternion().norm(), Scalar(1), + Constants::epsilon(), "Magnitude drift"); + return passed; + } + + bool testRawDataAcces() { + bool passed = true; + Eigen::Matrix raw = {Scalar(0), Scalar(1), Scalar(0), + Scalar(0)}; + Eigen::Map map_of_const_so3(raw.data()); + SOPHUS_TEST_APPROX(passed, + map_of_const_so3.unit_quaternion().coeffs().eval(), raw, + Constants::epsilon()); + SOPHUS_TEST_EQUAL( + passed, map_of_const_so3.unit_quaternion().coeffs().data(), raw.data()); + Eigen::Map const_shallow_copy = map_of_const_so3; + SOPHUS_TEST_EQUAL(passed, + const_shallow_copy.unit_quaternion().coeffs().eval(), + map_of_const_so3.unit_quaternion().coeffs().eval()); + + Eigen::Matrix raw2 = {Scalar(1), Scalar(0), Scalar(0), + Scalar(0)}; + Eigen::Map map_of_so3(raw.data()); + Eigen::Quaternion quat; + quat.coeffs() = raw2; + map_of_so3.setQuaternion(quat); + SOPHUS_TEST_APPROX(passed, map_of_so3.unit_quaternion().coeffs().eval(), + raw2, Constants::epsilon()); + SOPHUS_TEST_EQUAL(passed, map_of_so3.unit_quaternion().coeffs().data(), + raw.data()); + SOPHUS_TEST_NEQ(passed, map_of_so3.unit_quaternion().coeffs().data(), + quat.coeffs().data()); + Eigen::Map shallow_copy = map_of_so3; + SOPHUS_TEST_EQUAL(passed, shallow_copy.unit_quaternion().coeffs().eval(), + map_of_so3.unit_quaternion().coeffs().eval()); + + SO3Type const const_so3(quat); + for (int i = 0; i < 4; ++i) { + SOPHUS_TEST_EQUAL(passed, const_so3.data()[i], raw2.data()[i]); + } + + SO3Type so3(quat); + for (int i = 0; i < 4; ++i) { + so3.data()[i] = raw[i]; + } + + for (int i = 0; i < 4; ++i) { + SOPHUS_TEST_EQUAL(passed, so3.data()[i], raw.data()[i]); + } + + SOPHUS_TEST_EQUAL( + passed, SO3Type::rotX(Scalar(0.2)).matrix(), + SO3Type::exp(Point(Scalar(0.2), Scalar(0), Scalar(0))).matrix()); + SOPHUS_TEST_EQUAL( + passed, SO3Type::rotY(Scalar(-0.2)).matrix(), + SO3Type::exp(Point(Scalar(0), Scalar(-0.2), Scalar(0))).matrix()); + SOPHUS_TEST_EQUAL( + passed, SO3Type::rotZ(Scalar(1.1)).matrix(), + SO3Type::exp(Point(Scalar(0), Scalar(0), Scalar(1.1))).matrix()); + + Vector4 data1(Scalar{1}, Scalar{0}, Scalar{0}, Scalar{0}); + Vector4 data2(Scalar{0}, Scalar{1}, Scalar{0}, Scalar{0}); + Eigen::Map map1(data1.data()), map2(data2.data()); + + // map -> map assignment + map2 = map1; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), map2.matrix()); + + // map -> type assignment + SO3Type copy; + copy = map1; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), copy.matrix()); + + // type -> map assignment + copy = SO3Type::rotZ(Scalar(0.5)); + map1 = copy; + SOPHUS_TEST_EQUAL(passed, map1.matrix(), copy.matrix()); + + return passed; + } + + bool testConstructors() { + bool passed = true; + Matrix3 R = so3_vec_.front().matrix(); + SO3Type so3(R); + SOPHUS_TEST_APPROX(passed, R, so3.matrix(), Constants::epsilon()); + + return passed; + } + + template + enable_if_t::value, bool> testSampleUniformSymmetry() { + bool passed = true; + std::default_random_engine generator(0); + + // A non-rigorous test for checking that our sampleUniform() function is + // giving us symmetric results + // + // We (a) split the output space in half, (b) apply a series of random + // rotations to a point, (c) check which half of the output space each + // transformed point ends up, and then (d) apply a standard "coin toss" + // chi-square test + + for (size_t trial = 0; trial < 5; trial++) { + std::normal_distribution normal(0, 10); + + // Pick a random plane to split the output space by + Point plane_normal(normal(generator), normal(generator), + normal(generator)); + plane_normal /= plane_normal.norm(); + + // Pick a random point to be rotated + Point input_point(normal(generator), normal(generator), + normal(generator)); + input_point /= input_point.norm(); + + // Randomly rotate points and track # that land on each side of plane + size_t positive_count = 0; + size_t negative_count = 0; + size_t samples = 5000; + for (size_t i = 0; i < samples; ++i) { + SO3Type R = SO3Type::sampleUniform(generator); + if (plane_normal.dot(R * input_point) > 0) + positive_count++; + else + negative_count++; + } + + // Chi-square computation, compare against critical value (p=0.01) + double expected_count = static_cast(samples) / 2.0; + double chi_square = + pow(positive_count - expected_count, 2.0) / expected_count + + pow(negative_count - expected_count, 2.0) / expected_count; + SOPHUS_TEST(passed, chi_square < 6.635); + } + + return passed; + } + + template + enable_if_t::value, bool> testSampleUniformSymmetry() { + return true; + } + + template + enable_if_t::value, bool> testFit() { + bool passed = true; + + for (int i = 0; i < 100; ++i) { + Matrix3 R = Matrix3::Random(); + SO3Type so3 = SO3Type::fitToSO3(R); + SO3Type so3_2 = SO3Type::fitToSO3(so3.matrix()); + + SOPHUS_TEST_APPROX(passed, so3.matrix(), so3_2.matrix(), + Constants::epsilon()); + } + + for (Scalar const angle : + {Scalar(0.0), Scalar(0.1), Scalar(0.3), Scalar(-0.7)}) { + SOPHUS_TEST_APPROX(passed, SO3Type::rotX(angle).angleX(), angle, + Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, SO3Type::rotY(angle).angleY(), angle, + Constants::epsilon()); + SOPHUS_TEST_APPROX(passed, SO3Type::rotZ(angle).angleZ(), angle, + Constants::epsilon()); + } + return passed; + } + + template + enable_if_t::value, bool> testFit() { + return true; + } + + std::vector> so3_vec_; + std::vector> tangent_vec_; + std::vector> point_vec_; +}; + +int test_so3() { + using std::cerr; + using std::endl; + + cerr << "Test SO3" << endl << endl; + cerr << "Double tests: " << endl; + Tests().runAll(); + cerr << "Float tests: " << endl; + Tests().runAll(); + +#if SOPHUS_CERES + cerr << "ceres::Jet tests: " << endl; + Tests>().runAll(); +#endif + + return 0; +} +} // namespace Sophus + +int main() { return Sophus::test_so3(); } diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_velocities.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_velocities.cpp new file mode 100644 index 0000000..aa7befc --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/test_velocities.cpp @@ -0,0 +1,130 @@ +#include + +#include "tests.hpp" + +#include + +namespace Sophus { +namespace experimental { + +template +bool tests_linear_velocities() { + bool passed = true; + std::vector, Eigen::aligned_allocator>> bar_Ts_baz; + + for (size_t i = 0; i < 10; ++i) { + bar_Ts_baz.push_back(SE3::rotX(i * 0.001) * + SE3::rotY(i * 0.001) * + SE3::transX(0.01 * i)); + } + + SE3 foo_T_bar = + SE3::rotX(0.5) * SE3::rotZ(0.2) * SE3::transY(2); + + std::vector, Eigen::aligned_allocator>> foo_Ts_baz; + for (auto const& bar_T_baz : bar_Ts_baz) { + foo_Ts_baz.push_back(foo_T_bar * bar_T_baz); + } + + auto gen_linear_vels = + [](std::vector, Eigen::aligned_allocator>> const& + a_Ts_b) { + std::vector, Eigen::aligned_allocator>> + linearVels_a; + for (size_t i = 0; i < a_Ts_b.size() - 1; ++i) { + linearVels_a.push_back(a_Ts_b[i + 1].translation() - + a_Ts_b[i].translation()); + } + return linearVels_a; + }; + + // linear velocities in frame bar + std::vector, Eigen::aligned_allocator>> + linearVels_bar = gen_linear_vels(bar_Ts_baz); + // linear velocities in frame foo + std::vector, Eigen::aligned_allocator>> + linearVels_foo = gen_linear_vels(foo_Ts_baz); + + for (size_t i = 0; i < linearVels_bar.size(); ++i) { + SOPHUS_TEST_APPROX(passed, linearVels_foo[i], + transformVelocity(foo_T_bar, linearVels_bar[i]), + sqrt(Constants::epsilon())); + } + return passed; +} + +template +bool tests_rotational_velocities() { + bool passed = true; + + SE3 foo_T_bar = + SE3::rotX(0.5) * SE3::rotZ(0.2) * SE3::transY(2); + + // One parameter subgroup of SE3, motion through space given time t. + auto bar_T_baz = [](Scalar t) -> SE3 { + return SE3::rotX(t * Scalar(0.01)) * + SE3::rotY(t * Scalar(0.0001)) * + SE3::transX(t * Scalar(0.0001)); + }; + + std::vector ts = {Scalar(0), Scalar(0.3), Scalar(1)}; + + Scalar h = Constants::epsilon(); + for (Scalar t : ts) { + // finite difference approximiation of instantanious velocity in frame bar + Vector3 rotVel_in_frame_bar = + finiteDifferenceRotationalVelocity(bar_T_baz, t, h); + + // finite difference approximiation of instantanious velocity in frame foo + Vector3 rotVel_in_frame_foo = + finiteDifferenceRotationalVelocity( + [&foo_T_bar, bar_T_baz](Scalar t) -> SE3 { + return foo_T_bar * bar_T_baz(t); + }, + t, h); + + Vector3 rotVel_in_frame_bar2 = + transformVelocity(foo_T_bar.inverse(), rotVel_in_frame_foo); + SOPHUS_TEST_APPROX( + passed, rotVel_in_frame_bar, rotVel_in_frame_bar2, + // not too tight threshold, because of finit difference approximation + std::sqrt(Constants::epsilon())); + + // The rotational velocities rotVel_in_frame_foo and rotVel_in_frame_bar + // should not be equal since they are in different frames (foo != bar). + SOPHUS_TEST_NOT_APPROX(passed, rotVel_in_frame_foo, rotVel_in_frame_bar, + Scalar(1e-3)); + + // Expect same result when using adjoint instead since: + // vee(bar_R_foo * hat(vel_foo) * bar_R_foo^T = bar_R_foo 8 vel_foo. + SOPHUS_TEST_APPROX( + passed, transformVelocity(foo_T_bar.inverse(), rotVel_in_frame_foo), + SO3::vee(foo_T_bar.so3().inverse().matrix() * + SO3::hat(rotVel_in_frame_foo) * + foo_T_bar.so3().matrix()), + Constants::epsilon()); + } + return passed; +} + +int test_velocities() { + using std::cerr; + using std::endl; + + cerr << "Test Velocities" << endl << endl; + cerr << "Double tests: " << endl; + bool passed = tests_linear_velocities(); + passed &= tests_rotational_velocities(); + processTestResult(passed); + + cerr << "Float tests: " << endl; + passed = tests_linear_velocities(); + passed &= tests_rotational_velocities(); + processTestResult(passed); + + return 0; +} +} // namespace experimental +} // namespace Sophus + +int main() { return Sophus::experimental::test_velocities(); } diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/tests.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/tests.hpp new file mode 100644 index 0000000..1dfbdaf --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/Sophus/test/core/tests.hpp @@ -0,0 +1,562 @@ +#ifndef SOPUHS_TESTS_HPP +#define SOPUHS_TESTS_HPP + +#include + +#include +#include + +#include +#include +#include +#include + +#ifdef SOPHUS_CERES +#include +#endif + +namespace Sophus { + +template +class LieGroupTests { + public: + using LieGroup = LieGroup_; + using Scalar = typename LieGroup::Scalar; + using Transformation = typename LieGroup::Transformation; + using Tangent = typename LieGroup::Tangent; + using Point = typename LieGroup::Point; + using HomogeneousPoint = typename LieGroup::HomogeneousPoint; + using ConstPointMap = Eigen::Map; + using Line = typename LieGroup::Line; + using Adjoint = typename LieGroup::Adjoint; + static int constexpr N = LieGroup::N; + static int constexpr DoF = LieGroup::DoF; + static int constexpr num_parameters = LieGroup::num_parameters; + + LieGroupTests( + std::vector> const& + group_vec, + std::vector> const& + tangent_vec, + std::vector> const& point_vec) + : group_vec_(group_vec), + tangent_vec_(tangent_vec), + point_vec_(point_vec) {} + + bool adjointTest() { + bool passed = true; + for (size_t i = 0; i < group_vec_.size(); ++i) { + Transformation T = group_vec_[i].matrix(); + Adjoint Ad = group_vec_[i].Adj(); + for (size_t j = 0; j < tangent_vec_.size(); ++j) { + Tangent x = tangent_vec_[j]; + + Transformation I; + I.setIdentity(); + Tangent ad1 = Ad * x; + Tangent ad2 = LieGroup::vee(T * LieGroup::hat(x) * + group_vec_[i].inverse().matrix()); + SOPHUS_TEST_APPROX(passed, ad1, ad2, Scalar(10) * kSmallEps, + "Adjoint case %, %", i, j); + } + } + return passed; + } + + bool contructorAndAssignmentTest() { + bool passed = true; + for (LieGroup foo_T_bar : group_vec_) { + LieGroup foo_T2_bar = foo_T_bar; + SOPHUS_TEST_APPROX(passed, foo_T_bar.matrix(), foo_T2_bar.matrix(), + kSmallEps, "Copy constructor: %\nvs\n %", + transpose(foo_T_bar.matrix()), + transpose(foo_T2_bar.matrix())); + LieGroup foo_T3_bar; + foo_T3_bar = foo_T_bar; + SOPHUS_TEST_APPROX(passed, foo_T_bar.matrix(), foo_T3_bar.matrix(), + kSmallEps, "Copy assignment: %\nvs\n %", + transpose(foo_T_bar.matrix()), + transpose(foo_T3_bar.matrix())); + + LieGroup foo_T4_bar(foo_T_bar.matrix()); + SOPHUS_TEST_APPROX( + passed, foo_T_bar.matrix(), foo_T4_bar.matrix(), kSmallEps, + "Constructor from homogeneous matrix: %\nvs\n %", + transpose(foo_T_bar.matrix()), transpose(foo_T4_bar.matrix())); + + Eigen::Map foo_Tmap_bar(foo_T_bar.data()); + LieGroup foo_T5_bar = foo_Tmap_bar; + SOPHUS_TEST_APPROX( + passed, foo_T_bar.matrix(), foo_T5_bar.matrix(), kSmallEps, + "Assignment from Eigen::Map type: %\nvs\n %", + transpose(foo_T_bar.matrix()), transpose(foo_T5_bar.matrix())); + + Eigen::Map foo_Tcmap_bar(foo_T_bar.data()); + LieGroup foo_T6_bar; + foo_T6_bar = foo_Tcmap_bar; + SOPHUS_TEST_APPROX( + passed, foo_T_bar.matrix(), foo_T5_bar.matrix(), kSmallEps, + "Assignment from Eigen::Map type: %\nvs\n %", + transpose(foo_T_bar.matrix()), transpose(foo_T5_bar.matrix())); + + LieGroup I; + Eigen::Map foo_Tmap2_bar(I.data()); + foo_Tmap2_bar = foo_T_bar; + SOPHUS_TEST_APPROX(passed, foo_Tmap2_bar.matrix(), foo_T_bar.matrix(), + kSmallEps, "Assignment to Eigen::Map type: %\nvs\n %", + transpose(foo_Tmap2_bar.matrix()), + transpose(foo_T_bar.matrix())); + } + return passed; + } + + bool derivativeTest() { + bool passed = true; + + LieGroup g; + for (int i = 0; i < DoF; ++i) { + Transformation Gi = g.Dxi_exp_x_matrix_at_0(i); + Transformation Gi2 = curveNumDiff( + [i](Scalar xi) -> Transformation { + Tangent x; + setToZero(x); + setElementAt(x, xi, i); + return LieGroup::exp(x).matrix(); + }, + Scalar(0)); + SOPHUS_TEST_APPROX(passed, Gi, Gi2, kSmallEpsSqrt, + "Dxi_exp_x_matrix_at_ case %", i); + } + + return passed; + } + + template + enable_if_t>::value || + std::is_same>::value || + std::is_same>::value || + std::is_same>::value, + bool> + additionalDerivativeTest() { + bool passed = true; + for (size_t j = 0; j < tangent_vec_.size(); ++j) { + Tangent a = tangent_vec_[j]; + Eigen::Matrix J = LieGroup::Dx_exp_x(a); + Eigen::Matrix J_num = + vectorFieldNumDiff( + [](Tangent const& x) -> Sophus::Vector { + return LieGroup::exp(x).params(); + }, + a); + + SOPHUS_TEST_APPROX(passed, J, J_num, 3 * kSmallEpsSqrt, + "Dx_exp_x case: %", j); + } + + Tangent o; + setToZero(o); + Eigen::Matrix J = LieGroup::Dx_exp_x_at_0(); + Eigen::Matrix J_num = + vectorFieldNumDiff( + [](Tangent const& x) -> Sophus::Vector { + return LieGroup::exp(x).params(); + }, + o); + SOPHUS_TEST_APPROX(passed, J, J_num, kSmallEpsSqrt, "Dx_exp_x_at_0"); + + for (size_t i = 0; i < group_vec_.size(); ++i) { + LieGroup T = group_vec_[i]; + + Eigen::Matrix J = T.Dx_this_mul_exp_x_at_0(); + Eigen::Matrix J_num = + vectorFieldNumDiff( + [T](Tangent const& x) -> Sophus::Vector { + return (T * LieGroup::exp(x)).params(); + }, + o); + + SOPHUS_TEST_APPROX(passed, J, J_num, kSmallEpsSqrt, + "Dx_this_mul_exp_x_at_0 case: %", i); + } + + return passed; + } + + template + enable_if_t>::value && + !std::is_same>::value && + !std::is_same>::value && + !std::is_same>::value, + bool> + additionalDerivativeTest() { + return true; + } + + bool productTest() { + bool passed = true; + + for (size_t i = 0; i < group_vec_.size() - 1; ++i) { + LieGroup T1 = group_vec_[i]; + LieGroup T2 = group_vec_[i + 1]; + LieGroup mult = T1 * T2; + T1 *= T2; + SOPHUS_TEST_APPROX(passed, T1.matrix(), mult.matrix(), kSmallEps, + "Product case: %", i); + } + return passed; + } + + bool expLogTest() { + bool passed = true; + + for (size_t i = 0; i < group_vec_.size(); ++i) { + Transformation T1 = group_vec_[i].matrix(); + Transformation T2 = LieGroup::exp(group_vec_[i].log()).matrix(); + SOPHUS_TEST_APPROX(passed, T1, T2, kSmallEps, "G - exp(log(G)) case: %", + i); + } + return passed; + } + + bool expMapTest() { + bool passed = true; + for (size_t i = 0; i < tangent_vec_.size(); ++i) { + Tangent omega = tangent_vec_[i]; + Transformation exp_x = LieGroup::exp(omega).matrix(); + Transformation expmap_hat_x = (LieGroup::hat(omega)).exp(); + SOPHUS_TEST_APPROX(passed, exp_x, expmap_hat_x, Scalar(10) * kSmallEps, + "expmap(hat(x)) - exp(x) case: %", i); + } + return passed; + } + + bool groupActionTest() { + bool passed = true; + + for (size_t i = 0; i < group_vec_.size(); ++i) { + for (size_t j = 0; j < point_vec_.size(); ++j) { + Point const& p = point_vec_[j]; + Point point1 = group_vec_[i] * p; + + HomogeneousPoint hp = p.homogeneous(); + HomogeneousPoint hpoint1 = group_vec_[i] * hp; + + ConstPointMap p_map(p.data()); + Point pointmap1 = group_vec_[i] * p_map; + + Transformation T = group_vec_[i].matrix(); + Point gt_point1 = map(T, p); + + SOPHUS_TEST_APPROX(passed, point1, gt_point1, kSmallEps, + "Transform point case: %", i); + SOPHUS_TEST_APPROX(passed, hpoint1.hnormalized().eval(), gt_point1, + kSmallEps, "Transform homogeneous point case: %", i); + SOPHUS_TEST_APPROX(passed, pointmap1, gt_point1, kSmallEps, + "Transform map point case: %", i); + } + } + return passed; + } + + bool lineActionTest() { + bool passed = point_vec_.size() > 1; + + for (size_t i = 0; i < group_vec_.size(); ++i) { + for (size_t j = 0; j + 1 < point_vec_.size(); ++j) { + Point const& p1 = point_vec_[j]; + Point const& p2 = point_vec_[j + 1]; + Line l = Line::Through(p1, p2); + Point p1_t = group_vec_[i] * p1; + Point p2_t = group_vec_[i] * p2; + Line l_t = group_vec_[i] * l; + + SOPHUS_TEST_APPROX(passed, l_t.squaredDistance(p1_t), + static_cast(0), kSmallEps, + "Transform line case (1st point) : %", i); + SOPHUS_TEST_APPROX(passed, l_t.squaredDistance(p2_t), + static_cast(0), kSmallEps, + "Transform line case (2nd point) : %", i); + SOPHUS_TEST_APPROX(passed, l_t.direction().squaredNorm(), + l.direction().squaredNorm(), kSmallEps, + "Transform line case (direction) : %", i); + } + } + return passed; + } + + bool lieBracketTest() { + bool passed = true; + for (size_t i = 0; i < tangent_vec_.size(); ++i) { + for (size_t j = 0; j < tangent_vec_.size(); ++j) { + Tangent tangent1 = + LieGroup::lieBracket(tangent_vec_[i], tangent_vec_[j]); + Transformation hati = LieGroup::hat(tangent_vec_[i]); + Transformation hatj = LieGroup::hat(tangent_vec_[j]); + + Tangent tangent2 = LieGroup::vee(hati * hatj - hatj * hati); + SOPHUS_TEST_APPROX(passed, tangent1, tangent2, kSmallEps, + "Lie Bracket case: %", i); + } + } + return passed; + } + + bool veeHatTest() { + bool passed = true; + for (size_t i = 0; i < tangent_vec_.size(); ++i) { + SOPHUS_TEST_APPROX(passed, Tangent(tangent_vec_[i]), + LieGroup::vee(LieGroup::hat(tangent_vec_[i])), + kSmallEps, "Hat-vee case: %", i); + } + return passed; + } + + bool newDeleteSmokeTest() { + bool passed = true; + LieGroup* raw_ptr = nullptr; + raw_ptr = new LieGroup(); + SOPHUS_TEST_NEQ(passed, reinterpret_cast(raw_ptr), 0); + delete raw_ptr; + return passed; + } + + bool interpolateAndMeanTest() { + bool passed = true; + using std::sqrt; + Scalar const eps = Constants::epsilon(); + Scalar const sqrt_eps = sqrt(eps); + // TODO: Improve accuracy of ``interpolate`` (and hence ``exp`` and ``log``) + // so that we can use more accurate bounds in these tests, i.e. + // ``eps`` instead of ``sqrt_eps``. + + for (LieGroup const& foo_T_bar : group_vec_) { + for (LieGroup const& foo_T_baz : group_vec_) { + // Test boundary conditions ``alpha=0`` and ``alpha=1``. + LieGroup foo_T_quiz = interpolate(foo_T_bar, foo_T_baz, Scalar(0)); + SOPHUS_TEST_APPROX(passed, foo_T_quiz.matrix(), foo_T_bar.matrix(), + sqrt_eps); + foo_T_quiz = interpolate(foo_T_bar, foo_T_baz, Scalar(1)); + SOPHUS_TEST_APPROX(passed, foo_T_quiz.matrix(), foo_T_baz.matrix(), + sqrt_eps); + } + } + for (Scalar alpha : + {Scalar(0.1), Scalar(0.5), Scalar(0.75), Scalar(0.99)}) { + for (LieGroup const& foo_T_bar : group_vec_) { + for (LieGroup const& foo_T_baz : group_vec_) { + LieGroup foo_T_quiz = interpolate(foo_T_bar, foo_T_baz, alpha); + // test left-invariance: + // + // dash_T_foo * interp(foo_T_bar, foo_T_baz) + // == interp(dash_T_foo * foo_T_bar, dash_T_foo * foo_T_baz) + + if (interp_details::Traits::hasShortestPathAmbiguity( + foo_T_bar.inverse() * foo_T_baz)) { + // skip check since there is a shortest path ambiguity + continue; + } + for (LieGroup const& dash_T_foo : group_vec_) { + LieGroup dash_T_quiz = interpolate(dash_T_foo * foo_T_bar, + dash_T_foo * foo_T_baz, alpha); + SOPHUS_TEST_APPROX(passed, dash_T_quiz.matrix(), + (dash_T_foo * foo_T_quiz).matrix(), sqrt_eps); + } + // test inverse-invariance: + // + // interp(foo_T_bar, foo_T_baz).inverse() + // == interp(foo_T_bar.inverse(), dash_T_foo.inverse()) + LieGroup quiz_T_foo = + interpolate(foo_T_bar.inverse(), foo_T_baz.inverse(), alpha); + SOPHUS_TEST_APPROX(passed, quiz_T_foo.inverse().matrix(), + foo_T_quiz.matrix(), sqrt_eps); + } + } + + for (LieGroup const& bar_T_foo : group_vec_) { + for (LieGroup const& baz_T_foo : group_vec_) { + LieGroup quiz_T_foo = interpolate(bar_T_foo, baz_T_foo, alpha); + // test right-invariance: + // + // interp(bar_T_foo, bar_T_foo) * foo_T_dash + // == interp(bar_T_foo * foo_T_dash, bar_T_foo * foo_T_dash) + + if (interp_details::Traits::hasShortestPathAmbiguity( + bar_T_foo * baz_T_foo.inverse())) { + // skip check since there is a shortest path ambiguity + continue; + } + for (LieGroup const& foo_T_dash : group_vec_) { + LieGroup quiz_T_dash = interpolate(bar_T_foo * foo_T_dash, + baz_T_foo * foo_T_dash, alpha); + SOPHUS_TEST_APPROX(passed, quiz_T_dash.matrix(), + (quiz_T_foo * foo_T_dash).matrix(), sqrt_eps); + } + } + } + } + + for (LieGroup const& foo_T_bar : group_vec_) { + for (LieGroup const& foo_T_baz : group_vec_) { + if (interp_details::Traits::hasShortestPathAmbiguity( + foo_T_bar.inverse() * foo_T_baz)) { + // skip check since there is a shortest path ambiguity + continue; + } + + // test average({A, B}) == interp(A, B): + LieGroup foo_T_quiz = interpolate(foo_T_bar, foo_T_baz, 0.5); + optional foo_T_iaverage = iterativeMean( + std::array({{foo_T_bar, foo_T_baz}}), 20); + optional foo_T_average = + average(std::array({{foo_T_bar, foo_T_baz}})); + SOPHUS_TEST(passed, bool(foo_T_average), + "log(foo_T_bar): %\nlog(foo_T_baz): %", + transpose(foo_T_bar.log()), transpose(foo_T_baz.log())); + if (foo_T_average) { + SOPHUS_TEST_APPROX( + passed, foo_T_quiz.matrix(), foo_T_average->matrix(), sqrt_eps, + "log(foo_T_bar): %\nlog(foo_T_baz): %\n" + "log(interp): %\nlog(average): %", + transpose(foo_T_bar.log()), transpose(foo_T_baz.log()), + transpose(foo_T_quiz.log()), transpose(foo_T_average->log())); + } + SOPHUS_TEST(passed, bool(foo_T_iaverage), + "log(foo_T_bar): %\nlog(foo_T_baz): %\n" + "log(interp): %\nlog(iaverage): %", + transpose(foo_T_bar.log()), transpose(foo_T_baz.log()), + transpose(foo_T_quiz.log()), + transpose(foo_T_iaverage->log())); + if (foo_T_iaverage) { + SOPHUS_TEST_APPROX( + passed, foo_T_quiz.matrix(), foo_T_iaverage->matrix(), sqrt_eps, + "log(foo_T_bar): %\nlog(foo_T_baz): %", + transpose(foo_T_bar.log()), transpose(foo_T_baz.log())); + } + } + } + + return passed; + } + + bool testRandomSmoke() { + bool passed = true; + std::default_random_engine engine; + for (int i = 0; i < 100; ++i) { + LieGroup g = LieGroup::sampleUniform(engine); + SOPHUS_TEST_EQUAL(passed, g.params(), g.params()); + } + return passed; + } + + template + enable_if_t::value, bool> doAllTestsPass() { + return doesLargeTestSetPass(); + } + + template + enable_if_t::value, bool> doAllTestsPass() { + return doesSmallTestSetPass(); + } + + private: + bool doesSmallTestSetPass() { + bool passed = true; + passed &= adjointTest(); + passed &= contructorAndAssignmentTest(); + passed &= productTest(); + passed &= expLogTest(); + passed &= groupActionTest(); + passed &= lineActionTest(); + passed &= lieBracketTest(); + passed &= veeHatTest(); + passed &= newDeleteSmokeTest(); + return passed; + } + + bool doesLargeTestSetPass() { + bool passed = true; + passed &= doesSmallTestSetPass(); + passed &= additionalDerivativeTest(); + passed &= derivativeTest(); + passed &= expMapTest(); + passed &= interpolateAndMeanTest(); + passed &= testRandomSmoke(); + return passed; + } + + Scalar const kSmallEps = Constants::epsilon(); + Scalar const kSmallEpsSqrt = Constants::epsilonSqrt(); + + Eigen::Matrix map( + Eigen::Matrix const& T, + Eigen::Matrix const& p) { + return T.template topLeftCorner() * p + + T.template topRightCorner(); + } + + Eigen::Matrix map(Eigen::Matrix const& T, + Eigen::Matrix const& p) { + return T * p; + } + + std::vector> group_vec_; + std::vector> tangent_vec_; + std::vector> point_vec_; +}; + +template +std::vector, Eigen::aligned_allocator>> getTestSE3s() { + Scalar const kPi = Constants::pi(); + std::vector, Eigen::aligned_allocator>> se3_vec; + se3_vec.push_back(SE3( + SO3::exp(Vector3(Scalar(0.2), Scalar(0.5), Scalar(0.0))), + Vector3(Scalar(0), Scalar(0), Scalar(0)))); + se3_vec.push_back(SE3( + SO3::exp(Vector3(Scalar(0.2), Scalar(0.5), Scalar(-1.0))), + Vector3(Scalar(10), Scalar(0), Scalar(0)))); + se3_vec.push_back( + SE3::trans(Vector3(Scalar(0), Scalar(100), Scalar(5)))); + se3_vec.push_back(SE3::rotZ(Scalar(0.00001))); + se3_vec.push_back( + SE3::trans(Scalar(0), Scalar(-0.00000001), Scalar(0.0000000001)) * + SE3::rotZ(Scalar(0.00001))); + se3_vec.push_back(SE3::transX(Scalar(0.01)) * + SE3::rotZ(Scalar(0.00001))); + se3_vec.push_back(SE3::trans(Scalar(4), Scalar(-5), Scalar(0)) * + SE3::rotX(kPi)); + se3_vec.push_back( + SE3(SO3::exp( + Vector3(Scalar(0.2), Scalar(0.5), Scalar(0.0))), + Vector3(Scalar(0), Scalar(0), Scalar(0))) * + SE3::rotX(kPi) * + SE3(SO3::exp(Vector3(Scalar(-0.2), Scalar(-0.5), + Scalar(-0.0))), + Vector3(Scalar(0), Scalar(0), Scalar(0)))); + se3_vec.push_back( + SE3(SO3::exp( + Vector3(Scalar(0.3), Scalar(0.5), Scalar(0.1))), + Vector3(Scalar(2), Scalar(0), Scalar(-7))) * + SE3::rotX(kPi) * + SE3(SO3::exp(Vector3(Scalar(-0.3), Scalar(-0.5), + Scalar(-0.1))), + Vector3(Scalar(0), Scalar(6), Scalar(0)))); + return se3_vec; +} + +template +std::vector, Eigen::aligned_allocator>> getTestSE2s() { + T const kPi = Constants::pi(); + std::vector, Eigen::aligned_allocator>> se2_vec; + se2_vec.push_back(SE2()); + se2_vec.push_back(SE2(SO2(0.2), Vector2(10, 0))); + se2_vec.push_back(SE2::transY(100)); + se2_vec.push_back(SE2::trans(Vector2(1, 2))); + se2_vec.push_back(SE2(SO2(-1.), Vector2(20, -1))); + se2_vec.push_back( + SE2(SO2(0.00001), Vector2(-0.00000001, 0.0000000001))); + se2_vec.push_back(SE2(SO2(0.3), Vector2(2, 0)) * SE2::rot(kPi) * + SE2(SO2(-0.3), Vector2(0, 6))); + return se2_vec; +} +} // namespace Sophus +#endif // TESTS_HPP diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/CMakeLists.txt b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/CMakeLists.txt new file mode 100644 index 0000000..1a32ff9 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/CMakeLists.txt @@ -0,0 +1,178 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 2.6) +SET(CMAKE_LEGACY_CYGWIN_WIN32 0) + +PROJECT(g2o) + +SET(g2o_C_FLAGS) +SET(g2o_CXX_FLAGS) + +# default built type +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +MESSAGE(STATUS "BUILD TYPE:" ${CMAKE_BUILD_TYPE}) + +SET (G2O_LIB_TYPE SHARED) + +# There seems to be an issue with MSVC8 +# see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=83 +if(MSVC90) + add_definitions(-DEIGEN_DONT_ALIGN_STATICALLY=1) + message(STATUS "Disabling memory alignment for MSVC8") +endif(MSVC90) + +# Set the output directory for the build executables and libraries +IF(WIN32) + SET(g2o_LIBRARY_OUTPUT_DIRECTORY ${g2o_SOURCE_DIR}/bin CACHE PATH "Target for the libraries") +ELSE(WIN32) + SET(g2o_LIBRARY_OUTPUT_DIRECTORY ${g2o_SOURCE_DIR}/lib CACHE PATH "Target for the libraries") +ENDIF(WIN32) +SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${g2o_LIBRARY_OUTPUT_DIRECTORY}) +SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${g2o_LIBRARY_OUTPUT_DIRECTORY}) +SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${g2o_RUNTIME_OUTPUT_DIRECTORY}) + +# Set search directory for looking for our custom CMake scripts to +# look for SuiteSparse, QGLViewer, and Eigen3. +LIST(APPEND CMAKE_MODULE_PATH ${g2o_SOURCE_DIR}/cmake_modules) + +# Detect OS and define macros appropriately +IF(UNIX) + ADD_DEFINITIONS(-DUNIX) + MESSAGE(STATUS "Compiling on Unix") +ENDIF(UNIX) + +# Eigen library parallelise itself, though, presumably due to performance issues +# OPENMP is experimental. We experienced some slowdown with it +FIND_PACKAGE(OpenMP) +SET(G2O_USE_OPENMP OFF CACHE BOOL "Build g2o with OpenMP support (EXPERIMENTAL)") +IF(OPENMP_FOUND AND G2O_USE_OPENMP) + SET (G2O_OPENMP 1) + SET(g2o_C_FLAGS "${g2o_C_FLAGS} ${OpenMP_C_FLAGS}") + SET(g2o_CXX_FLAGS "${g2o_CXX_FLAGS} -DEIGEN_DONT_PARALLELIZE ${OpenMP_CXX_FLAGS}") + MESSAGE(STATUS "Compiling with OpenMP support") +ENDIF(OPENMP_FOUND AND G2O_USE_OPENMP) + +# Compiler specific options for gcc +SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -march=native") +SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3 -march=native") +# SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3") +# SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3") + +# activate warnings !!! +SET(g2o_C_FLAGS "${g2o_C_FLAGS} -Wall -W") +SET(g2o_CXX_FLAGS "${g2o_CXX_FLAGS} -Wall -W") + +# specifying compiler flags +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${g2o_CXX_FLAGS}") +SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${g2o_C_FLAGS}") + +# Find Eigen3 +SET(EIGEN3_INCLUDE_DIR ${G2O_EIGEN3_INCLUDE}) +FIND_PACKAGE(Eigen3 3.1.0 REQUIRED) +IF(EIGEN3_FOUND) + SET(G2O_EIGEN3_INCLUDE ${EIGEN3_INCLUDE_DIR} CACHE PATH "Directory of Eigen3") +ELSE(EIGEN3_FOUND) + SET(G2O_EIGEN3_INCLUDE "" CACHE PATH "Directory of Eigen3") +ENDIF(EIGEN3_FOUND) + +# Generate config.h +SET(G2O_CXX_COMPILER "${CMAKE_CXX_COMPILER_ID} ${CMAKE_CXX_COMPILER}") +configure_file(config.h.in ${g2o_SOURCE_DIR}/config.h) + +# Set up the top-level include directories +INCLUDE_DIRECTORIES( +${g2o_SOURCE_DIR}/core +${g2o_SOURCE_DIR}/types +${g2o_SOURCE_DIR}/stuff +${G2O_EIGEN3_INCLUDE}) + +# Include the subdirectories +ADD_LIBRARY(g2o ${G2O_LIB_TYPE} +#types +g2o/types/types_sba.h +g2o/types/types_six_dof_expmap.h +g2o/types/types_sba.cpp +g2o/types/types_six_dof_expmap.cpp +g2o/types/types_seven_dof_expmap.cpp +g2o/types/types_seven_dof_expmap.h +g2o/types/se3quat.h +g2o/types/se3_ops.h +g2o/types/se3_ops.hpp +#core +g2o/core/base_edge.h +g2o/core/base_binary_edge.h +g2o/core/hyper_graph_action.cpp +g2o/core/base_binary_edge.hpp +g2o/core/hyper_graph_action.h +g2o/core/base_multi_edge.h +g2o/core/hyper_graph.cpp +g2o/core/base_multi_edge.hpp +g2o/core/hyper_graph.h +g2o/core/base_unary_edge.h +g2o/core/linear_solver.h +g2o/core/base_unary_edge.hpp +g2o/core/marginal_covariance_cholesky.cpp +g2o/core/base_vertex.h +g2o/core/marginal_covariance_cholesky.h +g2o/core/base_vertex.hpp +g2o/core/matrix_structure.cpp +g2o/core/batch_stats.cpp +g2o/core/matrix_structure.h +g2o/core/batch_stats.h +g2o/core/openmp_mutex.h +g2o/core/block_solver.h +g2o/core/block_solver.hpp +g2o/core/parameter.cpp +g2o/core/parameter.h +g2o/core/cache.cpp +g2o/core/cache.h +g2o/core/optimizable_graph.cpp +g2o/core/optimizable_graph.h +g2o/core/solver.cpp +g2o/core/solver.h +g2o/core/creators.h +g2o/core/optimization_algorithm_factory.cpp +g2o/core/estimate_propagator.cpp +g2o/core/optimization_algorithm_factory.h +g2o/core/estimate_propagator.h +g2o/core/factory.cpp +g2o/core/optimization_algorithm_property.h +g2o/core/factory.h +g2o/core/sparse_block_matrix.h +g2o/core/sparse_optimizer.cpp +g2o/core/sparse_block_matrix.hpp +g2o/core/sparse_optimizer.h +g2o/core/hyper_dijkstra.cpp +g2o/core/hyper_dijkstra.h +g2o/core/parameter_container.cpp +g2o/core/parameter_container.h +g2o/core/optimization_algorithm.cpp +g2o/core/optimization_algorithm.h +g2o/core/optimization_algorithm_with_hessian.cpp +g2o/core/optimization_algorithm_with_hessian.h +g2o/core/optimization_algorithm_levenberg.cpp +g2o/core/optimization_algorithm_levenberg.h +g2o/core/optimization_algorithm_gauss_newton.cpp +g2o/core/optimization_algorithm_gauss_newton.h +g2o/core/jacobian_workspace.cpp +g2o/core/jacobian_workspace.h +g2o/core/robust_kernel.cpp +g2o/core/robust_kernel.h +g2o/core/robust_kernel_factory.cpp +g2o/core/robust_kernel_factory.h +g2o/core/robust_kernel_impl.cpp +g2o/core/robust_kernel_impl.h +#stuff +g2o/stuff/string_tools.h +g2o/stuff/color_macros.h +g2o/stuff/macros.h +g2o/stuff/timeutil.cpp +g2o/stuff/misc.h +g2o/stuff/timeutil.h +g2o/stuff/os_specific.c +g2o/stuff/os_specific.h +g2o/stuff/string_tools.cpp +g2o/stuff/property.cpp +g2o/stuff/property.h +) diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/README.txt b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/README.txt new file mode 100644 index 0000000..82641a8 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/README.txt @@ -0,0 +1,3 @@ +You should have received this g2o version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). +See the original g2o library at: https://github.com/RainerKuemmerle/g2o +All files included in this g2o version are BSD, see license-bsd.txt diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/config.h.in b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/config.h.in new file mode 100644 index 0000000..49c686b --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/config.h.in @@ -0,0 +1,13 @@ +#ifndef G2O_CONFIG_H +#define G2O_CONFIG_H + +#cmakedefine G2O_OPENMP 1 +#cmakedefine G2O_SHARED_LIBS 1 + +// give a warning if Eigen defaults to row-major matrices. +// We internally assume column-major matrices throughout the code. +#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR +# error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" +#endif + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_binary_edge.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_binary_edge.h new file mode 100644 index 0000000..660e83a --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_binary_edge.h @@ -0,0 +1,119 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_BASE_BINARY_EDGE_H +#define G2O_BASE_BINARY_EDGE_H + +#include +#include + +#include "base_edge.h" +#include "robust_kernel.h" +#include "../../config.h" + +namespace g2o { + + using namespace Eigen; + + template + class BaseBinaryEdge : public BaseEdge + { + public: + + typedef VertexXi VertexXiType; + typedef VertexXj VertexXjType; + + static const int Di = VertexXiType::Dimension; + static const int Dj = VertexXjType::Dimension; + + static const int Dimension = BaseEdge::Dimension; + typedef typename BaseEdge::Measurement Measurement; + typedef typename Matrix::AlignedMapType JacobianXiOplusType; + typedef typename Matrix::AlignedMapType JacobianXjOplusType; + typedef typename BaseEdge::ErrorVector ErrorVector; + typedef typename BaseEdge::InformationType InformationType; + + typedef Eigen::Map, Matrix::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType; + typedef Eigen::Map, Matrix::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockTransposedType; + + BaseBinaryEdge() : BaseEdge(), + _hessianRowMajor(false), + _hessian(0, VertexXiType::Dimension, VertexXjType::Dimension), // HACK we map to the null pointer for initializing the Maps + _hessianTransposed(0, VertexXjType::Dimension, VertexXiType::Dimension), + _jacobianOplusXi(0, D, Di), _jacobianOplusXj(0, D, Dj) + { + _vertices.resize(2); + } + + virtual OptimizableGraph::Vertex* createFrom(); + virtual OptimizableGraph::Vertex* createTo(); + + virtual void resize(size_t size); + + virtual bool allVerticesFixed() const; + + virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace); + + /** + * Linearizes the oplus operator in the vertex, and stores + * the result in temporary variables _jacobianOplusXi and _jacobianOplusXj + */ + virtual void linearizeOplus(); + + //! returns the result of the linearization in the manifold space for the node xi + const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;} + //! returns the result of the linearization in the manifold space for the node xj + const JacobianXjOplusType& jacobianOplusXj() const { return _jacobianOplusXj;} + + virtual void constructQuadraticForm() ; + + virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor); + + using BaseEdge::resize; + using BaseEdge::computeError; + + protected: + using BaseEdge::_measurement; + using BaseEdge::_information; + using BaseEdge::_error; + using BaseEdge::_vertices; + using BaseEdge::_dimension; + + bool _hessianRowMajor; + HessianBlockType _hessian; + HessianBlockTransposedType _hessianTransposed; + JacobianXiOplusType _jacobianOplusXi; + JacobianXjOplusType _jacobianOplusXj; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + +#include "base_binary_edge.hpp" + +} // end namespace g2o + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_binary_edge.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_binary_edge.hpp new file mode 100644 index 0000000..7542a9f --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_binary_edge.hpp @@ -0,0 +1,218 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +template +OptimizableGraph::Vertex* BaseBinaryEdge::createFrom(){ + return new VertexXiType(); +} + +template +OptimizableGraph::Vertex* BaseBinaryEdge::createTo(){ + return new VertexXjType(); +} + + +template +void BaseBinaryEdge::resize(size_t size) +{ + if (size != 2) { + std::cerr << "WARNING, attempting to resize binary edge " << BaseEdge::id() << " to " << size << std::endl; + } + BaseEdge::resize(size); +} + +template +bool BaseBinaryEdge::allVerticesFixed() const +{ + return (static_cast (_vertices[0])->fixed() && + static_cast (_vertices[1])->fixed()); +} + +template +void BaseBinaryEdge::constructQuadraticForm() +{ + VertexXiType* from = static_cast(_vertices[0]); + VertexXjType* to = static_cast(_vertices[1]); + + // get the Jacobian of the nodes in the manifold domain + const JacobianXiOplusType& A = jacobianOplusXi(); + const JacobianXjOplusType& B = jacobianOplusXj(); + + + bool fromNotFixed = !(from->fixed()); + bool toNotFixed = !(to->fixed()); + + if (fromNotFixed || toNotFixed) { +#ifdef G2O_OPENMP + from->lockQuadraticForm(); + to->lockQuadraticForm(); +#endif + const InformationType& omega = _information; + Matrix omega_r = - omega * _error; + if (this->robustKernel() == 0) { + if (fromNotFixed) { + Matrix AtO = A.transpose() * omega; + from->b().noalias() += A.transpose() * omega_r; + from->A().noalias() += AtO*A; + if (toNotFixed ) { + if (_hessianRowMajor) // we have to write to the block as transposed + _hessianTransposed.noalias() += B.transpose() * AtO.transpose(); + else + _hessian.noalias() += AtO * B; + } + } + if (toNotFixed) { + to->b().noalias() += B.transpose() * omega_r; + to->A().noalias() += B.transpose() * omega * B; + } + } else { // robust (weighted) error according to some kernel + double error = this->chi2(); + Eigen::Vector3d rho; + this->robustKernel()->robustify(error, rho); + InformationType weightedOmega = this->robustInformation(rho); + //std::cout << PVAR(rho.transpose()) << std::endl; + //std::cout << PVAR(weightedOmega) << std::endl; + + omega_r *= rho[1]; + if (fromNotFixed) { + from->b().noalias() += A.transpose() * omega_r; + from->A().noalias() += A.transpose() * weightedOmega * A; + if (toNotFixed ) { + if (_hessianRowMajor) // we have to write to the block as transposed + _hessianTransposed.noalias() += B.transpose() * weightedOmega * A; + else + _hessian.noalias() += A.transpose() * weightedOmega * B; + } + } + if (toNotFixed) { + to->b().noalias() += B.transpose() * omega_r; + to->A().noalias() += B.transpose() * weightedOmega * B; + } + } +#ifdef G2O_OPENMP + to->unlockQuadraticForm(); + from->unlockQuadraticForm(); +#endif + } +} + +template +void BaseBinaryEdge::linearizeOplus(JacobianWorkspace& jacobianWorkspace) +{ + new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, Di); + new (&_jacobianOplusXj) JacobianXjOplusType(jacobianWorkspace.workspaceForVertex(1), D, Dj); + linearizeOplus(); +} + +template +void BaseBinaryEdge::linearizeOplus() +{ + VertexXiType* vi = static_cast(_vertices[0]); + VertexXjType* vj = static_cast(_vertices[1]); + + bool iNotFixed = !(vi->fixed()); + bool jNotFixed = !(vj->fixed()); + + if (!iNotFixed && !jNotFixed) + return; + +#ifdef G2O_OPENMP + vi->lockQuadraticForm(); + vj->lockQuadraticForm(); +#endif + + const double delta = 1e-9; + const double scalar = 1.0 / (2*delta); + ErrorVector errorBak; + ErrorVector errorBeforeNumeric = _error; + + if (iNotFixed) { + //Xi - estimate the jacobian numerically + double add_vi[VertexXiType::Dimension]; + std::fill(add_vi, add_vi + VertexXiType::Dimension, 0.0); + // add small step along the unit vector in each dimension + for (int d = 0; d < VertexXiType::Dimension; ++d) { + vi->push(); + add_vi[d] = delta; + vi->oplus(add_vi); + computeError(); + errorBak = _error; + vi->pop(); + vi->push(); + add_vi[d] = -delta; + vi->oplus(add_vi); + computeError(); + errorBak -= _error; + vi->pop(); + add_vi[d] = 0.0; + + _jacobianOplusXi.col(d) = scalar * errorBak; + } // end dimension + } + + if (jNotFixed) { + //Xj - estimate the jacobian numerically + double add_vj[VertexXjType::Dimension]; + std::fill(add_vj, add_vj + VertexXjType::Dimension, 0.0); + // add small step along the unit vector in each dimension + for (int d = 0; d < VertexXjType::Dimension; ++d) { + vj->push(); + add_vj[d] = delta; + vj->oplus(add_vj); + computeError(); + errorBak = _error; + vj->pop(); + vj->push(); + add_vj[d] = -delta; + vj->oplus(add_vj); + computeError(); + errorBak -= _error; + vj->pop(); + add_vj[d] = 0.0; + + _jacobianOplusXj.col(d) = scalar * errorBak; + } + } // end dimension + + _error = errorBeforeNumeric; +#ifdef G2O_OPENMP + vj->unlockQuadraticForm(); + vi->unlockQuadraticForm(); +#endif +} + +template +void BaseBinaryEdge::mapHessianMemory(double* d, int i, int j, bool rowMajor) +{ + (void) i; (void) j; + //assert(i == 0 && j == 1); + if (rowMajor) { + new (&_hessianTransposed) HessianBlockTransposedType(d, VertexXjType::Dimension, VertexXiType::Dimension); + } else { + new (&_hessian) HessianBlockType(d, VertexXiType::Dimension, VertexXjType::Dimension); + } + _hessianRowMajor = rowMajor; +} diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_edge.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_edge.h new file mode 100644 index 0000000..91139e4 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_edge.h @@ -0,0 +1,110 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_BASE_EDGE_H +#define G2O_BASE_EDGE_H + +#include +#include + +#include + +#include "optimizable_graph.h" + +namespace g2o { + + using namespace Eigen; + + template + class BaseEdge : public OptimizableGraph::Edge + { + public: + + static const int Dimension = D; + typedef E Measurement; + typedef Matrix ErrorVector; + typedef Matrix InformationType; + + BaseEdge() : OptimizableGraph::Edge() + { + _dimension = D; + } + + virtual ~BaseEdge() {} + + virtual double chi2() const + { + return _error.dot(information()*_error); + } + + virtual const double* errorData() const { return _error.data();} + virtual double* errorData() { return _error.data();} + const ErrorVector& error() const { return _error;} + ErrorVector& error() { return _error;} + + //! information matrix of the constraint + const InformationType& information() const { return _information;} + InformationType& information() { return _information;} + void setInformation(const InformationType& information) { _information = information;} + + virtual const double* informationData() const { return _information.data();} + virtual double* informationData() { return _information.data();} + + //! accessor functions for the measurement represented by the edge + const Measurement& measurement() const { return _measurement;} + virtual void setMeasurement(const Measurement& m) { _measurement = m;} + + virtual int rank() const {return _dimension;} + + virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*) + { + std::cerr << "inititialEstimate() is not implemented, please give implementation in your derived class" << std::endl; + } + + protected: + + Measurement _measurement; + InformationType _information; + ErrorVector _error; + + /** + * calculate the robust information matrix by updating the information matrix of the error + */ + InformationType robustInformation(const Eigen::Vector3d& rho) + { + InformationType result = rho[1] * _information; + //ErrorVector weightedErrror = _information * _error; + //result.noalias() += 2 * rho[2] * (weightedErrror * weightedErrror.transpose()); + return result; + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + +} // end namespace g2o + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_multi_edge.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_multi_edge.h new file mode 100644 index 0000000..dd2261f --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_multi_edge.h @@ -0,0 +1,113 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_BASE_MULTI_EDGE_H +#define G2O_BASE_MULTI_EDGE_H + +#include +#include +#include + +#include + +#include "base_edge.h" +#include "robust_kernel.h" +#include "../../config.h" + +namespace g2o { + + using namespace Eigen; + + /** + * \brief base class to represent an edge connecting an arbitrary number of nodes + * + * D - Dimension of the measurement + * E - type to represent the measurement + */ + template + class BaseMultiEdge : public BaseEdge + { + public: + /** + * \brief helper for mapping the Hessian memory of the upper triangular block + */ + struct HessianHelper { + Eigen::Map matrix; ///< the mapped memory + bool transposed; ///< the block has to be transposed + HessianHelper() : matrix(0, 0, 0), transposed(false) {} + }; + + public: + static const int Dimension = BaseEdge::Dimension; + typedef typename BaseEdge::Measurement Measurement; + typedef MatrixXd::MapType JacobianType; + typedef typename BaseEdge::ErrorVector ErrorVector; + typedef typename BaseEdge::InformationType InformationType; + typedef Eigen::Map HessianBlockType; + + BaseMultiEdge() : BaseEdge() + { + } + + virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace); + + /** + * Linearizes the oplus operator in the vertex, and stores + * the result in temporary variable vector _jacobianOplus + */ + virtual void linearizeOplus(); + + virtual void resize(size_t size); + + virtual bool allVerticesFixed() const; + + virtual void constructQuadraticForm() ; + + virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor); + + using BaseEdge::computeError; + + protected: + using BaseEdge::_measurement; + using BaseEdge::_information; + using BaseEdge::_error; + using BaseEdge::_vertices; + using BaseEdge::_dimension; + + std::vector _hessian; + std::vector > _jacobianOplus; ///< jacobians of the edge (w.r.t. oplus) + + void computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError); + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + +#include "base_multi_edge.hpp" + +} // end namespace g2o + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_multi_edge.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_multi_edge.hpp new file mode 100644 index 0000000..a2fa66d --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_multi_edge.hpp @@ -0,0 +1,222 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +namespace internal { + inline int computeUpperTriangleIndex(int i, int j) + { + int elemsUpToCol = ((j-1) * j) / 2; + return elemsUpToCol + i; + } +} + +template +void BaseMultiEdge::constructQuadraticForm() +{ + if (this->robustKernel()) { + double error = this->chi2(); + Eigen::Vector3d rho; + this->robustKernel()->robustify(error, rho); + Matrix omega_r = - _information * _error; + omega_r *= rho[1]; + computeQuadraticForm(this->robustInformation(rho), omega_r); + } else { + computeQuadraticForm(_information, - _information * _error); + } +} + + +template +void BaseMultiEdge::linearizeOplus(JacobianWorkspace& jacobianWorkspace) +{ + for (size_t i = 0; i < _vertices.size(); ++i) { + OptimizableGraph::Vertex* v = static_cast(_vertices[i]); + assert(v->dimension() >= 0); + new (&_jacobianOplus[i]) JacobianType(jacobianWorkspace.workspaceForVertex(i), D, v->dimension()); + } + linearizeOplus(); +} + +template +void BaseMultiEdge::linearizeOplus() +{ +#ifdef G2O_OPENMP + for (size_t i = 0; i < _vertices.size(); ++i) { + OptimizableGraph::Vertex* v = static_cast(_vertices[i]); + v->lockQuadraticForm(); + } +#endif + + const double delta = 1e-9; + const double scalar = 1.0 / (2*delta); + ErrorVector errorBak; + ErrorVector errorBeforeNumeric = _error; + + for (size_t i = 0; i < _vertices.size(); ++i) { + //Xi - estimate the jacobian numerically + OptimizableGraph::Vertex* vi = static_cast(_vertices[i]); + + if (vi->fixed()) + continue; + + const int vi_dim = vi->dimension(); + assert(vi_dim >= 0); +#ifdef _MSC_VER + double* add_vi = new double[vi_dim]; +#else + double add_vi[vi_dim]; +#endif + std::fill(add_vi, add_vi + vi_dim, 0.0); + assert(_dimension >= 0); + assert(_jacobianOplus[i].rows() == _dimension && _jacobianOplus[i].cols() == vi_dim && "jacobian cache dimension does not match"); + _jacobianOplus[i].resize(_dimension, vi_dim); + // add small step along the unit vector in each dimension + for (int d = 0; d < vi_dim; ++d) { + vi->push(); + add_vi[d] = delta; + vi->oplus(add_vi); + computeError(); + errorBak = _error; + vi->pop(); + vi->push(); + add_vi[d] = -delta; + vi->oplus(add_vi); + computeError(); + errorBak -= _error; + vi->pop(); + add_vi[d] = 0.0; + + _jacobianOplus[i].col(d) = scalar * errorBak; + } // end dimension +#ifdef _MSC_VER + delete[] add_vi; +#endif + } + _error = errorBeforeNumeric; + +#ifdef G2O_OPENMP + for (int i = (int)(_vertices.size()) - 1; i >= 0; --i) { + OptimizableGraph::Vertex* v = static_cast(_vertices[i]); + v->unlockQuadraticForm(); + } +#endif + +} + +template +void BaseMultiEdge::mapHessianMemory(double* d, int i, int j, bool rowMajor) +{ + int idx = internal::computeUpperTriangleIndex(i, j); + assert(idx < (int)_hessian.size()); + OptimizableGraph::Vertex* vi = static_cast(HyperGraph::Edge::vertex(i)); + OptimizableGraph::Vertex* vj = static_cast(HyperGraph::Edge::vertex(j)); + assert(vi->dimension() >= 0); + assert(vj->dimension() >= 0); + HessianHelper& h = _hessian[idx]; + if (rowMajor) { + if (h.matrix.data() != d || h.transposed != rowMajor) + new (&h.matrix) HessianBlockType(d, vj->dimension(), vi->dimension()); + } else { + if (h.matrix.data() != d || h.transposed != rowMajor) + new (&h.matrix) HessianBlockType(d, vi->dimension(), vj->dimension()); + } + h.transposed = rowMajor; +} + +template +void BaseMultiEdge::resize(size_t size) +{ + BaseEdge::resize(size); + int n = (int)_vertices.size(); + int maxIdx = (n * (n-1))/2; + assert(maxIdx >= 0); + _hessian.resize(maxIdx); + _jacobianOplus.resize(size, JacobianType(0,0,0)); +} + +template +bool BaseMultiEdge::allVerticesFixed() const +{ + for (size_t i = 0; i < _vertices.size(); ++i) { + if (!static_cast (_vertices[i])->fixed()) { + return false; + } + } + return true; +} + +template +void BaseMultiEdge::computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError) +{ + for (size_t i = 0; i < _vertices.size(); ++i) { + OptimizableGraph::Vertex* from = static_cast(_vertices[i]); + bool istatus = !(from->fixed()); + + if (istatus) { + const MatrixXd& A = _jacobianOplus[i]; + + MatrixXd AtO = A.transpose() * omega; + int fromDim = from->dimension(); + assert(fromDim >= 0); + Eigen::Map fromMap(from->hessianData(), fromDim, fromDim); + Eigen::Map fromB(from->bData(), fromDim); + + // ii block in the hessian +#ifdef G2O_OPENMP + from->lockQuadraticForm(); +#endif + fromMap.noalias() += AtO * A; + fromB.noalias() += A.transpose() * weightedError; + + // compute the off-diagonal blocks ij for all j + for (size_t j = i+1; j < _vertices.size(); ++j) { + OptimizableGraph::Vertex* to = static_cast(_vertices[j]); +#ifdef G2O_OPENMP + to->lockQuadraticForm(); +#endif + bool jstatus = !(to->fixed()); + if (jstatus) { + const MatrixXd& B = _jacobianOplus[j]; + int idx = internal::computeUpperTriangleIndex(i, j); + assert(idx < (int)_hessian.size()); + HessianHelper& hhelper = _hessian[idx]; + if (hhelper.transposed) { // we have to write to the block as transposed + hhelper.matrix.noalias() += B.transpose() * AtO.transpose(); + } else { + hhelper.matrix.noalias() += AtO * B; + } + } +#ifdef G2O_OPENMP + to->unlockQuadraticForm(); +#endif + } + +#ifdef G2O_OPENMP + from->unlockQuadraticForm(); +#endif + } + + } +} diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_unary_edge.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_unary_edge.h new file mode 100644 index 0000000..28df8a5 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_unary_edge.h @@ -0,0 +1,100 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_BASE_UNARY_EDGE_H +#define G2O_BASE_UNARY_EDGE_H + +#include +#include +#include + +#include "base_edge.h" +#include "robust_kernel.h" +#include "../../config.h" + +namespace g2o { + + using namespace Eigen; + + template + class BaseUnaryEdge : public BaseEdge + { + public: + static const int Dimension = BaseEdge::Dimension; + typedef typename BaseEdge::Measurement Measurement; + typedef VertexXi VertexXiType; + typedef typename Matrix::AlignedMapType JacobianXiOplusType; + typedef typename BaseEdge::ErrorVector ErrorVector; + typedef typename BaseEdge::InformationType InformationType; + + BaseUnaryEdge() : BaseEdge(), + _jacobianOplusXi(0, D, VertexXiType::Dimension) + { + _vertices.resize(1); + } + + virtual void resize(size_t size); + + virtual bool allVerticesFixed() const; + + virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace); + + /** + * Linearizes the oplus operator in the vertex, and stores + * the result in temporary variables _jacobianOplusXi and _jacobianOplusXj + */ + virtual void linearizeOplus(); + + //! returns the result of the linearization in the manifold space for the node xi + const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;} + + virtual void constructQuadraticForm(); + + virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to); + + virtual void mapHessianMemory(double*, int, int, bool) {assert(0 && "BaseUnaryEdge does not map memory of the Hessian");} + + using BaseEdge::resize; + using BaseEdge::computeError; + + protected: + using BaseEdge::_measurement; + using BaseEdge::_information; + using BaseEdge::_error; + using BaseEdge::_vertices; + using BaseEdge::_dimension; + + JacobianXiOplusType _jacobianOplusXi; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + +#include "base_unary_edge.hpp" + +} // end namespace g2o + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_unary_edge.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_unary_edge.hpp new file mode 100644 index 0000000..464900f --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_unary_edge.hpp @@ -0,0 +1,129 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +template +void BaseUnaryEdge::resize(size_t size) +{ + if (size != 1) { + std::cerr << "WARNING, attempting to resize unary edge " << BaseEdge::id() << " to " << size << std::endl; + } + BaseEdge::resize(size); +} + +template +bool BaseUnaryEdge::allVerticesFixed() const +{ + return static_cast (_vertices[0])->fixed(); +} + +template +void BaseUnaryEdge::constructQuadraticForm() +{ + VertexXiType* from=static_cast(_vertices[0]); + + // chain rule to get the Jacobian of the nodes in the manifold domain + const JacobianXiOplusType& A = jacobianOplusXi(); + const InformationType& omega = _information; + + bool istatus = !from->fixed(); + if (istatus) { +#ifdef G2O_OPENMP + from->lockQuadraticForm(); +#endif + if (this->robustKernel()) { + double error = this->chi2(); + Eigen::Vector3d rho; + this->robustKernel()->robustify(error, rho); + InformationType weightedOmega = this->robustInformation(rho); + + from->b().noalias() -= rho[1] * A.transpose() * omega * _error; + from->A().noalias() += A.transpose() * weightedOmega * A; + } else { + from->b().noalias() -= A.transpose() * omega * _error; + from->A().noalias() += A.transpose() * omega * A; + } +#ifdef G2O_OPENMP + from->unlockQuadraticForm(); +#endif + } +} + +template +void BaseUnaryEdge::linearizeOplus(JacobianWorkspace& jacobianWorkspace) +{ + new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, VertexXiType::Dimension); + linearizeOplus(); +} + +template +void BaseUnaryEdge::linearizeOplus() +{ + //Xi - estimate the jacobian numerically + VertexXiType* vi = static_cast(_vertices[0]); + + if (vi->fixed()) + return; + +#ifdef G2O_OPENMP + vi->lockQuadraticForm(); +#endif + + const double delta = 1e-9; + const double scalar = 1.0 / (2*delta); + ErrorVector error1; + ErrorVector errorBeforeNumeric = _error; + + double add_vi[VertexXiType::Dimension]; + std::fill(add_vi, add_vi + VertexXiType::Dimension, 0.0); + // add small step along the unit vector in each dimension + for (int d = 0; d < VertexXiType::Dimension; ++d) { + vi->push(); + add_vi[d] = delta; + vi->oplus(add_vi); + computeError(); + error1 = _error; + vi->pop(); + vi->push(); + add_vi[d] = -delta; + vi->oplus(add_vi); + computeError(); + vi->pop(); + add_vi[d] = 0.0; + + _jacobianOplusXi.col(d) = scalar * (error1 - _error); + } // end dimension + + _error = errorBeforeNumeric; +#ifdef G2O_OPENMP + vi->unlockQuadraticForm(); +#endif +} + +template +void BaseUnaryEdge::initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*) +{ + std::cerr << __PRETTY_FUNCTION__ << " is not implemented, please give implementation in your derived class" << std::endl; +} diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_vertex.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_vertex.h new file mode 100644 index 0000000..e375fde --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_vertex.h @@ -0,0 +1,120 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_BASE_VERTEX_H +#define G2O_BASE_VERTEX_H + +#include "optimizable_graph.h" +#include "creators.h" +#include "../stuff/macros.h" + +#include +#include +#include +#include +#include + +namespace g2o { + + using namespace Eigen; + + +/** + * \brief Templatized BaseVertex + * + * Templatized BaseVertex + * D : minimal dimension of the vertex, e.g., 3 for rotation in 3D + * T : internal type to represent the estimate, e.g., Quaternion for rotation in 3D + */ + template + class BaseVertex : public OptimizableGraph::Vertex { + public: + typedef T EstimateType; + typedef std::stack > > + BackupStackType; + + static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space + + typedef Eigen::Map, Matrix::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType; + + public: + BaseVertex(); + + virtual const double& hessian(int i, int j) const { assert(i(_hessian.data());} + + virtual void mapHessianMemory(double* d); + + virtual int copyB(double* b_) const { + memcpy(b_, _b.data(), Dimension * sizeof(double)); + return Dimension; + } + + virtual const double& b(int i) const { assert(i < D); return _b(i);} + virtual double& b(int i) { assert(i < D); return _b(i);} + virtual double* bData() { return _b.data();} + + virtual void clearQuadraticForm(); + + //! updates the current vertex with the direct solution x += H_ii\b_ii + //! @returns the determinant of the inverted hessian + virtual double solveDirect(double lambda=0); + + //! return right hand side b of the constructed linear system + Matrix& b() { return _b;} + const Matrix& b() const { return _b;} + //! return the hessian block associated with the vertex + HessianBlockType& A() { return _hessian;} + const HessianBlockType& A() const { return _hessian;} + + virtual void push() { _backup.push(_estimate);} + virtual void pop() { assert(!_backup.empty()); _estimate = _backup.top(); _backup.pop(); updateCache();} + virtual void discardTop() { assert(!_backup.empty()); _backup.pop();} + virtual int stackSize() const {return _backup.size();} + + //! return the current estimate of the vertex + const EstimateType& estimate() const { return _estimate;} + //! set the estimate for the vertex also calls updateCache() + void setEstimate(const EstimateType& et) { _estimate = et; updateCache();} + + protected: + HessianBlockType _hessian; + Matrix _b; + EstimateType _estimate; + BackupStackType _backup; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +#include "base_vertex.hpp" + +} // end namespace g2o + + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_vertex.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_vertex.hpp new file mode 100644 index 0000000..f21ff16 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/base_vertex.hpp @@ -0,0 +1,55 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +template +BaseVertex::BaseVertex() : + OptimizableGraph::Vertex(), + _hessian(0, D, D) +{ + _dimension = D; +} + +template +double BaseVertex::solveDirect(double lambda) { + Matrix tempA=_hessian + Matrix ::Identity()*lambda; + double det=tempA.determinant(); + if (g2o_isnan(det) || det < std::numeric_limits::epsilon()) + return det; + Matrix dx=tempA.llt().solve(_b); + oplus(&dx[0]); + return det; +} + +template +void BaseVertex::clearQuadraticForm() { + _b.setZero(); +} + +template +void BaseVertex::mapHessianMemory(double* d) +{ + new (&_hessian) HessianBlockType(d, D, D); +} diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/batch_stats.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/batch_stats.cpp new file mode 100644 index 0000000..a6beb69 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/batch_stats.cpp @@ -0,0 +1,90 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "batch_stats.h" +#include + +namespace g2o { + using namespace std; + + G2OBatchStatistics* G2OBatchStatistics::_globalStats=0; + + #ifndef PTHING + #define PTHING(s) \ + #s << "= " << (st.s) << "\t " + #endif + + G2OBatchStatistics::G2OBatchStatistics(){ + // zero all. + memset (this, 0, sizeof(G2OBatchStatistics)); + + // set the iteration to -1 to show that it isn't valid + iteration = -1; + } + + std::ostream& operator << (std::ostream& os , const G2OBatchStatistics& st) + { + os << PTHING(iteration); + + os << PTHING( numVertices ); // how many vertices are involved + os << PTHING( numEdges ); // hoe many edges + os << PTHING( chi2 ); // total chi2 + + /** timings **/ + // nonlinear part + os << PTHING( timeResiduals ); + os << PTHING( timeLinearize ); // jacobians + os << PTHING( timeQuadraticForm ); // construct the quadratic form in the graph + + // block_solver (constructs Ax=b, plus maybe schur); + os << PTHING( timeSchurComplement ); // compute schur complement (0 if not done); + + // linear solver (computes Ax=b); ); + os << PTHING( timeSymbolicDecomposition ); // symbolic decomposition (0 if not done); + os << PTHING( timeNumericDecomposition ); // numeric decomposition (0 if not done); + os << PTHING( timeLinearSolution ); // total time for solving Ax=b + os << PTHING( iterationsLinearSolver ); // iterations of PCG + os << PTHING( timeUpdate ); // oplus + os << PTHING( timeIteration ); // total time ); + + os << PTHING( levenbergIterations ); + os << PTHING( timeLinearSolver); + + os << PTHING(hessianDimension); + os << PTHING(hessianPoseDimension); + os << PTHING(hessianLandmarkDimension); + os << PTHING(choleskyNNZ); + os << PTHING(timeMarginals); + + return os; + }; + + void G2OBatchStatistics::setGlobalStats(G2OBatchStatistics* b) + { + _globalStats = b; + } + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/batch_stats.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/batch_stats.h new file mode 100644 index 0000000..d039f65 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/batch_stats.h @@ -0,0 +1,83 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_BATCH_STATS_H_ +#define G2O_BATCH_STATS_H_ + +#include +#include + + +namespace g2o { + + /** + * \brief statistics about the optimization + */ + struct G2OBatchStatistics { + G2OBatchStatistics(); + int iteration; ///< which iteration + int numVertices; ///< how many vertices are involved + int numEdges; ///< how many edges + double chi2; ///< total chi2 + + /** timings **/ + // nonlinear part + double timeResiduals; ///< residuals + double timeLinearize; ///< jacobians + double timeQuadraticForm; ///< construct the quadratic form in the graph + int levenbergIterations; ///< number of iterations performed by LM + // block_solver (constructs Ax=b, plus maybe schur) + double timeSchurComplement; ///< compute schur complement (0 if not done) + + // linear solver (computes Ax=b); + double timeSymbolicDecomposition; ///< symbolic decomposition (0 if not done) + double timeNumericDecomposition; ///< numeric decomposition (0 if not done) + double timeLinearSolution; ///< total time for solving Ax=b (including detup for schur) + double timeLinearSolver; ///< time for solving, excluding Schur setup + int iterationsLinearSolver; ///< iterations of PCG, (0 if not used, i.e., Cholesky) + double timeUpdate; ///< time to apply the update + double timeIteration; ///< total time; + + double timeMarginals; ///< computing the inverse elements (solve blocks) and thus the marginal covariances + + // information about the Hessian matrix + size_t hessianDimension; ///< rows / cols of the Hessian + size_t hessianPoseDimension; ///< dimension of the pose matrix in Schur + size_t hessianLandmarkDimension; ///< dimension of the landmark matrix in Schur + size_t choleskyNNZ; ///< number of non-zeros in the cholesky factor + + static G2OBatchStatistics* globalStats() {return _globalStats;} + static void setGlobalStats(G2OBatchStatistics* b); + protected: + static G2OBatchStatistics* _globalStats; + }; + + std::ostream& operator<<(std::ostream&, const G2OBatchStatistics&); + + typedef std::vector BatchStatisticsContainer; +} + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/block_solver.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/block_solver.h new file mode 100644 index 0000000..a393398 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/block_solver.h @@ -0,0 +1,193 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_BLOCK_SOLVER_H +#define G2O_BLOCK_SOLVER_H +#include +#include "solver.h" +#include "linear_solver.h" +#include "sparse_block_matrix.h" +#include "sparse_block_matrix_diagonal.h" +#include "openmp_mutex.h" +#include "../../config.h" + +namespace g2o { + using namespace Eigen; + + /** + * \brief traits to summarize the properties of the fixed size optimization problem + */ + template + struct BlockSolverTraits + { + static const int PoseDim = _PoseDim; + static const int LandmarkDim = _LandmarkDim; + typedef Matrix PoseMatrixType; + typedef Matrix LandmarkMatrixType; + typedef Matrix PoseLandmarkMatrixType; + typedef Matrix PoseVectorType; + typedef Matrix LandmarkVectorType; + + typedef SparseBlockMatrix PoseHessianType; + typedef SparseBlockMatrix LandmarkHessianType; + typedef SparseBlockMatrix PoseLandmarkHessianType; + typedef LinearSolver LinearSolverType; + }; + + /** + * \brief traits to summarize the properties of the dynamic size optimization problem + */ + template <> + struct BlockSolverTraits + { + static const int PoseDim = Eigen::Dynamic; + static const int LandmarkDim = Eigen::Dynamic; + typedef MatrixXd PoseMatrixType; + typedef MatrixXd LandmarkMatrixType; + typedef MatrixXd PoseLandmarkMatrixType; + typedef VectorXd PoseVectorType; + typedef VectorXd LandmarkVectorType; + + typedef SparseBlockMatrix PoseHessianType; + typedef SparseBlockMatrix LandmarkHessianType; + typedef SparseBlockMatrix PoseLandmarkHessianType; + typedef LinearSolver LinearSolverType; + }; + + /** + * \brief base for the block solvers with some basic function interfaces + */ + class BlockSolverBase : public Solver + { + public: + virtual ~BlockSolverBase() {} + /** + * compute dest = H * src + */ + virtual void multiplyHessian(double* dest, const double* src) const = 0; + }; + + /** + * \brief Implementation of a solver operating on the blocks of the Hessian + */ + template + class BlockSolver: public BlockSolverBase { + public: + + static const int PoseDim = Traits::PoseDim; + static const int LandmarkDim = Traits::LandmarkDim; + typedef typename Traits::PoseMatrixType PoseMatrixType; + typedef typename Traits::LandmarkMatrixType LandmarkMatrixType; + typedef typename Traits::PoseLandmarkMatrixType PoseLandmarkMatrixType; + typedef typename Traits::PoseVectorType PoseVectorType; + typedef typename Traits::LandmarkVectorType LandmarkVectorType; + + typedef typename Traits::PoseHessianType PoseHessianType; + typedef typename Traits::LandmarkHessianType LandmarkHessianType; + typedef typename Traits::PoseLandmarkHessianType PoseLandmarkHessianType; + typedef typename Traits::LinearSolverType LinearSolverType; + + public: + + /** + * allocate a block solver ontop of the underlying linear solver. + * NOTE: The BlockSolver assumes exclusive access to the linear solver and will therefore free the pointer + * in its destructor. + */ + BlockSolver(LinearSolverType* linearSolver); + ~BlockSolver(); + + virtual bool init(SparseOptimizer* optmizer, bool online = false); + virtual bool buildStructure(bool zeroBlocks = false); + virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges); + virtual bool buildSystem(); + virtual bool solve(); + virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices); + virtual bool setLambda(double lambda, bool backup = false); + virtual void restoreDiagonal(); + virtual bool supportsSchur() {return true;} + virtual bool schur() { return _doSchur;} + virtual void setSchur(bool s) { _doSchur = s;} + + LinearSolver* linearSolver() const { return _linearSolver;} + + virtual void setWriteDebug(bool writeDebug); + virtual bool writeDebug() const {return _linearSolver->writeDebug();} + + virtual bool saveHessian(const std::string& fileName) const; + + virtual void multiplyHessian(double* dest, const double* src) const { _Hpp->multiplySymmetricUpperTriangle(dest, src);} + + protected: + void resize(int* blockPoseIndices, int numPoseBlocks, + int* blockLandmarkIndices, int numLandmarkBlocks, int totalDim); + + void deallocate(); + + SparseBlockMatrix* _Hpp; + SparseBlockMatrix* _Hll; + SparseBlockMatrix* _Hpl; + + SparseBlockMatrix* _Hschur; + SparseBlockMatrixDiagonal* _DInvSchur; + + SparseBlockMatrixCCS* _HplCCS; + SparseBlockMatrixCCS* _HschurTransposedCCS; + + LinearSolver* _linearSolver; + + std::vector > _diagonalBackupPose; + std::vector > _diagonalBackupLandmark; + +# ifdef G2O_OPENMP + std::vector _coefficientsMutex; +# endif + + bool _doSchur; + + double* _coefficients; + double* _bschur; + + int _numPoses, _numLandmarks; + int _sizePoses, _sizeLandmarks; + }; + + + //variable size solver + typedef BlockSolver< BlockSolverTraits > BlockSolverX; + // solver for BA/3D SLAM + typedef BlockSolver< BlockSolverTraits<6, 3> > BlockSolver_6_3; + // solver fo BA with scale + typedef BlockSolver< BlockSolverTraits<7, 3> > BlockSolver_7_3; + // 2Dof landmarks 3Dof poses + typedef BlockSolver< BlockSolverTraits<3, 2> > BlockSolver_3_2; + +} // end namespace + +#include "block_solver.hpp" + + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/block_solver.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/block_solver.hpp new file mode 100644 index 0000000..f8b032e --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/block_solver.hpp @@ -0,0 +1,634 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "sparse_optimizer.h" +#include +#include +#include + +#include "../stuff/timeutil.h" +#include "../stuff/macros.h" +#include "../stuff/misc.h" + +namespace g2o { + +using namespace std; +using namespace Eigen; + +template +BlockSolver::BlockSolver(LinearSolverType* linearSolver) : + BlockSolverBase(), + _linearSolver(linearSolver) +{ + // workspace + _Hpp=0; + _Hll=0; + _Hpl=0; + _HplCCS = 0; + _HschurTransposedCCS = 0; + _Hschur=0; + _DInvSchur=0; + _coefficients=0; + _bschur = 0; + _xSize=0; + _numPoses=0; + _numLandmarks=0; + _sizePoses=0; + _sizeLandmarks=0; + _doSchur=true; +} + +template +void BlockSolver::resize(int* blockPoseIndices, int numPoseBlocks, + int* blockLandmarkIndices, int numLandmarkBlocks, + int s) +{ + deallocate(); + + resizeVector(s); + + if (_doSchur) { + // the following two are only used in schur + assert(_sizePoses > 0 && "allocating with wrong size"); + _coefficients = new double [s]; + _bschur = new double[_sizePoses]; + } + + _Hpp=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks); + if (_doSchur) { + _Hschur=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks); + _Hll=new LandmarkHessianType(blockLandmarkIndices, blockLandmarkIndices, numLandmarkBlocks, numLandmarkBlocks); + _DInvSchur = new SparseBlockMatrixDiagonal(_Hll->colBlockIndices()); + _Hpl=new PoseLandmarkHessianType(blockPoseIndices, blockLandmarkIndices, numPoseBlocks, numLandmarkBlocks); + _HplCCS = new SparseBlockMatrixCCS(_Hpl->rowBlockIndices(), _Hpl->colBlockIndices()); + _HschurTransposedCCS = new SparseBlockMatrixCCS(_Hschur->colBlockIndices(), _Hschur->rowBlockIndices()); +#ifdef G2O_OPENMP + _coefficientsMutex.resize(numPoseBlocks); +#endif + } +} + +template +void BlockSolver::deallocate() +{ + if (_Hpp){ + delete _Hpp; + _Hpp=0; + } + if (_Hll){ + delete _Hll; + _Hll=0; + } + if (_Hpl){ + delete _Hpl; + _Hpl = 0; + } + if (_Hschur){ + delete _Hschur; + _Hschur=0; + } + if (_DInvSchur){ + delete _DInvSchur; + _DInvSchur=0; + } + if (_coefficients) { + delete[] _coefficients; + _coefficients = 0; + } + if (_bschur) { + delete[] _bschur; + _bschur = 0; + } + if (_HplCCS) { + delete _HplCCS; + _HplCCS = 0; + } + if (_HschurTransposedCCS) { + delete _HschurTransposedCCS; + _HschurTransposedCCS = 0; + } +} + +template +BlockSolver::~BlockSolver() +{ + delete _linearSolver; + deallocate(); +} + +template +bool BlockSolver::buildStructure(bool zeroBlocks) +{ + assert(_optimizer); + + size_t sparseDim = 0; + _numPoses=0; + _numLandmarks=0; + _sizePoses=0; + _sizeLandmarks=0; + int* blockPoseIndices = new int[_optimizer->indexMapping().size()]; + int* blockLandmarkIndices = new int[_optimizer->indexMapping().size()]; + + for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) { + OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i]; + int dim = v->dimension(); + if (! v->marginalized()){ + v->setColInHessian(_sizePoses); + _sizePoses+=dim; + blockPoseIndices[_numPoses]=_sizePoses; + ++_numPoses; + } else { + v->setColInHessian(_sizeLandmarks); + _sizeLandmarks+=dim; + blockLandmarkIndices[_numLandmarks]=_sizeLandmarks; + ++_numLandmarks; + } + sparseDim += dim; + } + resize(blockPoseIndices, _numPoses, blockLandmarkIndices, _numLandmarks, sparseDim); + delete[] blockLandmarkIndices; + delete[] blockPoseIndices; + + // allocate the diagonal on Hpp and Hll + int poseIdx = 0; + int landmarkIdx = 0; + for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) { + OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i]; + if (! v->marginalized()){ + //assert(poseIdx == v->hessianIndex()); + PoseMatrixType* m = _Hpp->block(poseIdx, poseIdx, true); + if (zeroBlocks) + m->setZero(); + v->mapHessianMemory(m->data()); + ++poseIdx; + } else { + LandmarkMatrixType* m = _Hll->block(landmarkIdx, landmarkIdx, true); + if (zeroBlocks) + m->setZero(); + v->mapHessianMemory(m->data()); + ++landmarkIdx; + } + } + assert(poseIdx == _numPoses && landmarkIdx == _numLandmarks); + + // temporary structures for building the pattern of the Schur complement + SparseBlockMatrixHashMap* schurMatrixLookup = 0; + if (_doSchur) { + schurMatrixLookup = new SparseBlockMatrixHashMap(_Hschur->rowBlockIndices(), _Hschur->colBlockIndices()); + schurMatrixLookup->blockCols().resize(_Hschur->blockCols().size()); + } + + // here we assume that the landmark indices start after the pose ones + // create the structure in Hpp, Hll and in Hpl + for (SparseOptimizer::EdgeContainer::const_iterator it=_optimizer->activeEdges().begin(); it!=_optimizer->activeEdges().end(); ++it){ + OptimizableGraph::Edge* e = *it; + + for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) { + OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx); + int ind1 = v1->hessianIndex(); + if (ind1 == -1) + continue; + int indexV1Bak = ind1; + for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) { + OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx); + int ind2 = v2->hessianIndex(); + if (ind2 == -1) + continue; + ind1 = indexV1Bak; + bool transposedBlock = ind1 > ind2; + if (transposedBlock){ // make sure, we allocate the upper triangle block + swap(ind1, ind2); + } + if (! v1->marginalized() && !v2->marginalized()){ + PoseMatrixType* m = _Hpp->block(ind1, ind2, true); + if (zeroBlocks) + m->setZero(); + e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock); + if (_Hschur) {// assume this is only needed in case we solve with the schur complement + schurMatrixLookup->addBlock(ind1, ind2); + } + } else if (v1->marginalized() && v2->marginalized()){ + // RAINER hmm.... should we ever reach this here???? + LandmarkMatrixType* m = _Hll->block(ind1-_numPoses, ind2-_numPoses, true); + if (zeroBlocks) + m->setZero(); + e->mapHessianMemory(m->data(), viIdx, vjIdx, false); + } else { + if (v1->marginalized()){ + PoseLandmarkMatrixType* m = _Hpl->block(v2->hessianIndex(),v1->hessianIndex()-_numPoses, true); + if (zeroBlocks) + m->setZero(); + e->mapHessianMemory(m->data(), viIdx, vjIdx, true); // transpose the block before writing to it + } else { + PoseLandmarkMatrixType* m = _Hpl->block(v1->hessianIndex(),v2->hessianIndex()-_numPoses, true); + if (zeroBlocks) + m->setZero(); + e->mapHessianMemory(m->data(), viIdx, vjIdx, false); // directly the block + } + } + } + } + } + + if (! _doSchur) + return true; + + _DInvSchur->diagonal().resize(landmarkIdx); + _Hpl->fillSparseBlockMatrixCCS(*_HplCCS); + + for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) { + OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i]; + if (v->marginalized()){ + const HyperGraph::EdgeSet& vedges=v->edges(); + for (HyperGraph::EdgeSet::const_iterator it1=vedges.begin(); it1!=vedges.end(); ++it1){ + for (size_t i=0; i<(*it1)->vertices().size(); ++i) + { + OptimizableGraph::Vertex* v1= (OptimizableGraph::Vertex*) (*it1)->vertex(i); + if (v1->hessianIndex()==-1 || v1==v) + continue; + for (HyperGraph::EdgeSet::const_iterator it2=vedges.begin(); it2!=vedges.end(); ++it2){ + for (size_t j=0; j<(*it2)->vertices().size(); ++j) + { + OptimizableGraph::Vertex* v2= (OptimizableGraph::Vertex*) (*it2)->vertex(j); + if (v2->hessianIndex()==-1 || v2==v) + continue; + int i1=v1->hessianIndex(); + int i2=v2->hessianIndex(); + if (i1<=i2) { + schurMatrixLookup->addBlock(i1, i2); + } + } + } + } + } + } + } + + _Hschur->takePatternFromHash(*schurMatrixLookup); + delete schurMatrixLookup; + _Hschur->fillSparseBlockMatrixCCSTransposed(*_HschurTransposedCCS); + + return true; +} + +template +bool BlockSolver::updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges) +{ + for (std::vector::const_iterator vit = vset.begin(); vit != vset.end(); ++vit) { + OptimizableGraph::Vertex* v = static_cast(*vit); + int dim = v->dimension(); + if (! v->marginalized()){ + v->setColInHessian(_sizePoses); + _sizePoses+=dim; + _Hpp->rowBlockIndices().push_back(_sizePoses); + _Hpp->colBlockIndices().push_back(_sizePoses); + _Hpp->blockCols().push_back(typename SparseBlockMatrix::IntBlockMap()); + ++_numPoses; + int ind = v->hessianIndex(); + PoseMatrixType* m = _Hpp->block(ind, ind, true); + v->mapHessianMemory(m->data()); + } else { + std::cerr << "updateStructure(): Schur not supported" << std::endl; + abort(); + } + } + resizeVector(_sizePoses + _sizeLandmarks); + + for (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) { + OptimizableGraph::Edge* e = static_cast(*it); + + for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) { + OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx); + int ind1 = v1->hessianIndex(); + int indexV1Bak = ind1; + if (ind1 == -1) + continue; + for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) { + OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx); + int ind2 = v2->hessianIndex(); + if (ind2 == -1) + continue; + ind1 = indexV1Bak; + bool transposedBlock = ind1 > ind2; + if (transposedBlock) // make sure, we allocate the upper triangular block + swap(ind1, ind2); + + if (! v1->marginalized() && !v2->marginalized()) { + PoseMatrixType* m = _Hpp->block(ind1, ind2, true); + e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock); + } else { + std::cerr << __PRETTY_FUNCTION__ << ": not supported" << std::endl; + } + } + } + + } + + return true; +} + +template +bool BlockSolver::solve(){ + //cerr << __PRETTY_FUNCTION__ << endl; + if (! _doSchur){ + double t=get_monotonic_time(); + bool ok = _linearSolver->solve(*_Hpp, _x, _b); + G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats(); + if (globalStats) { + globalStats->timeLinearSolver = get_monotonic_time() - t; + globalStats->hessianDimension = globalStats->hessianPoseDimension = _Hpp->cols(); + } + return ok; + } + + // schur thing + + // backup the coefficient matrix + double t=get_monotonic_time(); + + // _Hschur = _Hpp, but keeping the pattern of _Hschur + _Hschur->clear(); + _Hpp->add(_Hschur); + + //_DInvSchur->clear(); + memset (_coefficients, 0, _sizePoses*sizeof(double)); +# ifdef G2O_OPENMP +# pragma omp parallel for default (shared) schedule(dynamic, 10) +# endif + for (int landmarkIndex = 0; landmarkIndex < static_cast(_Hll->blockCols().size()); ++landmarkIndex) { + const typename SparseBlockMatrix::IntBlockMap& marginalizeColumn = _Hll->blockCols()[landmarkIndex]; + assert(marginalizeColumn.size() == 1 && "more than one block in _Hll column"); + + // calculate inverse block for the landmark + const LandmarkMatrixType * D = marginalizeColumn.begin()->second; + assert (D && D->rows()==D->cols() && "Error in landmark matrix"); + LandmarkMatrixType& Dinv = _DInvSchur->diagonal()[landmarkIndex]; + Dinv = D->inverse(); + + LandmarkVectorType db(D->rows()); + for (int j=0; jrows(); ++j) { + db[j]=_b[_Hll->rowBaseOfBlock(landmarkIndex) + _sizePoses + j]; + } + db=Dinv*db; + + assert((size_t)landmarkIndex < _HplCCS->blockCols().size() && "Index out of bounds"); + const typename SparseBlockMatrixCCS::SparseColumn& landmarkColumn = _HplCCS->blockCols()[landmarkIndex]; + + for (typename SparseBlockMatrixCCS::SparseColumn::const_iterator it_outer = landmarkColumn.begin(); + it_outer != landmarkColumn.end(); ++it_outer) { + int i1 = it_outer->row; + + const PoseLandmarkMatrixType* Bi = it_outer->block; + assert(Bi); + + PoseLandmarkMatrixType BDinv = (*Bi)*(Dinv); + assert(_HplCCS->rowBaseOfBlock(i1) < _sizePoses && "Index out of bounds"); + typename PoseVectorType::MapType Bb(&_coefficients[_HplCCS->rowBaseOfBlock(i1)], Bi->rows()); +# ifdef G2O_OPENMP + ScopedOpenMPMutex mutexLock(&_coefficientsMutex[i1]); +# endif + Bb.noalias() += (*Bi)*db; + + assert(i1 >= 0 && i1 < static_cast(_HschurTransposedCCS->blockCols().size()) && "Index out of bounds"); + typename SparseBlockMatrixCCS::SparseColumn::iterator targetColumnIt = _HschurTransposedCCS->blockCols()[i1].begin(); + + typename SparseBlockMatrixCCS::RowBlock aux(i1, 0); + typename SparseBlockMatrixCCS::SparseColumn::const_iterator it_inner = lower_bound(landmarkColumn.begin(), landmarkColumn.end(), aux); + for (; it_inner != landmarkColumn.end(); ++it_inner) { + int i2 = it_inner->row; + const PoseLandmarkMatrixType* Bj = it_inner->block; + assert(Bj); + while (targetColumnIt->row < i2 /*&& targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end()*/) + ++targetColumnIt; + assert(targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end() && targetColumnIt->row == i2 && "invalid iterator, something wrong with the matrix structure"); + PoseMatrixType* Hi1i2 = targetColumnIt->block;//_Hschur->block(i1,i2); + assert(Hi1i2); + (*Hi1i2).noalias() -= BDinv*Bj->transpose(); + } + } + } + //cerr << "Solve [marginalize] = " << get_monotonic_time()-t << endl; + + // _bschur = _b for calling solver, and not touching _b + memcpy(_bschur, _b, _sizePoses * sizeof(double)); + for (int i=0; i<_sizePoses; ++i){ + _bschur[i]-=_coefficients[i]; + } + + G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats(); + if (globalStats){ + globalStats->timeSchurComplement = get_monotonic_time() - t; + } + + t=get_monotonic_time(); + bool solvedPoses = _linearSolver->solve(*_Hschur, _x, _bschur); + if (globalStats) { + globalStats->timeLinearSolver = get_monotonic_time() - t; + globalStats->hessianPoseDimension = _Hpp->cols(); + globalStats->hessianLandmarkDimension = _Hll->cols(); + globalStats->hessianDimension = globalStats->hessianPoseDimension + globalStats->hessianLandmarkDimension; + } + //cerr << "Solve [decompose and solve] = " << get_monotonic_time()-t << endl; + + if (! solvedPoses) + return false; + + // _x contains the solution for the poses, now applying it to the landmarks to get the new part of the + // solution; + double* xp = _x; + double* cp = _coefficients; + + double* xl=_x+_sizePoses; + double* cl=_coefficients + _sizePoses; + double* bl=_b+_sizePoses; + + // cp = -xp + for (int i=0; i<_sizePoses; ++i) + cp[i]=-xp[i]; + + // cl = bl + memcpy(cl,bl,_sizeLandmarks*sizeof(double)); + + // cl = bl - Bt * xp + //Bt->multiply(cl, cp); + _HplCCS->rightMultiply(cl, cp); + + // xl = Dinv * cl + memset(xl,0, _sizeLandmarks*sizeof(double)); + _DInvSchur->multiply(xl,cl); + //_DInvSchur->rightMultiply(xl,cl); + //cerr << "Solve [landmark delta] = " << get_monotonic_time()-t << endl; + + return true; +} + + +template +bool BlockSolver::computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices) +{ + double t = get_monotonic_time(); + bool ok = _linearSolver->solvePattern(spinv, blockIndices, *_Hpp); + G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats(); + if (globalStats) { + globalStats->timeMarginals = get_monotonic_time() - t; + } + return ok; +} + +template +bool BlockSolver::buildSystem() +{ + // clear b vector +# ifdef G2O_OPENMP +# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000) +# endif + for (int i = 0; i < static_cast(_optimizer->indexMapping().size()); ++i) { + OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i]; + assert(v); + v->clearQuadraticForm(); + } + _Hpp->clear(); + if (_doSchur) { + _Hll->clear(); + _Hpl->clear(); + } + + // resetting the terms for the pairwise constraints + // built up the current system by storing the Hessian blocks in the edges and vertices +# ifndef G2O_OPENMP + // no threading, we do not need to copy the workspace + JacobianWorkspace& jacobianWorkspace = _optimizer->jacobianWorkspace(); +# else + // if running with threads need to produce copies of the workspace for each thread + JacobianWorkspace jacobianWorkspace = _optimizer->jacobianWorkspace(); +# pragma omp parallel for default (shared) firstprivate(jacobianWorkspace) if (_optimizer->activeEdges().size() > 100) +# endif + for (int k = 0; k < static_cast(_optimizer->activeEdges().size()); ++k) { + OptimizableGraph::Edge* e = _optimizer->activeEdges()[k]; + e->linearizeOplus(jacobianWorkspace); // jacobian of the nodes' oplus (manifold) + e->constructQuadraticForm(); +# ifndef NDEBUG + for (size_t i = 0; i < e->vertices().size(); ++i) { + const OptimizableGraph::Vertex* v = static_cast(e->vertex(i)); + if (! v->fixed()) { + bool hasANan = arrayHasNaN(jacobianWorkspace.workspaceForVertex(i), e->dimension() * v->dimension()); + if (hasANan) { + cerr << "buildSystem(): NaN within Jacobian for edge " << e << " for vertex " << i << endl; + break; + } + } + } +# endif + } + + // flush the current system in a sparse block matrix +# ifdef G2O_OPENMP +# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000) +# endif + for (int i = 0; i < static_cast(_optimizer->indexMapping().size()); ++i) { + OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i]; + int iBase = v->colInHessian(); + if (v->marginalized()) + iBase+=_sizePoses; + v->copyB(_b+iBase); + } + + return 0; +} + + +template +bool BlockSolver::setLambda(double lambda, bool backup) +{ + if (backup) { + _diagonalBackupPose.resize(_numPoses); + _diagonalBackupLandmark.resize(_numLandmarks); + } +# ifdef G2O_OPENMP +# pragma omp parallel for default (shared) if (_numPoses > 100) +# endif + for (int i = 0; i < _numPoses; ++i) { + PoseMatrixType *b=_Hpp->block(i,i); + if (backup) + _diagonalBackupPose[i] = b->diagonal(); + b->diagonal().array() += lambda; + } +# ifdef G2O_OPENMP +# pragma omp parallel for default (shared) if (_numLandmarks > 100) +# endif + for (int i = 0; i < _numLandmarks; ++i) { + LandmarkMatrixType *b=_Hll->block(i,i); + if (backup) + _diagonalBackupLandmark[i] = b->diagonal(); + b->diagonal().array() += lambda; + } + return true; +} + +template +void BlockSolver::restoreDiagonal() +{ + assert((int) _diagonalBackupPose.size() == _numPoses && "Mismatch in dimensions"); + assert((int) _diagonalBackupLandmark.size() == _numLandmarks && "Mismatch in dimensions"); + for (int i = 0; i < _numPoses; ++i) { + PoseMatrixType *b=_Hpp->block(i,i); + b->diagonal() = _diagonalBackupPose[i]; + } + for (int i = 0; i < _numLandmarks; ++i) { + LandmarkMatrixType *b=_Hll->block(i,i); + b->diagonal() = _diagonalBackupLandmark[i]; + } +} + +template +bool BlockSolver::init(SparseOptimizer* optimizer, bool online) +{ + _optimizer = optimizer; + if (! online) { + if (_Hpp) + _Hpp->clear(); + if (_Hpl) + _Hpl->clear(); + if (_Hll) + _Hll->clear(); + } + _linearSolver->init(); + return true; +} + +template +void BlockSolver::setWriteDebug(bool writeDebug) +{ + _linearSolver->setWriteDebug(writeDebug); +} + +template +bool BlockSolver::saveHessian(const std::string& fileName) const +{ + return _Hpp->writeOctave(fileName.c_str(), true); +} + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/cache.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/cache.cpp new file mode 100644 index 0000000..89f9c24 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/cache.cpp @@ -0,0 +1,183 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 G. Grisetti, R. Kuemmerle, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "cache.h" +#include "optimizable_graph.h" +#include "factory.h" + +#include + +namespace g2o { + using namespace std; + + Cache::CacheKey::CacheKey() : + _type(), _parameters() + { + } + + Cache::CacheKey::CacheKey(const std::string& type_, const ParameterVector& parameters_) : + _type(type_), _parameters(parameters_) + { + } + + Cache::Cache(CacheContainer* container_, const ParameterVector& parameters_) : + _updateNeeded(true), _parameters(parameters_), _container(container_) + { + } + + bool Cache::CacheKey::operator<(const Cache::CacheKey& c) const{ + if (_type < c._type) + return true; + return std::lexicographical_compare (_parameters.begin( ), _parameters.end( ), + c._parameters.begin( ), c._parameters.end( ) ); + } + + + OptimizableGraph::Vertex* Cache::vertex() { + if (container() ) + return container()->vertex(); + return 0; + } + + OptimizableGraph* Cache::graph() { + if (container()) + return container()->graph(); + return 0; + } + + CacheContainer* Cache::container() { + return _container; + } + + ParameterVector& Cache::parameters() { + return _parameters; + } + + Cache::CacheKey Cache::key() const { + Factory* factory=Factory::instance(); + return CacheKey(factory->tag(this), _parameters); + }; + + + void Cache::update(){ + if (! _updateNeeded) + return; + for(std::vector::iterator it=_parentCaches.begin(); it!=_parentCaches.end(); it++){ + (*it)->update(); + } + updateImpl(); + _updateNeeded=false; + } + + Cache* Cache::installDependency(const std::string& type_, const std::vector& parameterIndices){ + ParameterVector pv(parameterIndices.size()); + for (size_t i=0; i=(int)_parameters.size()) + return 0; + pv[i]=_parameters[ parameterIndices[i] ]; + } + CacheKey k(type_, pv); + if (!container()) + return 0; + Cache* c=container()->findCache(k); + if (!c) { + c = container()->createCache(k); + } + if (c) + _parentCaches.push_back(c); + return c; + } + + bool Cache::resolveDependancies(){ + return true; + } + + CacheContainer::CacheContainer(OptimizableGraph::Vertex* vertex_) { + _vertex = vertex_; + } + + Cache* CacheContainer::findCache(const Cache::CacheKey& key) { + iterator it=find(key); + if (it==end()) + return 0; + return it->second; + } + + Cache* CacheContainer::createCache(const Cache::CacheKey& key){ + Factory* f = Factory::instance(); + HyperGraph::HyperGraphElement* e = f->construct(key.type()); + if (!e) { + cerr << __PRETTY_FUNCTION__ << endl; + cerr << "fatal error in creating cache of type " << key.type() << endl; + return 0; + } + Cache* c = dynamic_cast(e); + if (! c){ + cerr << __PRETTY_FUNCTION__ << endl; + cerr << "fatal error in creating cache of type " << key.type() << endl; + return 0; + } + c->_container = this; + c->_parameters = key._parameters; + if (c->resolveDependancies()){ + insert(make_pair(key,c)); + c->update(); + return c; + } + return 0; + } + + OptimizableGraph::Vertex* CacheContainer::vertex() { + return _vertex; + } + + OptimizableGraph* CacheContainer::graph(){ + if (_vertex) + return _vertex->graph(); + return 0; + } + + void CacheContainer::update() { + for (iterator it=begin(); it!=end(); it++){ + (it->second)->update(); + } + _updateNeeded=false; + } + + void CacheContainer::setUpdateNeeded(bool needUpdate) { + _updateNeeded=needUpdate; + for (iterator it=begin(); it!=end(); ++it){ + (it->second)->_updateNeeded = needUpdate; + } + } + + CacheContainer::~CacheContainer(){ + for (iterator it=begin(); it!=end(); ++it){ + delete (it->second); + } + } + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/cache.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/cache.h new file mode 100644 index 0000000..c5b00a4 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/cache.h @@ -0,0 +1,140 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_CACHE_HH_ +#define G2O_CACHE_HH_ + +#include + +#include "optimizable_graph.h" + +namespace g2o { + + class CacheContainer; + + class Cache: public HyperGraph::HyperGraphElement + { + public: + friend class CacheContainer; + class CacheKey + { + public: + friend class CacheContainer; + CacheKey(); + CacheKey(const std::string& type_, const ParameterVector& parameters_); + + bool operator<(const CacheKey& c) const; + + const std::string& type() const { return _type;} + const ParameterVector& parameters() const { return _parameters;} + + protected: + std::string _type; + ParameterVector _parameters; + }; + + Cache(CacheContainer* container_ = 0, const ParameterVector& parameters_ = ParameterVector()); + + CacheKey key() const; + + OptimizableGraph::Vertex* vertex(); + OptimizableGraph* graph(); + CacheContainer* container(); + ParameterVector& parameters(); + + void update(); + + virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_CACHE;} + + protected: + //! redefine this to do the update + virtual void updateImpl() = 0; + + /** + * this function installs and satisfies a cache + * @param type_: the typename of the dependency + * @param parameterIndices: a vector containing the indices if the parameters + * in _parameters that will be used to assemble the Key of the cache being created + * For example if I have a cache of type C2, having parameters "A, B, and C", + * and it depends on a cache of type C1 that depends on the parameters A and C, + * the parameterIndices should contain "0, 2", since they are the positions in the + * parameter vector of C2 of the parameters needed to construct C1. + * @returns the newly created cache + */ + Cache* installDependency(const std::string& type_, const std::vector& parameterIndices); + + /** + * Function to be called from a cache that has dependencies. It just invokes a + * sequence of installDependency(). + * Although the caches returned are stored in the _parentCache vector, + * it is better that you redefine your own cache member variables, for better readability + */ + virtual bool resolveDependancies(); + + bool _updateNeeded; + ParameterVector _parameters; + std::vector _parentCaches; + CacheContainer* _container; + }; + + class CacheContainer: public std::map + { + public: + CacheContainer(OptimizableGraph::Vertex* vertex_); + virtual ~CacheContainer(); + OptimizableGraph::Vertex* vertex(); + OptimizableGraph* graph(); + Cache* findCache(const Cache::CacheKey& key); + Cache* createCache(const Cache::CacheKey& key); + void setUpdateNeeded(bool needUpdate=true); + void update(); + protected: + OptimizableGraph::Vertex* _vertex; + bool _updateNeeded; + }; + + + template + void OptimizableGraph::Edge::resolveCache(CacheType*& cache, + OptimizableGraph::Vertex* v, + const std::string& type_, + const ParameterVector& parameters_) + { + cache = 0; + CacheContainer* container= v->cacheContainer(); + Cache::CacheKey key(type_, parameters_); + Cache* c = container->findCache(key); + if (!c) { + c = container->createCache(key); + } + if (c) { + cache = dynamic_cast(c); + } + } + +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/creators.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/creators.h new file mode 100644 index 0000000..9ca9967 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/creators.h @@ -0,0 +1,75 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_CREATORS_H +#define G2O_CREATORS_H + +#include "hyper_graph.h" + +#include +#include + +namespace g2o +{ + + /** + * \brief Abstract interface for allocating HyperGraphElement + */ + class AbstractHyperGraphElementCreator + { + public: + /** + * create a hyper graph element. Has to implemented in derived class. + */ + virtual HyperGraph::HyperGraphElement* construct() = 0; + /** + * name of the class to be created. Has to implemented in derived class. + */ + virtual const std::string& name() const = 0; + + virtual ~AbstractHyperGraphElementCreator() { } + }; + + /** + * \brief templatized creator class which creates graph elements + */ + template + class HyperGraphElementCreator : public AbstractHyperGraphElementCreator + { + public: + HyperGraphElementCreator() : _name(typeid(T).name()) {} +#if defined (WINDOWS) && defined(__GNUC__) // force stack alignment on Windows with GCC + __attribute__((force_align_arg_pointer)) +#endif + HyperGraph::HyperGraphElement* construct() { return new T;} + virtual const std::string& name() const { return _name;} + protected: + std::string _name; + }; + +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/eigen_types.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/eigen_types.h new file mode 100644 index 0000000..f2c6856 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/eigen_types.h @@ -0,0 +1,73 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_EIGEN_TYPES_H +#define G2O_EIGEN_TYPES_H + +#include +#include + +namespace g2o { + + typedef Eigen::Matrix Vector2I; + typedef Eigen::Matrix Vector3I; + typedef Eigen::Matrix Vector4I; + typedef Eigen::Matrix VectorXI; + + typedef Eigen::Matrix Vector2F; + typedef Eigen::Matrix Vector3F; + typedef Eigen::Matrix Vector4F; + typedef Eigen::Matrix VectorXF; + + typedef Eigen::Matrix Vector2D; + typedef Eigen::Matrix Vector3D; + typedef Eigen::Matrix Vector4D; + typedef Eigen::Matrix VectorXD; + + typedef Eigen::Matrix Matrix2I; + typedef Eigen::Matrix Matrix3I; + typedef Eigen::Matrix Matrix4I; + typedef Eigen::Matrix MatrixXI; + + typedef Eigen::Matrix Matrix2F; + typedef Eigen::Matrix Matrix3F; + typedef Eigen::Matrix Matrix4F; + typedef Eigen::Matrix MatrixXF; + + typedef Eigen::Matrix Matrix2D; + typedef Eigen::Matrix Matrix3D; + typedef Eigen::Matrix Matrix4D; + typedef Eigen::Matrix MatrixXD; + + typedef Eigen::Transform Isometry2D; + typedef Eigen::Transform Isometry3D; + + typedef Eigen::Transform Affine2D; + typedef Eigen::Transform Affine3D; + +} // end namespace g2o + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/estimate_propagator.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/estimate_propagator.cpp new file mode 100644 index 0000000..010dac1 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/estimate_propagator.cpp @@ -0,0 +1,267 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "estimate_propagator.h" + +#include +#include +#include +#include +#include +#include + +//#define DEBUG_ESTIMATE_PROPAGATOR + +using namespace std; + +namespace g2o { + +# ifdef DEBUG_ESTIMATE_PROPAGATOR + struct FrontierLevelCmp { + bool operator()(EstimatePropagator::AdjacencyMapEntry* e1, EstimatePropagator::AdjacencyMapEntry* e2) const + { + return e1->frontierLevel() < e2->frontierLevel(); + } + }; +# endif + + EstimatePropagator::AdjacencyMapEntry::AdjacencyMapEntry() + { + reset(); + } + + void EstimatePropagator::AdjacencyMapEntry::reset() + { + _child = 0; + _parent.clear(); + _edge = 0; + _distance = numeric_limits::max(); + _frontierLevel = -1; + inQueue = false; + } + + EstimatePropagator::EstimatePropagator(OptimizableGraph* g): _graph(g) + { + for (OptimizableGraph::VertexIDMap::const_iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); ++it){ + AdjacencyMapEntry entry; + entry._child = static_cast(it->second); + _adjacencyMap.insert(make_pair(entry.child(), entry)); + } + } + + void EstimatePropagator::reset() + { + for (OptimizableGraph::VertexSet::iterator it=_visited.begin(); it!=_visited.end(); ++it){ + OptimizableGraph::Vertex* v = static_cast(*it); + AdjacencyMap::iterator at = _adjacencyMap.find(v); + assert(at != _adjacencyMap.end()); + at->second.reset(); + } + _visited.clear(); + } + + void EstimatePropagator::propagate(OptimizableGraph::Vertex* v, + const EstimatePropagator::PropagateCost& cost, + const EstimatePropagator::PropagateAction& action, + double maxDistance, + double maxEdgeCost) + { + OptimizableGraph::VertexSet vset; + vset.insert(v); + propagate(vset, cost, action, maxDistance, maxEdgeCost); + } + + void EstimatePropagator::propagate(OptimizableGraph::VertexSet& vset, + const EstimatePropagator::PropagateCost& cost, + const EstimatePropagator::PropagateAction& action, + double maxDistance, + double maxEdgeCost) + { + reset(); + + PriorityQueue frontier; + for (OptimizableGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){ + OptimizableGraph::Vertex* v = static_cast(*vit); + AdjacencyMap::iterator it = _adjacencyMap.find(v); + assert(it != _adjacencyMap.end()); + it->second._distance = 0.; + it->second._parent.clear(); + it->second._frontierLevel = 0; + frontier.push(&it->second); + } + + while(! frontier.empty()){ + AdjacencyMapEntry* entry = frontier.pop(); + OptimizableGraph::Vertex* u = entry->child(); + double uDistance = entry->distance(); + //cerr << "uDistance " << uDistance << endl; + + // initialize the vertex + if (entry->_frontierLevel > 0) { + action(entry->edge(), entry->parent(), u); + } + + /* std::pair< OptimizableGraph::VertexSet::iterator, bool> insertResult = */ _visited.insert(u); + OptimizableGraph::EdgeSet::iterator et = u->edges().begin(); + while (et != u->edges().end()){ + OptimizableGraph::Edge* edge = static_cast(*et); + ++et; + + int maxFrontier = -1; + OptimizableGraph::VertexSet initializedVertices; + for (size_t i = 0; i < edge->vertices().size(); ++i) { + OptimizableGraph::Vertex* z = static_cast(edge->vertex(i)); + AdjacencyMap::iterator ot = _adjacencyMap.find(z); + if (ot->second._distance != numeric_limits::max()) { + initializedVertices.insert(z); + maxFrontier = (max)(maxFrontier, ot->second._frontierLevel); + } + } + assert(maxFrontier >= 0); + + for (size_t i = 0; i < edge->vertices().size(); ++i) { + OptimizableGraph::Vertex* z = static_cast(edge->vertex(i)); + if (z == u) + continue; + + size_t wasInitialized = initializedVertices.erase(z); + + double edgeDistance = cost(edge, initializedVertices, z); + if (edgeDistance > 0. && edgeDistance != std::numeric_limits::max() && edgeDistance < maxEdgeCost) { + double zDistance = uDistance + edgeDistance; + //cerr << z->id() << " " << zDistance << endl; + + AdjacencyMap::iterator ot = _adjacencyMap.find(z); + assert(ot!=_adjacencyMap.end()); + + if (zDistance < ot->second.distance() && zDistance < maxDistance){ + //if (ot->second.inQueue) + //cerr << "Updating" << endl; + ot->second._distance = zDistance; + ot->second._parent = initializedVertices; + ot->second._edge = edge; + ot->second._frontierLevel = maxFrontier + 1; + frontier.push(&ot->second); + } + } + + if (wasInitialized > 0) + initializedVertices.insert(z); + + } + } + } + + // writing debug information like cost for reaching each vertex and the parent used to initialize +#ifdef DEBUG_ESTIMATE_PROPAGATOR + cerr << "Writing cost.dat" << endl; + ofstream costStream("cost.dat"); + for (AdjacencyMap::const_iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) { + HyperGraph::Vertex* u = it->second.child(); + costStream << "vertex " << u->id() << " cost " << it->second._distance << endl; + } + cerr << "Writing init.dat" << endl; + ofstream initStream("init.dat"); + vector frontierLevels; + for (AdjacencyMap::iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) { + if (it->second._frontierLevel > 0) + frontierLevels.push_back(&it->second); + } + sort(frontierLevels.begin(), frontierLevels.end(), FrontierLevelCmp()); + for (vector::const_iterator it = frontierLevels.begin(); it != frontierLevels.end(); ++it) { + AdjacencyMapEntry* entry = *it; + OptimizableGraph::Vertex* to = entry->child(); + + initStream << "calling init level = " << entry->_frontierLevel << "\t ("; + for (OptimizableGraph::VertexSet::iterator pit = entry->parent().begin(); pit != entry->parent().end(); ++pit) { + initStream << " " << (*pit)->id(); + } + initStream << " ) -> " << to->id() << endl; + } +#endif + + } + + void EstimatePropagator::PriorityQueue::push(AdjacencyMapEntry* entry) + { + assert(entry != NULL); + if (entry->inQueue) { + assert(entry->queueIt->second == entry); + erase(entry->queueIt); + } + + entry->queueIt = insert(std::make_pair(entry->distance(), entry)); + assert(entry->queueIt != end()); + entry->inQueue = true; + } + + EstimatePropagator::AdjacencyMapEntry* EstimatePropagator::PriorityQueue::pop() + { + assert(!empty()); + iterator it = begin(); + AdjacencyMapEntry* entry = it->second; + erase(it); + + assert(entry != NULL); + entry->queueIt = end(); + entry->inQueue = false; + return entry; + } + + EstimatePropagatorCost::EstimatePropagatorCost (SparseOptimizer* graph) : + _graph(graph) + { + } + + double EstimatePropagatorCost::operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to_) const + { + OptimizableGraph::Edge* e = dynamic_cast(edge); + OptimizableGraph::Vertex* to = dynamic_cast(to_); + SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e); + if (it == _graph->activeEdges().end()) // it has to be an active edge + return std::numeric_limits::max(); + return e->initialEstimatePossible(from, to); + } + + EstimatePropagatorCostOdometry::EstimatePropagatorCostOdometry(SparseOptimizer* graph) : + EstimatePropagatorCost(graph) + { + } + + double EstimatePropagatorCostOdometry::operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* to_) const + { + OptimizableGraph::Edge* e = dynamic_cast(edge); + OptimizableGraph::Vertex* from = dynamic_cast(*from_.begin()); + OptimizableGraph::Vertex* to = dynamic_cast(to_); + if (std::abs(from->id() - to->id()) != 1) // simple method to identify odometry edges in a pose graph + return std::numeric_limits::max(); + SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e); + if (it == _graph->activeEdges().end()) // it has to be an active edge + return std::numeric_limits::max(); + return e->initialEstimatePossible(from_, to); + } + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/estimate_propagator.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/estimate_propagator.h new file mode 100644 index 0000000..6a16d11 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/estimate_propagator.h @@ -0,0 +1,175 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_ESTIMATE_PROPAGATOR_H +#define G2O_ESTIMATE_PROPAGATOR_H + +#include "optimizable_graph.h" +#include "sparse_optimizer.h" + +#include +#include +#include + +#ifdef _MSC_VER +#include +#else +#include +#endif + +namespace g2o { + + /** + * \brief cost for traversing along active edges in the optimizer + * + * You may derive an own one, if necessary. The default is to return initialEstimatePossible(from, to) for the edge. + */ + class EstimatePropagatorCost { + public: + EstimatePropagatorCost (SparseOptimizer* graph); + virtual double operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to_) const; + virtual const char* name() const { return "spanning tree";} + protected: + SparseOptimizer* _graph; + }; + + /** + * \brief cost for traversing only odometry edges. + * + * Initialize your graph along odometry edges. An odometry edge is assumed to connect vertices + * whose IDs only differs by one. + */ + class EstimatePropagatorCostOdometry : public EstimatePropagatorCost { + public: + EstimatePropagatorCostOdometry(SparseOptimizer* graph); + virtual double operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* to_) const; + virtual const char* name() const { return "odometry";} + }; + + /** + * \brief propagation of an initial guess + */ + class EstimatePropagator { + public: + + /** + * \brief Applying the action for propagating. + * + * You may derive an own one, if necessary. The default is to call initialEstimate(from, to) for the edge. + */ + struct PropagateAction { + virtual void operator()(OptimizableGraph::Edge* e, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) const + { + if (! to->fixed()) + e->initialEstimate(from, to); + } + }; + + typedef EstimatePropagatorCost PropagateCost; + + class AdjacencyMapEntry; + + /** + * \brief priority queue for AdjacencyMapEntry + */ + class PriorityQueue : public std::multimap { + public: + void push(AdjacencyMapEntry* entry); + AdjacencyMapEntry* pop(); + }; + + /** + * \brief data structure for loopuk during Dijkstra + */ + class AdjacencyMapEntry { + public: + friend class EstimatePropagator; + friend class PriorityQueue; + AdjacencyMapEntry(); + void reset(); + OptimizableGraph::Vertex* child() const {return _child;} + const OptimizableGraph::VertexSet& parent() const {return _parent;} + OptimizableGraph::Edge* edge() const {return _edge;} + double distance() const {return _distance;} + int frontierLevel() const { return _frontierLevel;} + + protected: + OptimizableGraph::Vertex* _child; + OptimizableGraph::VertexSet _parent; + OptimizableGraph::Edge* _edge; + double _distance; + int _frontierLevel; + private: // for PriorityQueue + bool inQueue; + PriorityQueue::iterator queueIt; + }; + + /** + * \brief hash function for a vertex + */ + class VertexIDHashFunction { + public: + size_t operator ()(const OptimizableGraph::Vertex* v) const { return v->id();} + }; + + typedef std::tr1::unordered_map AdjacencyMap; + + public: + EstimatePropagator(OptimizableGraph* g); + OptimizableGraph::VertexSet& visited() {return _visited; } + AdjacencyMap& adjacencyMap() {return _adjacencyMap; } + OptimizableGraph* graph() {return _graph;} + + /** + * propagate an initial guess starting from v. The function computes a spanning tree + * whereas the cost for each edge is determined by calling cost() and the action applied to + * each vertex is action(). + */ + void propagate(OptimizableGraph::Vertex* v, + const EstimatePropagator::PropagateCost& cost, + const EstimatePropagator::PropagateAction& action = PropagateAction(), + double maxDistance=std::numeric_limits::max(), + double maxEdgeCost=std::numeric_limits::max()); + + /** + * same as above but starting to propagate from a set of vertices instead of just a single one. + */ + void propagate(OptimizableGraph::VertexSet& vset, + const EstimatePropagator::PropagateCost& cost, + const EstimatePropagator::PropagateAction& action = PropagateAction(), + double maxDistance=std::numeric_limits::max(), + double maxEdgeCost=std::numeric_limits::max()); + + protected: + void reset(); + + AdjacencyMap _adjacencyMap; + OptimizableGraph::VertexSet _visited; + OptimizableGraph* _graph; + }; + +} +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/factory.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/factory.cpp new file mode 100644 index 0000000..6263ae2 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/factory.cpp @@ -0,0 +1,217 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "factory.h" + +#include "creators.h" +#include "parameter.h" +#include "cache.h" +#include "optimizable_graph.h" +#include "../stuff/color_macros.h" + +#include +#include +#include + +using namespace std; + +namespace g2o { + +Factory* Factory::factoryInstance = 0; + +Factory::Factory() +{ +} + +Factory::~Factory() +{ +# ifdef G2O_DEBUG_FACTORY + cerr << "# Factory destroying " << (void*)this << endl; +# endif + for (CreatorMap::iterator it = _creator.begin(); it != _creator.end(); ++it) { + delete it->second->creator; + } + _creator.clear(); + _tagLookup.clear(); +} + +Factory* Factory::instance() +{ + if (factoryInstance == 0) { + factoryInstance = new Factory; +# ifdef G2O_DEBUG_FACTORY + cerr << "# Factory allocated " << (void*)factoryInstance << endl; +# endif + } + + return factoryInstance; +} + +void Factory::registerType(const std::string& tag, AbstractHyperGraphElementCreator* c) +{ + CreatorMap::const_iterator foundIt = _creator.find(tag); + if (foundIt != _creator.end()) { + cerr << "FACTORY WARNING: Overwriting Vertex tag " << tag << endl; + assert(0); + } + TagLookup::const_iterator tagIt = _tagLookup.find(c->name()); + if (tagIt != _tagLookup.end()) { + cerr << "FACTORY WARNING: Registering same class for two tags " << c->name() << endl; + assert(0); + } + + CreatorInformation* ci = new CreatorInformation(); + ci->creator = c; + +#ifdef G2O_DEBUG_FACTORY + cerr << "# Factory " << (void*)this << " constructing type " << tag << " "; +#endif + // construct an element once to figure out its type + HyperGraph::HyperGraphElement* element = c->construct(); + ci->elementTypeBit = element->elementType(); + +#ifdef G2O_DEBUG_FACTORY + cerr << "done." << endl; + cerr << "# Factory " << (void*)this << " registering " << tag; + cerr << " " << (void*) c << " "; + switch (element->elementType()) { + case HyperGraph::HGET_VERTEX: + cerr << " -> Vertex"; + break; + case HyperGraph::HGET_EDGE: + cerr << " -> Edge"; + break; + case HyperGraph::HGET_PARAMETER: + cerr << " -> Parameter"; + break; + case HyperGraph::HGET_CACHE: + cerr << " -> Cache"; + break; + case HyperGraph::HGET_DATA: + cerr << " -> Data"; + break; + default: + assert(0 && "Unknown element type occured, fix elementTypes"); + break; + } + cerr << endl; +#endif + + _creator[tag] = ci; + _tagLookup[c->name()] = tag; + delete element; +} + + void Factory::unregisterType(const std::string& tag) + { + // Look for the tag + CreatorMap::iterator tagPosition = _creator.find(tag); + + if (tagPosition != _creator.end()) { + + AbstractHyperGraphElementCreator* c = tagPosition->second->creator; + + // If we found it, remove the creator from the tag lookup map + TagLookup::iterator classPosition = _tagLookup.find(c->name()); + if (classPosition != _tagLookup.end()) + { + _tagLookup.erase(classPosition); + } + _creator.erase(tagPosition); + } + } + +HyperGraph::HyperGraphElement* Factory::construct(const std::string& tag) const +{ + CreatorMap::const_iterator foundIt = _creator.find(tag); + if (foundIt != _creator.end()) { + //cerr << "tag " << tag << " -> " << (void*) foundIt->second->creator << " " << foundIt->second->creator->name() << endl; + return foundIt->second->creator->construct(); + } + return 0; +} + +const std::string& Factory::tag(const HyperGraph::HyperGraphElement* e) const +{ + static std::string emptyStr(""); + TagLookup::const_iterator foundIt = _tagLookup.find(typeid(*e).name()); + if (foundIt != _tagLookup.end()) + return foundIt->second; + return emptyStr; +} + +void Factory::fillKnownTypes(std::vector& types) const +{ + types.clear(); + for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it) + types.push_back(it->first); +} + +bool Factory::knowsTag(const std::string& tag, int* elementType) const +{ + CreatorMap::const_iterator foundIt = _creator.find(tag); + if (foundIt == _creator.end()) { + if (elementType) + *elementType = -1; + return false; + } + if (elementType) + *elementType = foundIt->second->elementTypeBit; + return true; +} + +void Factory::destroy() +{ + delete factoryInstance; + factoryInstance = 0; +} + +void Factory::printRegisteredTypes(std::ostream& os, bool comment) const +{ + if (comment) + os << "# "; + os << "types:" << endl; + for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it) { + if (comment) + os << "#"; + cerr << "\t" << it->first << endl; + } +} + +HyperGraph::HyperGraphElement* Factory::construct(const std::string& tag, const HyperGraph::GraphElemBitset& elemsToConstruct) const +{ + if (elemsToConstruct.none()) { + return construct(tag); + } + CreatorMap::const_iterator foundIt = _creator.find(tag); + if (foundIt != _creator.end() && foundIt->second->elementTypeBit >= 0 && elemsToConstruct.test(foundIt->second->elementTypeBit)) { + return foundIt->second->creator->construct(); + } + return 0; +} + +} // end namespace + diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/factory.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/factory.h new file mode 100644 index 0000000..0e189ae --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/factory.h @@ -0,0 +1,179 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_FACTORY_H +#define G2O_FACTORY_H + +#include "../../config.h" +#include "../stuff/misc.h" +#include "hyper_graph.h" +#include "creators.h" + +#include +#include +#include + +// define to get some verbose output +//#define G2O_DEBUG_FACTORY + +namespace g2o { + + class AbstractHyperGraphElementCreator; + + /** + * \brief create vertices and edges based on TAGs in, for example, a file + */ + class Factory + { + public: + + //! return the instance + static Factory* instance(); + + //! free the instance + static void destroy(); + + /** + * register a tag for a specific creator + */ + void registerType(const std::string& tag, AbstractHyperGraphElementCreator* c); + + /** + * unregister a tag for a specific creator + */ + void unregisterType(const std::string& tag); + + /** + * construct a graph element based on its tag + */ + HyperGraph::HyperGraphElement* construct(const std::string& tag) const; + + /** + * construct a graph element based on its tag, but only if it's type (a bitmask) matches. A bitmask without any + * bit set will construct any item. Otherwise a bit has to be set to allow construction of a graph element. + */ + HyperGraph::HyperGraphElement* construct(const std::string& tag, const HyperGraph::GraphElemBitset& elemsToConstruct) const; + + /** + * return whether the factory knows this tag or not + */ + bool knowsTag(const std::string& tag, int* elementType = 0) const; + + //! return the TAG given a vertex + const std::string& tag(const HyperGraph::HyperGraphElement* v) const; + + /** + * get a list of all known types + */ + void fillKnownTypes(std::vector& types) const; + + /** + * print a list of the known registered types to the given stream + */ + void printRegisteredTypes(std::ostream& os, bool comment = false) const; + + protected: + class CreatorInformation + { + public: + AbstractHyperGraphElementCreator* creator; + int elementTypeBit; + CreatorInformation() + { + creator = 0; + elementTypeBit = -1; + } + + ~CreatorInformation() + { + std::cout << "Deleting " << (void*) creator << std::endl; + + delete creator; + } + }; + + typedef std::map CreatorMap; + typedef std::map TagLookup; + Factory(); + ~Factory(); + + CreatorMap _creator; ///< look-up map for the existing creators + TagLookup _tagLookup; ///< reverse look-up, class name to tag + + private: + static Factory* factoryInstance; + }; + + template + class RegisterTypeProxy + { + public: + RegisterTypeProxy(const std::string& name) : _name(name) + { +#ifdef G2O_DEBUG_FACTORY + std::cout << __FUNCTION__ << ": Registering " << _name << " of type " << typeid(T).name() << std::endl; +#endif + Factory::instance()->registerType(_name, new HyperGraphElementCreator()); + } + + ~RegisterTypeProxy() + { +#ifdef G2O_DEBUG_FACTORY + std::cout << __FUNCTION__ << ": Unregistering " << _name << " of type " << typeid(T).name() << std::endl; +#endif + Factory::instance()->unregisterType(_name); + } + + private: + std::string _name; + }; + +#if defined _MSC_VER && defined G2O_SHARED_LIBS +# define G2O_FACTORY_EXPORT __declspec(dllexport) +# define G2O_FACTORY_IMPORT __declspec(dllimport) +#else +# define G2O_FACTORY_EXPORT +# define G2O_FACTORY_IMPORT +#endif + + // These macros are used to automate registering types and forcing linkage +#define G2O_REGISTER_TYPE(name, classname) \ + extern "C" void G2O_FACTORY_EXPORT g2o_type_##classname(void) {} \ + static g2o::RegisterTypeProxy g_type_proxy_##classname(#name); + +#define G2O_USE_TYPE_BY_CLASS_NAME(classname) \ + extern "C" void G2O_FACTORY_IMPORT g2o_type_##classname(void); \ + static g2o::ForceLinker proxy_##classname(g2o_type_##classname); + +#define G2O_REGISTER_TYPE_GROUP(typeGroupName) \ + extern "C" void G2O_FACTORY_EXPORT g2o_type_group_##typeGroupName(void) {} + +#define G2O_USE_TYPE_GROUP(typeGroupName) \ + extern "C" void G2O_FACTORY_IMPORT g2o_type_group_##typeGroupName(void); \ + static g2o::ForceLinker g2o_force_type_link_##typeGroupName(g2o_type_group_##typeGroupName); +} + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/hyper_dijkstra.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/hyper_dijkstra.cpp new file mode 100644 index 0000000..a54eca5 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/hyper_dijkstra.cpp @@ -0,0 +1,261 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include +#include +#include +#include +#include +#include "hyper_dijkstra.h" +#include "../stuff/macros.h" + +namespace g2o{ + + using namespace std; + + double HyperDijkstra::TreeAction::perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e){ + (void) v; + (void) vParent; + (void) e; + return std::numeric_limits::max(); + } + + double HyperDijkstra::TreeAction::perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e, double distance){ + if (distance==-1) + return perform (v,vParent,e); + return std::numeric_limits::max(); + } + + HyperDijkstra::AdjacencyMapEntry::AdjacencyMapEntry(HyperGraph::Vertex* child_, HyperGraph::Vertex* parent_, + HyperGraph::Edge* edge_, double distance_) + { + _child=child_; + _parent=parent_; + _edge=edge_; + _distance=distance_; + } + + HyperDijkstra::HyperDijkstra(HyperGraph* g): _graph(g) + { + for (HyperGraph::VertexIDMap::const_iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); it++){ + AdjacencyMapEntry entry(it->second, 0,0,std::numeric_limits< double >::max()); + _adjacencyMap.insert(make_pair(entry.child(), entry)); + } + } + + void HyperDijkstra::reset() + { + for (HyperGraph::VertexSet::iterator it=_visited.begin(); it!=_visited.end(); it++){ + AdjacencyMap::iterator at=_adjacencyMap.find(*it); + assert(at!=_adjacencyMap.end()); + at->second=AdjacencyMapEntry(at->first,0,0,std::numeric_limits< double >::max()); + } + _visited.clear(); + } + + + bool operator<(const HyperDijkstra::AdjacencyMapEntry& a, const HyperDijkstra::AdjacencyMapEntry& b) + { + return a.distance()>b.distance(); + } + + + void HyperDijkstra::shortestPaths(HyperGraph::VertexSet& vset, HyperDijkstra::CostFunction* cost, + double maxDistance, double comparisonConditioner, bool directed, double maxEdgeCost) + { + reset(); + std::priority_queue< AdjacencyMapEntry > frontier; + for (HyperGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){ + HyperGraph::Vertex* v=*vit; + assert(v!=0); + AdjacencyMap::iterator it=_adjacencyMap.find(v); + if (it == _adjacencyMap.end()) { + cerr << __PRETTY_FUNCTION__ << "Vertex " << v->id() << " is not in the adjacency map" << endl; + } + assert(it!=_adjacencyMap.end()); + it->second._distance=0.; + it->second._parent=0; + frontier.push(it->second); + } + + while(! frontier.empty()){ + AdjacencyMapEntry entry=frontier.top(); + frontier.pop(); + HyperGraph::Vertex* u=entry.child(); + AdjacencyMap::iterator ut=_adjacencyMap.find(u); + if (ut == _adjacencyMap.end()) { + cerr << __PRETTY_FUNCTION__ << "Vertex " << u->id() << " is not in the adjacency map" << endl; + } + assert(ut!=_adjacencyMap.end()); + double uDistance=ut->second.distance(); + + std::pair< HyperGraph::VertexSet::iterator, bool> insertResult=_visited.insert(u); (void) insertResult; + HyperGraph::EdgeSet::iterator et=u->edges().begin(); + while (et != u->edges().end()){ + HyperGraph::Edge* edge=*et; + ++et; + + if (directed && edge->vertex(0) != u) + continue; + + for (size_t i = 0; i < edge->vertices().size(); ++i) { + HyperGraph::Vertex* z = edge->vertex(i); + if (z == u) + continue; + + double edgeDistance=(*cost)(edge, u, z); + if (edgeDistance==std::numeric_limits< double >::max() || edgeDistance > maxEdgeCost) + continue; + double zDistance=uDistance+edgeDistance; + //cerr << z->id() << " " << zDistance << endl; + + AdjacencyMap::iterator ot=_adjacencyMap.find(z); + assert(ot!=_adjacencyMap.end()); + + if (zDistance+comparisonConditionersecond.distance() && zDistancesecond._distance=zDistance; + ot->second._parent=u; + ot->second._edge=edge; + frontier.push(ot->second); + } + } + } + } + } + + void HyperDijkstra::shortestPaths(HyperGraph::Vertex* v, HyperDijkstra::CostFunction* cost, double maxDistance, + double comparisonConditioner, bool directed, double maxEdgeCost) + { + HyperGraph::VertexSet vset; + vset.insert(v); + shortestPaths(vset, cost, maxDistance, comparisonConditioner, directed, maxEdgeCost); + } + + void HyperDijkstra::computeTree(AdjacencyMap& amap) + { + for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){ + AdjacencyMapEntry& entry(it->second); + entry._children.clear(); + } + for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){ + AdjacencyMapEntry& entry(it->second); + HyperGraph::Vertex* parent=entry.parent(); + if (!parent){ + continue; + } + HyperGraph::Vertex* v=entry.child(); + assert (v==it->first); + + AdjacencyMap::iterator pt=amap.find(parent); + assert(pt!=amap.end()); + pt->second._children.insert(v); + } + } + + + void HyperDijkstra::visitAdjacencyMap(AdjacencyMap& amap, TreeAction* action, bool useDistance) + { + + typedef std::deque Deque; + Deque q; + // scans for the vertices without the parent (whcih are the roots of the trees) and applies the action to them. + for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){ + AdjacencyMapEntry& entry(it->second); + if (! entry.parent()) { + action->perform(it->first,0,0); + q.push_back(it->first); + } + } + + //std::cerr << "q.size()" << q.size() << endl; + int count=0; + while (! q.empty()){ + HyperGraph::Vertex* parent=q.front(); + q.pop_front(); + ++count; + AdjacencyMap::iterator parentIt=amap.find(parent); + if (parentIt==amap.end()) { + continue; + } + //cerr << "parent= " << parent << " parent id= " << parent->id() << "\t children id ="; + HyperGraph::VertexSet& childs(parentIt->second.children()); + for (HyperGraph::VertexSet::iterator childsIt=childs.begin(); childsIt!=childs.end(); ++childsIt){ + HyperGraph::Vertex* child=*childsIt; + //cerr << child->id(); + AdjacencyMap::iterator adjacencyIt=amap.find(child); + assert (adjacencyIt!=amap.end()); + HyperGraph::Edge* edge=adjacencyIt->second.edge(); + + assert(adjacencyIt->first==child); + assert(adjacencyIt->second.child()==child); + assert(adjacencyIt->second.parent()==parent); + if (! useDistance) { + action->perform(child, parent, edge); + } else { + action->perform(child, parent, edge, adjacencyIt->second.distance()); + } + q.push_back(child); + } + //cerr << endl; + } + + } + + void HyperDijkstra::connectedSubset(HyperGraph::VertexSet& connected, HyperGraph::VertexSet& visited, + HyperGraph::VertexSet& startingSet, + HyperGraph* g, HyperGraph::Vertex* v, + HyperDijkstra::CostFunction* cost, double distance, + double comparisonConditioner, double maxEdgeCost) + { + typedef std::queue VertexDeque; + visited.clear(); + connected.clear(); + VertexDeque frontier; + HyperDijkstra dv(g); + connected.insert(v); + frontier.push(v); + while (! frontier.empty()){ + HyperGraph::Vertex* v0=frontier.front(); + frontier.pop(); + dv.shortestPaths(v0, cost, distance, comparisonConditioner, false, maxEdgeCost); + for (HyperGraph::VertexSet::iterator it=dv.visited().begin(); it!=dv.visited().end(); ++it){ + visited.insert(*it); + if (startingSet.find(*it)==startingSet.end()) + continue; + std::pair insertOutcome=connected.insert(*it); + if (insertOutcome.second){ // the node was not in the connectedSet; + frontier.push(dynamic_cast(*it)); + } + } + } + } + + double UniformCostFunction::operator () (HyperGraph::Edge* /*edge*/, HyperGraph::Vertex* /*from*/, HyperGraph::Vertex* /*to*/) + { + return 1.; + } + +}; diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/hyper_dijkstra.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/hyper_dijkstra.h new file mode 100644 index 0000000..fe2c2f4 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/hyper_dijkstra.h @@ -0,0 +1,112 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_AIS_GENERAL_DIJKSTRA_HH +#define G2O_AIS_GENERAL_DIJKSTRA_HH + +#include +#include +#include + +#include "hyper_graph.h" + +namespace g2o{ + + struct HyperDijkstra{ + struct CostFunction { + virtual double operator() (HyperGraph::Edge* e, HyperGraph::Vertex* from, HyperGraph::Vertex* to)=0; + virtual ~CostFunction() { } + }; + + struct TreeAction { + virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e); + virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e, double distance); + }; + + + struct AdjacencyMapEntry{ + friend struct HyperDijkstra; + AdjacencyMapEntry(HyperGraph::Vertex* _child=0, + HyperGraph::Vertex* _parent=0, + HyperGraph::Edge* _edge=0, + double _distance=std::numeric_limits::max()); + HyperGraph::Vertex* child() const {return _child;} + HyperGraph::Vertex* parent() const {return _parent;} + HyperGraph::Edge* edge() const {return _edge;} + double distance() const {return _distance;} + HyperGraph::VertexSet& children() {return _children;} + const HyperGraph::VertexSet& children() const {return _children;} + protected: + HyperGraph::Vertex* _child; + HyperGraph::Vertex* _parent; + HyperGraph::Edge* _edge; + double _distance; + HyperGraph::VertexSet _children; + }; + + typedef std::map AdjacencyMap; + HyperDijkstra(HyperGraph* g); + HyperGraph::VertexSet& visited() {return _visited; } + AdjacencyMap& adjacencyMap() {return _adjacencyMap; } + HyperGraph* graph() {return _graph;} + + void shortestPaths(HyperGraph::Vertex* v, + HyperDijkstra::CostFunction* cost, + double maxDistance=std::numeric_limits< double >::max(), + double comparisonConditioner=1e-3, + bool directed=false, + double maxEdgeCost=std::numeric_limits< double >::max()); + + void shortestPaths(HyperGraph::VertexSet& vset, + HyperDijkstra::CostFunction* cost, + double maxDistance=std::numeric_limits< double >::max(), + double comparisonConditioner=1e-3, + bool directed=false, + double maxEdgeCost=std::numeric_limits< double >::max()); + + + static void computeTree(AdjacencyMap& amap); + static void visitAdjacencyMap(AdjacencyMap& amap, TreeAction* action, bool useDistance=false); + static void connectedSubset(HyperGraph::VertexSet& connected, HyperGraph::VertexSet& visited, + HyperGraph::VertexSet& startingSet, + HyperGraph* g, HyperGraph::Vertex* v, + HyperDijkstra::CostFunction* cost, double distance, double comparisonConditioner, + double maxEdgeCost=std::numeric_limits< double >::max() ); + + protected: + void reset(); + + AdjacencyMap _adjacencyMap; + HyperGraph::VertexSet _visited; + HyperGraph* _graph; + }; + + struct UniformCostFunction: public HyperDijkstra::CostFunction { + virtual double operator ()(HyperGraph::Edge* edge, HyperGraph::Vertex* from, HyperGraph::Vertex* to); + }; + +} +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/hyper_graph.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/hyper_graph.cpp new file mode 100644 index 0000000..1e1ea76 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/hyper_graph.cpp @@ -0,0 +1,166 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "hyper_graph.h" + +#include +#include + +namespace g2o { + + HyperGraph::Vertex::Vertex(int id) : _id(id) + { + } + + HyperGraph::Vertex::~Vertex() + { + } + + HyperGraph::Edge::Edge(int id) : _id(id) + { + } + + HyperGraph::Edge::~Edge() + { + } + + void HyperGraph::Edge::resize(size_t size) + { + _vertices.resize(size, 0); + } + + void HyperGraph::Edge::setId(int id) + { + _id = id; + } + + HyperGraph::Vertex* HyperGraph::vertex(int id) + { + VertexIDMap::iterator it=_vertices.find(id); + if (it==_vertices.end()) + return 0; + return it->second; + } + + const HyperGraph::Vertex* HyperGraph::vertex(int id) const + { + VertexIDMap::const_iterator it=_vertices.find(id); + if (it==_vertices.end()) + return 0; + return it->second; + } + + bool HyperGraph::addVertex(Vertex* v) + { + Vertex* vn=vertex(v->id()); + if (vn) + return false; + _vertices.insert( std::make_pair(v->id(),v) ); + return true; + } + + /** + * changes the id of a vertex already in the graph, and updates the bookkeeping + @ returns false if the vertex is not in the graph; + */ + bool HyperGraph::changeId(Vertex* v, int newId){ + Vertex* v2 = vertex(v->id()); + if (v != v2) + return false; + _vertices.erase(v->id()); + v->setId(newId); + _vertices.insert(std::make_pair(v->id(), v)); + return true; + } + + bool HyperGraph::addEdge(Edge* e) + { + std::pair result = _edges.insert(e); + if (! result.second) + return false; + for (std::vector::iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) { + Vertex* v = *it; + v->edges().insert(e); + } + return true; + } + + bool HyperGraph::removeVertex(Vertex* v) + { + VertexIDMap::iterator it=_vertices.find(v->id()); + if (it==_vertices.end()) + return false; + assert(it->second==v); + //remove all edges which are entering or leaving v; + EdgeSet tmp(v->edges()); + for (EdgeSet::iterator it=tmp.begin(); it!=tmp.end(); ++it){ + if (!removeEdge(*it)){ + assert(0); + } + } + _vertices.erase(it); + delete v; + return true; + } + + bool HyperGraph::removeEdge(Edge* e) + { + EdgeSet::iterator it = _edges.find(e); + if (it == _edges.end()) + return false; + _edges.erase(it); + + for (std::vector::iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) { + Vertex* v = *vit; + it = v->edges().find(e); + assert(it!=v->edges().end()); + v->edges().erase(it); + } + + delete e; + return true; + } + + HyperGraph::HyperGraph() + { + } + + void HyperGraph::clear() + { + for (VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) + delete (it->second); + for (EdgeSet::iterator it=_edges.begin(); it!=_edges.end(); ++it) + delete (*it); + _vertices.clear(); + _edges.clear(); + } + + HyperGraph::~HyperGraph() + { + clear(); + } + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/hyper_graph.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/hyper_graph.h new file mode 100644 index 0000000..da6bb3d --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/hyper_graph.h @@ -0,0 +1,220 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_AIS_HYPER_GRAPH_HH +#define G2O_AIS_HYPER_GRAPH_HH + +#include +#include +#include +#include +#include +#include +#include + +#ifdef _MSC_VER +#include +#else +#include +#endif + + +/** @addtogroup graph */ +//@{ +namespace g2o { + + /** + Class that models a directed Hyper-Graph. An hyper graph is a graph where an edge + can connect one or more nodes. Both Vertices and Edges of an hyoper graph + derive from the same class HyperGraphElement, thus one can implement generic algorithms + that operate transparently on edges or vertices (see HyperGraphAction). + + The vertices are uniquely identified by an int id, while the edges are + identfied by their pointers. + */ + class HyperGraph + { + public: + + /** + * \brief enum of all the types we have in our graphs + */ + enum HyperGraphElementType { + HGET_VERTEX, + HGET_EDGE, + HGET_PARAMETER, + HGET_CACHE, + HGET_DATA, + HGET_NUM_ELEMS // keep as last elem + }; + + typedef std::bitset GraphElemBitset; + + class Vertex; + class Edge; + + /** + * base hyper graph element, specialized in vertex and edge + */ + struct HyperGraphElement { + virtual ~HyperGraphElement() {} + /** + * returns the type of the graph element, see HyperGraphElementType + */ + virtual HyperGraphElementType elementType() const = 0; + }; + + typedef std::set EdgeSet; + typedef std::set VertexSet; + + typedef std::tr1::unordered_map VertexIDMap; + typedef std::vector VertexContainer; + + //! abstract Vertex, your types must derive from that one + class Vertex : public HyperGraphElement { + public: + //! creates a vertex having an ID specified by the argument + explicit Vertex(int id=-1); + virtual ~Vertex(); + //! returns the id + int id() const {return _id;} + virtual void setId( int newId) { _id=newId; } + //! returns the set of hyper-edges that are leaving/entering in this vertex + const EdgeSet& edges() const {return _edges;} + //! returns the set of hyper-edges that are leaving/entering in this vertex + EdgeSet& edges() {return _edges;} + virtual HyperGraphElementType elementType() const { return HGET_VERTEX;} + protected: + int _id; + EdgeSet _edges; + }; + + /** + * Abstract Edge class. Your nice edge classes should inherit from that one. + * An hyper-edge has pointers to the vertices it connects and stores them in a vector. + */ + class Edge : public HyperGraphElement { + public: + //! creates and empty edge with no vertices + explicit Edge(int id = -1); + virtual ~Edge(); + + /** + * resizes the number of vertices connected by this edge + */ + virtual void resize(size_t size); + /** + returns the vector of pointers to the vertices connected by the hyper-edge. + */ + const VertexContainer& vertices() const { return _vertices;} + /** + returns the vector of pointers to the vertices connected by the hyper-edge. + */ + VertexContainer& vertices() { return _vertices;} + /** + returns the pointer to the ith vertex connected to the hyper-edge. + */ + const Vertex* vertex(size_t i) const { assert(i < _vertices.size() && "index out of bounds"); return _vertices[i];} + /** + returns the pointer to the ith vertex connected to the hyper-edge. + */ + Vertex* vertex(size_t i) { assert(i < _vertices.size() && "index out of bounds"); return _vertices[i];} + /** + set the ith vertex on the hyper-edge to the pointer supplied + */ + void setVertex(size_t i, Vertex* v) { assert(i < _vertices.size() && "index out of bounds"); _vertices[i]=v;} + + int id() const {return _id;} + void setId(int id); + virtual HyperGraphElementType elementType() const { return HGET_EDGE;} + protected: + VertexContainer _vertices; + int _id; ///< unique id + }; + + public: + //! constructs an empty hyper graph + HyperGraph(); + //! destroys the hyper-graph and all the vertices of the graph + virtual ~HyperGraph(); + + //! returns a vertex id in the hyper-graph, or 0 if the vertex id is not present + Vertex* vertex(int id); + //! returns a vertex id in the hyper-graph, or 0 if the vertex id is not present + const Vertex* vertex(int id) const; + + //! removes a vertex from the graph. Returns true on success (vertex was present) + virtual bool removeVertex(Vertex* v); + //! removes a vertex from the graph. Returns true on success (edge was present) + virtual bool removeEdge(Edge* e); + //! clears the graph and empties all structures. + virtual void clear(); + + //! @returns the map id -> vertex where the vertices are stored + const VertexIDMap& vertices() const {return _vertices;} + //! @returns the map id -> vertex where the vertices are stored + VertexIDMap& vertices() {return _vertices;} + + //! @returns the set of edges of the hyper graph + const EdgeSet& edges() const {return _edges;} + //! @returns the set of edges of the hyper graph + EdgeSet& edges() {return _edges;} + + /** + * adds a vertex to the graph. The id of the vertex should be set before + * invoking this function. the function fails if another vertex + * with the same id is already in the graph. + * returns true, on success, or false on failure. + */ + virtual bool addVertex(Vertex* v); + + /** + * Adds an edge to the graph. If the edge is already in the graph, it + * does nothing and returns false. Otherwise it returns true. + */ + virtual bool addEdge(Edge* e); + + /** + * changes the id of a vertex already in the graph, and updates the bookkeeping + @ returns false if the vertex is not in the graph; + */ + virtual bool changeId(Vertex* v, int newId); + + protected: + VertexIDMap _vertices; + EdgeSet _edges; + + private: + // Disable the copy constructor and assignment operator + HyperGraph(const HyperGraph&) { } + HyperGraph& operator= (const HyperGraph&) { return *this; } + }; + +} // end namespace + +//@} + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/hyper_graph_action.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/hyper_graph_action.cpp new file mode 100644 index 0000000..1fe2439 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/hyper_graph_action.cpp @@ -0,0 +1,268 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "hyper_graph_action.h" +#include "optimizable_graph.h" +#include "../stuff/macros.h" + + +#include + +namespace g2o { + using namespace std; + + HyperGraphActionLibrary* HyperGraphActionLibrary::actionLibInstance = 0; + + HyperGraphAction::Parameters::~Parameters() + { + } + + HyperGraphAction::ParametersIteration::ParametersIteration(int iter) : + HyperGraphAction::Parameters(), + iteration(iter) + { + } + + HyperGraphAction::~HyperGraphAction() + { + } + + HyperGraphAction* HyperGraphAction::operator()(const HyperGraph*, Parameters*) + { + return 0; + } + + HyperGraphElementAction::Parameters::~Parameters() + { + } + + HyperGraphElementAction::HyperGraphElementAction(const std::string& typeName_) + { + _typeName = typeName_; + } + + void HyperGraphElementAction::setTypeName(const std::string& typeName_) + { + _typeName = typeName_; + } + + + HyperGraphElementAction* HyperGraphElementAction::operator()(HyperGraph::HyperGraphElement* , HyperGraphElementAction::Parameters* ) + { + return 0; + } + + HyperGraphElementAction* HyperGraphElementAction::operator()(const HyperGraph::HyperGraphElement* , HyperGraphElementAction::Parameters* ) + { + return 0; + } + + HyperGraphElementAction::~HyperGraphElementAction() + { + } + + HyperGraphElementActionCollection::HyperGraphElementActionCollection(const std::string& name_) + { + _name = name_; + } + + HyperGraphElementActionCollection::~HyperGraphElementActionCollection() + { + for (ActionMap::iterator it = _actionMap.begin(); it != _actionMap.end(); ++it) { + delete it->second; + } + } + + HyperGraphElementAction* HyperGraphElementActionCollection::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params) + { + ActionMap::iterator it=_actionMap.find(typeid(*element).name()); + //cerr << typeid(*element).name() << endl; + if (it==_actionMap.end()) + return 0; + HyperGraphElementAction* action=it->second; + return (*action)(element, params); + } + + HyperGraphElementAction* HyperGraphElementActionCollection::operator()(const HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params) + { + ActionMap::iterator it=_actionMap.find(typeid(*element).name()); + if (it==_actionMap.end()) + return 0; + HyperGraphElementAction* action=it->second; + return (*action)(element, params); + } + + bool HyperGraphElementActionCollection::registerAction(HyperGraphElementAction* action) + { +# ifdef G2O_DEBUG_ACTIONLIB + cerr << __PRETTY_FUNCTION__ << " " << action->name() << " " << action->typeName() << endl; +# endif + if (action->name()!=name()){ + cerr << __PRETTY_FUNCTION__ << ": invalid attempt to register an action in a collection with a different name " << name() << " " << action->name() << endl; + } + _actionMap.insert(make_pair ( action->typeName(), action) ); + return true; + } + + bool HyperGraphElementActionCollection::unregisterAction(HyperGraphElementAction* action) + { + for (HyperGraphElementAction::ActionMap::iterator it=_actionMap.begin(); it != _actionMap.end(); ++it) { + if (it->second == action){ + _actionMap.erase(it); + return true; + } + } + return false; + } + + HyperGraphActionLibrary::HyperGraphActionLibrary() + { + } + + HyperGraphActionLibrary* HyperGraphActionLibrary::instance() + { + if (actionLibInstance == 0) { + actionLibInstance = new HyperGraphActionLibrary; + } + return actionLibInstance; + } + + void HyperGraphActionLibrary::destroy() + { + delete actionLibInstance; + actionLibInstance = 0; + } + + HyperGraphActionLibrary::~HyperGraphActionLibrary() + { + for (HyperGraphElementAction::ActionMap::iterator it = _actionMap.begin(); it != _actionMap.end(); ++it) { + delete it->second; + } + } + + HyperGraphElementAction* HyperGraphActionLibrary::actionByName(const std::string& name) + { + HyperGraphElementAction::ActionMap::iterator it=_actionMap.find(name); + if (it!=_actionMap.end()) + return it->second; + return 0; + } + + bool HyperGraphActionLibrary::registerAction(HyperGraphElementAction* action) + { + HyperGraphElementAction* oldAction = actionByName(action->name()); + HyperGraphElementActionCollection* collection = 0; + if (oldAction) { + collection = dynamic_cast(oldAction); + if (! collection) { + cerr << __PRETTY_FUNCTION__ << ": fatal error, a collection is not at the first level in the library" << endl; + return 0; + } + } + if (! collection) { +#ifdef G2O_DEBUG_ACTIONLIB + cerr << __PRETTY_FUNCTION__ << ": creating collection for \"" << action->name() << "\"" << endl; +#endif + collection = new HyperGraphElementActionCollection(action->name()); + _actionMap.insert(make_pair(action->name(), collection)); + } + return collection->registerAction(action); + } + + bool HyperGraphActionLibrary::unregisterAction(HyperGraphElementAction* action) + { + list collectionDeleteList; + + // Search all the collections and delete the registered actions; if a collection becomes empty, schedule it for deletion; note that we can't delete the collections as we go because this will screw up the state of the iterators + for (HyperGraphElementAction::ActionMap::iterator it=_actionMap.begin(); it != _actionMap.end(); ++it) { + HyperGraphElementActionCollection* collection = dynamic_cast (it->second); + if (collection != 0) { + collection->unregisterAction(action); + if (collection->actionMap().size() == 0) { + collectionDeleteList.push_back(collection); + } + } + } + + // Delete any empty action collections + for (list::iterator itc = collectionDeleteList.begin(); itc != collectionDeleteList.end(); ++itc) { + //cout << "Deleting collection " << (*itc)->name() << endl; + _actionMap.erase((*itc)->name()); + } + + return true; + } + + + WriteGnuplotAction::WriteGnuplotAction(const std::string& typeName_) + : HyperGraphElementAction(typeName_) + { + _name="writeGnuplot"; + } + + DrawAction::Parameters::Parameters(){ + } + + DrawAction::DrawAction(const std::string& typeName_) + : HyperGraphElementAction(typeName_) + { + _name="draw"; + _previousParams = (Parameters*)0x42; + refreshPropertyPtrs(0); + } + + bool DrawAction::refreshPropertyPtrs(HyperGraphElementAction::Parameters* params_){ + if (_previousParams == params_) + return false; + DrawAction::Parameters* p=dynamic_cast(params_); + if (! p){ + _previousParams = 0; + _show = 0; + _showId = 0; + } else { + _previousParams = p; + _show = p->makeProperty(_typeName+"::SHOW", true); + _showId = p->makeProperty(_typeName+"::SHOW_ID", false); + } + return true; + } + + void applyAction(HyperGraph* graph, HyperGraphElementAction* action, HyperGraphElementAction::Parameters* params, const std::string& typeName) + { + for (HyperGraph::VertexIDMap::iterator it=graph->vertices().begin(); + it!=graph->vertices().end(); ++it){ + if ( typeName.empty() || typeid(*it->second).name()==typeName){ + (*action)(it->second, params); + } + } + for (HyperGraph::EdgeSet::iterator it=graph->edges().begin(); + it!=graph->edges().end(); ++it){ + if ( typeName.empty() || typeid(**it).name()==typeName) + (*action)(*it, params); + } + } + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/hyper_graph_action.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/hyper_graph_action.h new file mode 100644 index 0000000..21417c9 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/hyper_graph_action.h @@ -0,0 +1,222 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_HYPER_GRAPH_ACTION_H +#define G2O_HYPER_GRAPH_ACTION_H + +#include "hyper_graph.h" +#include "../stuff/property.h" + +#include +#include +#include +#include +#include + + +// define to get verbose output +//#define G2O_DEBUG_ACTIONLIB + +namespace g2o { + + /** + * \brief Abstract action that operates on an entire graph + */ + class HyperGraphAction { + public: + class Parameters { + public: + virtual ~Parameters(); + }; + + class ParametersIteration : public Parameters { + public: + explicit ParametersIteration(int iter); + int iteration; + }; + + virtual ~HyperGraphAction(); + + /** + * re-implement to carry out an action given the graph + */ + virtual HyperGraphAction* operator()(const HyperGraph* graph, Parameters* parameters = 0); + }; + + /** + * \brief Abstract action that operates on a graph entity + */ + class HyperGraphElementAction{ + public: + struct Parameters{ + virtual ~Parameters(); + }; + typedef std::map ActionMap; + //! an action should be instantiated with the typeid.name of the graph element + //! on which it operates + HyperGraphElementAction(const std::string& typeName_=""); + + //! redefine this to do the action stuff. If successful, the action returns a pointer to itself + virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, Parameters* parameters); + + //! redefine this to do the action stuff. If successful, the action returns a pointer to itself + virtual HyperGraphElementAction* operator()(const HyperGraph::HyperGraphElement* element, Parameters* parameters); + + //! destroyed actions release the memory + virtual ~HyperGraphElementAction(); + + //! returns the typeid name of the action + const std::string& typeName() const { return _typeName;} + + //! returns the name of an action, e.g "draw" + const std::string& name() const{ return _name;} + + //! sets the type on which an action has to operate + void setTypeName(const std::string& typeName_); + + protected: + std::string _typeName; + std::string _name; + }; + + /** + * \brief collection of actions + * + * collection of actions calls contains homogeneous actions operating on different types + * all collected actions have the same name and should have the same functionality + */ + class HyperGraphElementActionCollection: public HyperGraphElementAction{ + public: + //! constructor. name_ is the name of the action e.g.draw). + HyperGraphElementActionCollection(const std::string& name_); + //! destructor: it deletes all actions in the pool. + virtual ~HyperGraphElementActionCollection(); + //! calling functions, they return a pointer to the instance of action in actionMap + //! that was active on element + virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, Parameters* parameters); + virtual HyperGraphElementAction* operator()(const HyperGraph::HyperGraphElement* element, Parameters* parameters); + ActionMap& actionMap() {return _actionMap;} + //! inserts an action in the pool. The action should have the same name of the container. + //! returns false on failure (the container has a different name than the action); + bool registerAction(HyperGraphElementAction* action); + bool unregisterAction(HyperGraphElementAction* action); + protected: + ActionMap _actionMap; + }; + + /** + * \brief library of actions, indexed by the action name; + * + * library of actions, indexed by the action name; + * one can use ti to register a collection of actions + */ + class HyperGraphActionLibrary{ + public: + //! return the single instance of the HyperGraphActionLibrary + static HyperGraphActionLibrary* instance(); + //! free the instance + static void destroy(); + + // returns a pointer to a collection indexed by name + HyperGraphElementAction* actionByName(const std::string& name); + // registers a basic action in the pool. If necessary a container is created + bool registerAction(HyperGraphElementAction* action); + bool unregisterAction(HyperGraphElementAction* action); + + inline HyperGraphElementAction::ActionMap& actionMap() {return _actionMap;} + protected: + HyperGraphActionLibrary(); + ~HyperGraphActionLibrary(); + HyperGraphElementAction::ActionMap _actionMap; + private: + static HyperGraphActionLibrary* actionLibInstance; + }; + + /** + * apply an action to all the elements of the graph. + */ + void applyAction(HyperGraph* graph, HyperGraphElementAction* action, HyperGraphElementAction::Parameters* parameters=0, const std::string& typeName=""); + + /** + * brief write into gnuplot + */ + class WriteGnuplotAction: public HyperGraphElementAction{ + public: + struct Parameters: public HyperGraphElementAction::Parameters{ + std::ostream* os; + }; + WriteGnuplotAction(const std::string& typeName_); + }; + + /** + * \brief draw actions + */ + + class DrawAction : public HyperGraphElementAction{ + public: + class Parameters: public HyperGraphElementAction::Parameters, public PropertyMap{ + public: + Parameters(); + }; + DrawAction(const std::string& typeName_); + protected: + virtual bool refreshPropertyPtrs(HyperGraphElementAction::Parameters* params_); + Parameters* _previousParams; + BoolProperty* _show; + BoolProperty* _showId; + }; + + template class RegisterActionProxy + { + public: + RegisterActionProxy() + { +#ifdef G2O_DEBUG_ACTIONLIB + std::cout << __FUNCTION__ << ": Registering action of type " << typeid(T).name() << std::endl; +#endif + _action = new T(); + HyperGraphActionLibrary::instance()->registerAction(_action); + } + + ~RegisterActionProxy() + { +#ifdef G2O_DEBUG_ACTIONLIB + std::cout << __FUNCTION__ << ": Unregistering action of type " << typeid(T).name() << std::endl; +#endif + HyperGraphActionLibrary::instance()->unregisterAction(_action); + delete _action; + } + + private: + HyperGraphElementAction* _action; + }; + +#define G2O_REGISTER_ACTION(classname) \ + extern "C" void g2o_action_##classname(void) {} \ + static g2o::RegisterActionProxy g_action_proxy_##classname; +}; + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/jacobian_workspace.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/jacobian_workspace.cpp new file mode 100644 index 0000000..9890a18 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/jacobian_workspace.cpp @@ -0,0 +1,89 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "jacobian_workspace.h" + +#include + +#include "optimizable_graph.h" + +using namespace std; + +namespace g2o { + +JacobianWorkspace::JacobianWorkspace() : + _maxNumVertices(-1), _maxDimension(-1) +{ +} + +JacobianWorkspace::~JacobianWorkspace() +{ +} + +bool JacobianWorkspace::allocate() +{ + //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl; + if (_maxNumVertices <=0 || _maxDimension <= 0) + return false; + _workspace.resize(_maxNumVertices); + for (WorkspaceVector::iterator it = _workspace.begin(); it != _workspace.end(); ++it) { + it->resize(_maxDimension); + it->setZero(); + } + return true; +} + +void JacobianWorkspace::updateSize(const HyperGraph::Edge* e_) +{ + const OptimizableGraph::Edge* e = static_cast(e_); + int errorDimension = e->dimension(); + int numVertices = e->vertices().size(); + int maxDimensionForEdge = -1; + for (int i = 0; i < numVertices; ++i) { + const OptimizableGraph::Vertex* v = static_cast(e->vertex(i)); + assert(v && "Edge has no vertex assigned"); + maxDimensionForEdge = max(v->dimension() * errorDimension, maxDimensionForEdge); + } + _maxNumVertices = max(numVertices, _maxNumVertices); + _maxDimension = max(maxDimensionForEdge, _maxDimension); + //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl; +} + +void JacobianWorkspace::updateSize(const OptimizableGraph& graph) +{ + for (OptimizableGraph::EdgeSet::const_iterator it = graph.edges().begin(); it != graph.edges().end(); ++it) { + const OptimizableGraph::Edge* e = static_cast(*it); + updateSize(e); + } +} + +void JacobianWorkspace::updateSize(int numVertices, int dimension) +{ + _maxNumVertices = max(numVertices, _maxNumVertices); + _maxDimension = max(dimension, _maxDimension); +} + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/jacobian_workspace.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/jacobian_workspace.h new file mode 100644 index 0000000..e1f1602 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/jacobian_workspace.h @@ -0,0 +1,96 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef JACOBIAN_WORKSPACE_H +#define JACOBIAN_WORKSPACE_H + +#include +#include + +#include +#include + +#include "hyper_graph.h" + +namespace g2o { + + struct OptimizableGraph; + + /** + * \brief provide memory workspace for computing the Jacobians + * + * The workspace is used by an OptimizableGraph to provide temporary memory + * for computing the Jacobian of the error functions. + * Before calling linearizeOplus on an edge, the workspace needs to be allocated + * by calling allocate(). + */ + class JacobianWorkspace + { + public: + typedef std::vector > WorkspaceVector; + + public: + JacobianWorkspace(); + ~JacobianWorkspace(); + + /** + * allocate the workspace + */ + bool allocate(); + + /** + * update the maximum required workspace needed by taking into account this edge + */ + void updateSize(const HyperGraph::Edge* e); + + /** + * update the required workspace by looking at a full graph + */ + void updateSize(const OptimizableGraph& graph); + + /** + * manually update with the given parameters + */ + void updateSize(int numVertices, int dimension); + + /** + * return the workspace for a vertex in an edge + */ + double* workspaceForVertex(int vertexIndex) + { + assert(vertexIndex >= 0 && (size_t)vertexIndex < _workspace.size() && "Index out of bounds"); + return _workspace[vertexIndex].data(); + } + + protected: + WorkspaceVector _workspace; ///< the memory pre-allocated for computing the Jacobians + int _maxNumVertices; ///< the maximum number of vertices connected by a hyper-edge + int _maxDimension; ///< the maximum dimension (number of elements) for a Jacobian + }; + +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/linear_solver.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/linear_solver.h new file mode 100644 index 0000000..20213d4 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/linear_solver.h @@ -0,0 +1,109 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_LINEAR_SOLVER_H +#define G2O_LINEAR_SOLVER_H +#include "sparse_block_matrix.h" +#include "sparse_block_matrix_ccs.h" + +namespace g2o { + +/** + * \brief basic solver for Ax = b + * + * basic solver for Ax = b which has to reimplemented for different linear algebra libraries. + * A is assumed to be symmetric (only upper triangular block is stored) and positive-semi-definit. + */ +template +class LinearSolver +{ + public: + LinearSolver() {}; + virtual ~LinearSolver() {} + + /** + * init for operating on matrices with a different non-zero pattern like before + */ + virtual bool init() = 0; + + /** + * Assumes that A is the same matrix for several calls. + * Among other assumptions, the non-zero pattern does not change! + * If the matrix changes call init() before. + * solve system Ax = b, x and b have to allocated beforehand!! + */ + virtual bool solve(const SparseBlockMatrix& A, double* x, double* b) = 0; + + /** + * Inverts the diagonal blocks of A + * @returns false if not defined. + */ + virtual bool solveBlocks(double**&blocks, const SparseBlockMatrix& A) { (void)blocks; (void) A; return false; } + + + /** + * Inverts the a block pattern of A in spinv + * @returns false if not defined. + */ + virtual bool solvePattern(SparseBlockMatrix& spinv, const std::vector >& blockIndices, const SparseBlockMatrix& A){ + (void) spinv; + (void) blockIndices; + (void) A; + return false; + } + + //! write a debug dump of the system matrix if it is not PSD in solve + virtual bool writeDebug() const { return false;} + virtual void setWriteDebug(bool) {} +}; + +/** + * \brief Solver with faster iterating structure for the linear matrix + */ +template +class LinearSolverCCS : public LinearSolver +{ + public: + LinearSolverCCS() : LinearSolver(), _ccsMatrix(0) {} + ~LinearSolverCCS() + { + delete _ccsMatrix; + } + + protected: + SparseBlockMatrixCCS* _ccsMatrix; + + void initMatrixStructure(const SparseBlockMatrix& A) + { + delete _ccsMatrix; + _ccsMatrix = new SparseBlockMatrixCCS(A.rowBlockIndices(), A.colBlockIndices()); + A.fillSparseBlockMatrixCCS(*_ccsMatrix); + } +}; + +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.cpp new file mode 100644 index 0000000..bc11f5c --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.cpp @@ -0,0 +1,222 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "marginal_covariance_cholesky.h" + +#include +#include +using namespace std; + +namespace g2o { + +struct MatrixElem +{ + int r, c; + MatrixElem(int r_, int c_) : r(r_), c(c_) {} + bool operator<(const MatrixElem& other) const + { + return c > other.c || (c == other.c && r > other.r); + } +}; + +MarginalCovarianceCholesky::MarginalCovarianceCholesky() : + _n(0), _Ap(0), _Ai(0), _Ax(0), _perm(0) +{ +} + +MarginalCovarianceCholesky::~MarginalCovarianceCholesky() +{ +} + +void MarginalCovarianceCholesky::setCholeskyFactor(int n, int* Lp, int* Li, double* Lx, int* permInv) +{ + _n = n; + _Ap = Lp; + _Ai = Li; + _Ax = Lx; + _perm = permInv; + + // pre-compute reciprocal values of the diagonal of L + _diag.resize(n); + for (int r = 0; r < n; ++r) { + const int& sc = _Ap[r]; // L is lower triangular, thus the first elem in the column is the diagonal entry + assert(r == _Ai[sc] && "Error in CCS storage of L"); + _diag[r] = 1.0 / _Ax[sc]; + } +} + +double MarginalCovarianceCholesky::computeEntry(int r, int c) +{ + assert(r <= c); + int idx = computeIndex(r, c); + + LookupMap::const_iterator foundIt = _map.find(idx); + if (foundIt != _map.end()) { + return foundIt->second; + } + + // compute the summation over column r + double s = 0.; + const int& sc = _Ap[r]; + const int& ec = _Ap[r+1]; + for (int j = sc+1; j < ec; ++j) { // sum over row r while skipping the element on the diagonal + const int& rr = _Ai[j]; + double val = rr < c ? computeEntry(rr, c) : computeEntry(c, rr); + s += val * _Ax[j]; + } + + double result; + if (r == c) { + const double& diagElem = _diag[r]; + result = diagElem * (diagElem - s); + } else { + result = -s * _diag[r]; + } + _map[idx] = result; + return result; +} + +void MarginalCovarianceCholesky::computeCovariance(double** covBlocks, const std::vector& blockIndices) +{ + _map.clear(); + int base = 0; + vector elemsToCompute; + for (size_t i = 0; i < blockIndices.size(); ++i) { + int nbase = blockIndices[i]; + int vdim = nbase - base; + for (int rr = 0; rr < vdim; ++rr) + for (int cc = rr; cc < vdim; ++cc) { + int r = _perm ? _perm[rr + base] : rr + base; // apply permutation + int c = _perm ? _perm[cc + base] : cc + base; + if (r > c) // make sure it's still upper triangular after applying the permutation + swap(r, c); + elemsToCompute.push_back(MatrixElem(r, c)); + } + base = nbase; + } + + // sort the elems to reduce the recursive calls + sort(elemsToCompute.begin(), elemsToCompute.end()); + + // compute the inverse elements we need + for (size_t i = 0; i < elemsToCompute.size(); ++i) { + const MatrixElem& me = elemsToCompute[i]; + computeEntry(me.r, me.c); + } + + // set the marginal covariance for the vertices, by writing to the blocks memory + base = 0; + for (size_t i = 0; i < blockIndices.size(); ++i) { + int nbase = blockIndices[i]; + int vdim = nbase - base; + double* cov = covBlocks[i]; + for (int rr = 0; rr < vdim; ++rr) + for (int cc = rr; cc < vdim; ++cc) { + int r = _perm ? _perm[rr + base] : rr + base; // apply permutation + int c = _perm ? _perm[cc + base] : cc + base; + if (r > c) // upper triangle + swap(r, c); + int idx = computeIndex(r, c); + LookupMap::const_iterator foundIt = _map.find(idx); + assert(foundIt != _map.end()); + cov[rr*vdim + cc] = foundIt->second; + if (rr != cc) + cov[cc*vdim + rr] = foundIt->second; + } + base = nbase; + } +} + + +void MarginalCovarianceCholesky::computeCovariance(SparseBlockMatrix& spinv, const std::vector& rowBlockIndices, const std::vector< std::pair >& blockIndices) +{ + // allocate the sparse + spinv = SparseBlockMatrix(&rowBlockIndices[0], + &rowBlockIndices[0], + rowBlockIndices.size(), + rowBlockIndices.size(), true); + _map.clear(); + vector elemsToCompute; + for (size_t i = 0; i < blockIndices.size(); ++i) { + int blockRow=blockIndices[i].first; + int blockCol=blockIndices[i].second; + assert(blockRow>=0); + assert(blockRow < (int)rowBlockIndices.size()); + assert(blockCol>=0); + assert(blockCol < (int)rowBlockIndices.size()); + + int rowBase=spinv.rowBaseOfBlock(blockRow); + int colBase=spinv.colBaseOfBlock(blockCol); + + MatrixXd *block=spinv.block(blockRow, blockCol, true); + assert(block); + for (int iRow=0; iRowrows(); ++iRow) + for (int iCol=0; iColcols(); ++iCol){ + int rr=rowBase+iRow; + int cc=colBase+iCol; + int r = _perm ? _perm[rr] : rr; // apply permutation + int c = _perm ? _perm[cc] : cc; + if (r > c) + swap(r, c); + elemsToCompute.push_back(MatrixElem(r, c)); + } + } + + // sort the elems to reduce the number of recursive calls + sort(elemsToCompute.begin(), elemsToCompute.end()); + + // compute the inverse elements we need + for (size_t i = 0; i < elemsToCompute.size(); ++i) { + const MatrixElem& me = elemsToCompute[i]; + computeEntry(me.r, me.c); + } + + // set the marginal covariance + for (size_t i = 0; i < blockIndices.size(); ++i) { + int blockRow=blockIndices[i].first; + int blockCol=blockIndices[i].second; + int rowBase=spinv.rowBaseOfBlock(blockRow); + int colBase=spinv.colBaseOfBlock(blockCol); + + MatrixXd *block=spinv.block(blockRow, blockCol); + assert(block); + for (int iRow=0; iRowrows(); ++iRow) + for (int iCol=0; iColcols(); ++iCol){ + int rr=rowBase+iRow; + int cc=colBase+iCol; + int r = _perm ? _perm[rr] : rr; // apply permutation + int c = _perm ? _perm[cc] : cc; + if (r > c) + swap(r, c); + int idx = computeIndex(r, c); + LookupMap::const_iterator foundIt = _map.find(idx); + assert(foundIt != _map.end()); + (*block)(iRow, iCol) = foundIt->second; + } + } +} + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.h new file mode 100644 index 0000000..e7dfce8 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.h @@ -0,0 +1,103 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_MARGINAL_COVARIANCE_CHOLESKY_H +#define G2O_MARGINAL_COVARIANCE_CHOLESKY_H + +#include "optimizable_graph.h" +#include "sparse_block_matrix.h" + +#include +#include + +#ifdef _MSC_VER +#include +#else +#include +#endif + + +namespace g2o { + + /** + * \brief computing the marginal covariance given a cholesky factor (lower triangle of the factor) + */ + class MarginalCovarianceCholesky { + protected: + /** + * hash struct for storing the matrix elements needed to compute the covariance + */ + typedef std::tr1::unordered_map LookupMap; + + public: + MarginalCovarianceCholesky(); + ~MarginalCovarianceCholesky(); + + /** + * compute the marginal cov for the given block indices, write the result to the covBlocks memory (which has to + * be provided by the caller). + */ + void computeCovariance(double** covBlocks, const std::vector& blockIndices); + + + /** + * compute the marginal cov for the given block indices, write the result in spinv). + */ + void computeCovariance(SparseBlockMatrix& spinv, const std::vector& rowBlockIndices, const std::vector< std::pair >& blockIndices); + + + /** + * set the CCS representation of the cholesky factor along with the inverse permutation used to reduce the fill-in. + * permInv might be 0, will then not permute the entries. + * + * The pointers provided by the user need to be still valid when calling computeCovariance(). The pointers + * are owned by the caller, MarginalCovarianceCholesky does not free the pointers. + */ + void setCholeskyFactor(int n, int* Lp, int* Li, double* Lx, int* permInv); + + protected: + // information about the cholesky factor (lower triangle) + int _n; ///< L is an n X n matrix + int* _Ap; ///< column pointer of the CCS storage + int* _Ai; ///< row indices of the CCS storage + double* _Ax; ///< values of the cholesky factor + int* _perm; ///< permutation of the cholesky factor. Variable re-ordering for better fill-in + + LookupMap _map; ///< hash look up table for the already computed entries + std::vector _diag; ///< cache 1 / H_ii to avoid recalculations + + //! compute the index used for hashing + int computeIndex(int r, int c) const { /*assert(r <= c);*/ return r*_n + c;} + /** + * compute one entry in the covariance, r and c are values after applying the permutation, and upper triangular. + * May issue recursive calls to itself to compute the missing values. + */ + double computeEntry(int r, int c); + }; + +} + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/matrix_operations.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/matrix_operations.h new file mode 100644 index 0000000..28e6fbe --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/matrix_operations.h @@ -0,0 +1,74 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_CORE_MATRIX_OPERATIONS_H +#define G2O_CORE_MATRIX_OPERATIONS_H + +#include + +namespace g2o { + namespace internal { + + template + inline void axpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) + { + y.segment(yoff) += A * x.segment(xoff); + } + + template + inline void axpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) + { + y.segment(yoff, A.rows()) += A * x.segment::ColsAtCompileTime>(xoff); + } + + template<> + inline void axpy(const Eigen::MatrixXd& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) + { + y.segment(yoff, A.rows()) += A * x.segment(xoff, A.cols()); + } + + template + inline void atxpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) + { + y.segment(yoff) += A.transpose() * x.segment(xoff); + } + + template + inline void atxpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) + { + y.segment::ColsAtCompileTime>(yoff) += A.transpose() * x.segment(xoff, A.rows()); + } + + template<> + inline void atxpy(const Eigen::MatrixXd& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) + { + y.segment(yoff, A.cols()) += A.transpose() * x.segment(xoff, A.rows()); + } + + } // end namespace internal +} // end namespace g2o + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/matrix_structure.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/matrix_structure.cpp new file mode 100644 index 0000000..3152195 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/matrix_structure.cpp @@ -0,0 +1,125 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "matrix_structure.h" + +#include +#include +#include +#include +using namespace std; + +namespace g2o { + +struct ColSort +{ + bool operator()(const pair& e1, const pair& e2) const + { + return e1.second < e2.second || (e1.second == e2.second && e1.first < e2.first); + } +}; + +MatrixStructure::MatrixStructure() : + n(0), m(0), Ap(0), Aii(0), maxN(0), maxNz(0) +{ +} + +MatrixStructure::~MatrixStructure() +{ + free(); +} + +void MatrixStructure::alloc(int n_, int nz) +{ + if (n == 0) { + maxN = n = n_; + maxNz = nz; + Ap = new int[maxN + 1]; + Aii = new int[maxNz]; + } + else { + n = n_; + if (maxNz < nz) { + maxNz = 2 * nz; + delete[] Aii; + Aii = new int[maxNz]; + } + if (maxN < n) { + maxN = 2 * n; + delete[] Ap; + Ap = new int[maxN + 1]; + } + } +} + +void MatrixStructure::free() +{ + n = 0; + m = 0; + maxN = 0; + maxNz = 0; + delete[] Aii; Aii = 0; + delete[] Ap; Ap = 0; +} + +bool MatrixStructure::write(const char* filename) const +{ + const int& cols = n; + const int& rows = m; + + string name = filename; + std::string::size_type lastDot = name.find_last_of('.'); + if (lastDot != std::string::npos) + name = name.substr(0, lastDot); + + vector > entries; + for (int i=0; i < cols; ++i) { + const int& rbeg = Ap[i]; + const int& rend = Ap[i+1]; + for (int j = rbeg; j < rend; ++j) { + entries.push_back(make_pair(Aii[j], i)); + if (Aii[j] != i) + entries.push_back(make_pair(i, Aii[j])); + } + } + + sort(entries.begin(), entries.end(), ColSort()); + + std::ofstream fout(filename); + fout << "# name: " << name << std::endl; + fout << "# type: sparse matrix" << std::endl; + fout << "# nnz: " << entries.size() << std::endl; + fout << "# rows: " << rows << std::endl; + fout << "# columns: " << cols << std::endl; + for (vector >::const_iterator it = entries.begin(); it != entries.end(); ++it) { + const pair& entry = *it; + fout << entry.first << " " << entry.second << " 0" << std::endl; // write a constant value of 0 + } + + return fout.good(); +} + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/matrix_structure.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/matrix_structure.h new file mode 100644 index 0000000..fd70e53 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/matrix_structure.h @@ -0,0 +1,69 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_MATRIX_STRUCTURE_H +#define G2O_MATRIX_STRUCTURE_H + + +namespace g2o { + +/** + * \brief representing the structure of a matrix in column compressed structure (only the upper triangular part of the matrix) + */ +class MatrixStructure +{ + public: + MatrixStructure(); + ~MatrixStructure(); + /** + * allocate space for the Matrix Structure. You may call this on an already allocated struct, it will + * then reallocate the memory + additional space (double the required space). + */ + void alloc(int n_, int nz); + + void free(); + + /** + * Write the matrix pattern to a file. File is also loadable by octave, e.g., then use spy(matrix) + */ + bool write(const char* filename) const; + + int n; ///< A is m-by-n. n must be >= 0. + int m; ///< A is m-by-n. m must be >= 0. + int* Ap; ///< column pointers for A, of size n+1 + int* Aii; ///< row indices of A, of size nz = Ap [n] + + //! max number of non-zeros blocks + int nzMax() const { return maxNz;} + + protected: + int maxN; ///< size of the allocated memory + int maxNz; ///< size of the allocated memory +}; + +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/openmp_mutex.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/openmp_mutex.h new file mode 100644 index 0000000..6a9b8f4 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/openmp_mutex.h @@ -0,0 +1,97 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_OPENMP_MUTEX +#define G2O_OPENMP_MUTEX + +#include "../../config.h" + +#ifdef G2O_OPENMP +#include +#else +#include +#endif + +namespace g2o { + +#ifdef G2O_OPENMP + + /** + * \brief Mutex realized via OpenMP + */ + class OpenMPMutex + { + public: + OpenMPMutex() { omp_init_lock(&_lock); } + ~OpenMPMutex() { omp_destroy_lock(&_lock); } + void lock() { omp_set_lock(&_lock); } + void unlock() { omp_unset_lock(&_lock); } + protected: + omp_lock_t _lock; + }; + +#else + + /* + * dummy which does nothing in case we don't have OpenMP support. + * In debug mode, the mutex allows to verify the correct lock and unlock behavior + */ + class OpenMPMutex + { + public: +#ifdef NDEBUG + OpenMPMutex() {} +#else + OpenMPMutex() : _cnt(0) {} +#endif + ~OpenMPMutex() { assert(_cnt == 0 && "Freeing locked mutex");} + void lock() { assert(++_cnt == 1 && "Locking already locked mutex");} + void unlock() { assert(--_cnt == 0 && "Trying to unlock a mutex which is not locked");} + protected: +#ifndef NDEBUG + char _cnt; +#endif + }; + +#endif + + /** + * \brief lock a mutex within a scope + */ + class ScopedOpenMPMutex + { + public: + explicit ScopedOpenMPMutex(OpenMPMutex* mutex) : _mutex(mutex) { _mutex->lock(); } + ~ScopedOpenMPMutex() { _mutex->unlock(); } + private: + OpenMPMutex* const _mutex; + ScopedOpenMPMutex(const ScopedOpenMPMutex&); + void operator=(const ScopedOpenMPMutex&); + }; + +} + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimizable_graph.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimizable_graph.cpp new file mode 100644 index 0000000..bb6b54b --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimizable_graph.cpp @@ -0,0 +1,910 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "optimizable_graph.h" + +#include +#include +#include +#include +#include + +#include + +#include "estimate_propagator.h" +#include "factory.h" +#include "optimization_algorithm_property.h" +#include "hyper_graph_action.h" +#include "cache.h" +#include "robust_kernel.h" + +#include "../stuff/macros.h" +#include "../stuff/color_macros.h" +#include "../stuff/string_tools.h" +#include "../stuff/misc.h" + +namespace g2o { + + using namespace std; + + OptimizableGraph::Data::Data(){ + _next = 0; + } + + OptimizableGraph::Data::~Data(){ + if (_next) + delete _next; + } + + + OptimizableGraph::Vertex::Vertex() : + HyperGraph::Vertex(), + _graph(0), _userData(0), _hessianIndex(-1), _fixed(false), _marginalized(false), + _colInHessian(-1), _cacheContainer(0) + { + } + + CacheContainer* OptimizableGraph::Vertex::cacheContainer(){ + if (! _cacheContainer) + _cacheContainer = new CacheContainer(this); + return _cacheContainer; + } + + + void OptimizableGraph::Vertex::updateCache(){ + if (_cacheContainer){ + _cacheContainer->setUpdateNeeded(); + _cacheContainer->update(); + } + } + + OptimizableGraph::Vertex::~Vertex() + { + if (_cacheContainer) + delete (_cacheContainer); + if (_userData) + delete _userData; + } + + OptimizableGraph::Vertex* OptimizableGraph::Vertex::clone() const + { + return 0; + } + + bool OptimizableGraph::Vertex::setEstimateData(const double* v) + { + bool ret = setEstimateDataImpl(v); + updateCache(); + return ret; + } + + bool OptimizableGraph::Vertex::getEstimateData(double *) const + { + return false; + } + + int OptimizableGraph::Vertex::estimateDimension() const + { + return -1; + } + + bool OptimizableGraph::Vertex::setMinimalEstimateData(const double* v) + { + bool ret = setMinimalEstimateDataImpl(v); + updateCache(); + return ret; + } + + bool OptimizableGraph::Vertex::getMinimalEstimateData(double *) const + { + return false; + } + + int OptimizableGraph::Vertex::minimalEstimateDimension() const + { + return -1; + } + + + OptimizableGraph::Edge::Edge() : + HyperGraph::Edge(), + _dimension(-1), _level(0), _robustKernel(0) + { + } + + OptimizableGraph::Edge::~Edge() + { + delete _robustKernel; + } + + OptimizableGraph* OptimizableGraph::Edge::graph(){ + if (! _vertices.size()) + return 0; + OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)_vertices[0]; + if (!v) + return 0; + return v->graph(); + } + + const OptimizableGraph* OptimizableGraph::Edge::graph() const{ + if (! _vertices.size()) + return 0; + const OptimizableGraph::Vertex* v=(const OptimizableGraph::Vertex*) _vertices[0]; + if (!v) + return 0; + return v->graph(); + } + + bool OptimizableGraph::Edge::setParameterId(int argNum, int paramId){ + if ((int)_parameters.size()<=argNum) + return false; + if (argNum<0) + return false; + *_parameters[argNum] = 0; + _parameterIds[argNum] = paramId; + return true; + } + + bool OptimizableGraph::Edge::resolveParameters() { + if (!graph()) { + cerr << __PRETTY_FUNCTION__ << ": edge not registered with a graph" << endl; + return false; + } + + assert (_parameters.size() == _parameterIds.size()); + //cerr << __PRETTY_FUNCTION__ << ": encountered " << _parameters.size() << " parameters" << endl; + for (size_t i=0; i<_parameters.size(); i++){ + int index = _parameterIds[i]; + *_parameters[i] = graph()->parameter(index); + if (typeid(**_parameters[i]).name()!=_parameterTypes[i]){ + cerr << __PRETTY_FUNCTION__ << ": FATAL, parameter type mismatch - encountered " << typeid(**_parameters[i]).name() << "; should be " << _parameterTypes[i] << endl; + } + if (!*_parameters[i]) { + cerr << __PRETTY_FUNCTION__ << ": FATAL, *_parameters[i] == 0" << endl; + return false; + } + } + return true; + } + + void OptimizableGraph::Edge::setRobustKernel(RobustKernel* ptr) + { + if (_robustKernel) + delete _robustKernel; + _robustKernel = ptr; + } + + bool OptimizableGraph::Edge::resolveCaches() { + return true; + } + + bool OptimizableGraph::Edge::setMeasurementData(const double *) + { + return false; + } + + bool OptimizableGraph::Edge::getMeasurementData(double *) const + { + return false; + } + + int OptimizableGraph::Edge::measurementDimension() const + { + return -1; + } + + bool OptimizableGraph::Edge::setMeasurementFromState(){ + return false; + } + + + OptimizableGraph::Edge* OptimizableGraph::Edge::clone() const + { + // TODO + return 0; + } + + + OptimizableGraph::OptimizableGraph() + { + _nextEdgeId = 0; _edge_has_id = false; + _graphActions.resize(AT_NUM_ELEMENTS); + } + + OptimizableGraph::~OptimizableGraph() + { + clear(); + clearParameters(); + } + + bool OptimizableGraph::addVertex(HyperGraph::Vertex* v, Data* userData) + { + Vertex* inserted = vertex(v->id()); + if (inserted) { + cerr << __FUNCTION__ << ": FATAL, a vertex with ID " << v->id() << " has already been registered with this graph" << endl; + assert(0 && "Vertex with this ID already contained in the graph"); + return false; + } + OptimizableGraph::Vertex* ov=dynamic_cast(v); + assert(ov && "Vertex does not inherit from OptimizableGraph::Vertex"); + if (ov->_graph != 0 && ov->_graph != this) { + cerr << __FUNCTION__ << ": FATAL, vertex with ID " << v->id() << " has already registered with another graph " << ov->_graph << endl; + assert(0 && "Vertex already registered with another graph"); + return false; + } + if (userData) + ov->setUserData(userData); + ov->_graph=this; + return HyperGraph::addVertex(v); + } + + bool OptimizableGraph::addEdge(HyperGraph::Edge* e_) + { + OptimizableGraph::Edge* e = dynamic_cast(e_); + assert(e && "Edge does not inherit from OptimizableGraph::Edge"); + if (! e) + return false; + bool eresult = HyperGraph::addEdge(e); + if (! eresult) + return false; + e->_internalId = _nextEdgeId++; + if (! e->resolveParameters()){ + cerr << __FUNCTION__ << ": FATAL, cannot resolve parameters for edge " << e << endl; + return false; + } + if (! e->resolveCaches()){ + cerr << __FUNCTION__ << ": FATAL, cannot resolve caches for edge " << e << endl; + return false; + } + _jacobianWorkspace.updateSize(e); + + return true; + } + + int OptimizableGraph::optimize(int /*iterations*/, bool /*online*/) {return 0;} + +double OptimizableGraph::chi2() const +{ + double chi = 0.0; + for (OptimizableGraph::EdgeSet::const_iterator it = this->edges().begin(); it != this->edges().end(); ++it) { + const OptimizableGraph::Edge* e = static_cast(*it); + chi += e->chi2(); + } + return chi; +} + +void OptimizableGraph::push() +{ + for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) { + OptimizableGraph::Vertex* v = static_cast(it->second); + v->push(); + } +} + +void OptimizableGraph::pop() +{ + for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) { + OptimizableGraph::Vertex* v= static_cast(it->second); + v->pop(); + } +} + +void OptimizableGraph::discardTop() +{ + for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) { + OptimizableGraph::Vertex* v= static_cast(it->second); + v->discardTop(); + } +} + +void OptimizableGraph::push(HyperGraph::VertexSet& vset) +{ + for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) { + OptimizableGraph::Vertex* v = static_cast(*it); + v->push(); + } +} + +void OptimizableGraph::pop(HyperGraph::VertexSet& vset) +{ + for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) { + OptimizableGraph::Vertex* v = static_cast(*it); + v->pop(); + } +} + +void OptimizableGraph::discardTop(HyperGraph::VertexSet& vset) +{ + for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) { + OptimizableGraph::Vertex* v = static_cast(*it); + v->discardTop(); + } +} + + void OptimizableGraph::setFixed(HyperGraph::VertexSet& vset, bool fixed) +{ + for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) { + OptimizableGraph::Vertex* v = static_cast(*it); + v->setFixed(fixed); + } +} + + +bool OptimizableGraph::load(istream& is, bool createEdges) +{ + // scna for the paramers in the whole file + if (!_parameters.read(is,&_renamedTypesLookup)) + return false; +#ifndef NDEBUG + cerr << "Loaded " << _parameters.size() << " parameters" << endl; +#endif + is.clear(); + is.seekg(ios_base::beg); + set warnedUnknownTypes; + stringstream currentLine; + string token; + + Factory* factory = Factory::instance(); + HyperGraph::GraphElemBitset elemBitset; + elemBitset[HyperGraph::HGET_PARAMETER] = 1; + elemBitset.flip(); + + Vertex* previousVertex = 0; + Data* previousData = 0; + + while (1) { + int bytesRead = readLine(is, currentLine); + if (bytesRead == -1) + break; + currentLine >> token; + //cerr << "Token=" << token << endl; + if (bytesRead == 0 || token.size() == 0 || token[0] == '#') + continue; + + // handle commands encoded in the file + bool handledCommand = false; + + if (token == "FIX") { + handledCommand = true; + int id; + while (currentLine >> id) { + OptimizableGraph::Vertex* v = static_cast(vertex(id)); + if (v) { +# ifndef NDEBUG + cerr << "Fixing vertex " << v->id() << endl; +# endif + v->setFixed(true); + } else { + cerr << "Warning: Unable to fix vertex with id " << id << ". Not found in the graph." << endl; + } + } + } + + if (handledCommand) + continue; + + // do the mapping to an internal type if it matches + if (_renamedTypesLookup.size() > 0) { + map::const_iterator foundIt = _renamedTypesLookup.find(token); + if (foundIt != _renamedTypesLookup.end()) { + token = foundIt->second; + } + } + + if (! factory->knowsTag(token)) { + if (warnedUnknownTypes.count(token) != 1) { + warnedUnknownTypes.insert(token); + cerr << CL_RED(__PRETTY_FUNCTION__ << " unknown type: " << token) << endl; + } + continue; + } + + HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset); + if (dynamic_cast(element)) { // it's a vertex type + //cerr << "it is a vertex" << endl; + previousData = 0; + Vertex* v = static_cast(element); + int id; + currentLine >> id; + bool r = v->read(currentLine); + if (! r) + cerr << __PRETTY_FUNCTION__ << ": Error reading vertex " << token << " " << id << endl; + v->setId(id); + if (!addVertex(v)) { + cerr << __PRETTY_FUNCTION__ << ": Failure adding Vertex, " << token << " " << id << endl; + delete v; + } else { + previousVertex = v; + } + } + else if (dynamic_cast(element)) { + //cerr << "it is an edge" << endl; + previousData = 0; + Edge* e = static_cast(element); + int numV = e->vertices().size(); + if (_edge_has_id){ + int id; + currentLine >> id; + e->setId(id); + } + //cerr << PVAR(token) << " " << PVAR(numV) << endl; + if (numV == 2) { // it's a pairwise / binary edge type which we handle in a special way + int id1, id2; + currentLine >> id1 >> id2; + Vertex* from = vertex(id1); + Vertex* to = vertex(id2); + int doInit=0; + if ((!from || !to) ) { + if (! createEdges) { + cerr << __PRETTY_FUNCTION__ << ": Unable to find vertices for edge " << token << " " << id1 << " <-> " << id2 << endl; + delete e; + } else { + if (! from) { + from=e->createFrom(); + from->setId(id1); + addVertex(from); + doInit=2; + } + if (! to) { + to=e->createTo(); + to->setId(id2); + addVertex(to); + doInit=1; + } + } + } + if (from && to) { + e->setVertex(0, from); + e->setVertex(1, to); + e->read(currentLine); + if (!addEdge(e)) { + cerr << __PRETTY_FUNCTION__ << ": Unable to add edge " << token << " " << id1 << " <-> " << id2 << endl; + delete e; + } else { + switch (doInit){ + case 1: + { + HyperGraph::VertexSet fromSet; + fromSet.insert(from); + e->initialEstimate(fromSet, to); + break; + } + case 2: + { + HyperGraph::VertexSet toSet; + toSet.insert(to); + e->initialEstimate(toSet, from); + break; + } + default:; + } + } + } + } + else { + vector ids; + ids.resize(numV); + for (int l = 0; l < numV; ++l) + currentLine >> ids[l]; + bool vertsOkay = true; + for (int l = 0; l < numV; ++l) { + e->setVertex(l, vertex(ids[l])); + if (e->vertex(l) == 0) { + vertsOkay = false; + break; + } + } + if (! vertsOkay) { + cerr << __PRETTY_FUNCTION__ << ": Unable to find vertices for edge " << token; + for (int l = 0; l < numV; ++l) { + if (l > 0) + cerr << " <->"; + cerr << " " << ids[l]; + } + delete e; + } else { + bool r = e->read(currentLine); + if (!r || !addEdge(e)) { + cerr << __PRETTY_FUNCTION__ << ": Unable to add edge " << token; + for (int l = 0; l < numV; ++l) { + if (l > 0) + cerr << " <->"; + cerr << " " << ids[l]; + } + delete e; + } + } + } + } else if (dynamic_cast(element)) { // reading in the data packet for the vertex + //cerr << "read data packet " << token << " vertex " << previousVertex->id() << endl; + Data* d = static_cast(element); + bool r = d->read(currentLine); + if (! r) { + cerr << __PRETTY_FUNCTION__ << ": Error reading data " << token << " for vertex " << previousVertex->id() << endl; + delete d; + previousData = 0; + } else if (previousData){ + //cerr << "chaining" << endl; + previousData->setNext(d); + previousData = d; + //cerr << "done" << endl; + } else if (previousVertex){ + //cerr << "embedding in vertex" << endl; + previousVertex->setUserData(d); + previousData = d; + previousVertex = 0; + //cerr << "done" << endl; + } else { + cerr << __PRETTY_FUNCTION__ << ": got data element, but no vertex available" << endl; + delete d; + previousData = 0; + } + } + } // while read line + + return true; +} + +bool OptimizableGraph::load(const char* filename, bool createEdges) +{ + ifstream ifs(filename); + if (!ifs) { + cerr << __PRETTY_FUNCTION__ << " unable to open file " << filename << endl; + return false; + } + return load(ifs, createEdges); +} + +bool OptimizableGraph::save(const char* filename, int level) const +{ + ofstream ofs(filename); + if (!ofs) + return false; + return save(ofs, level); +} + +bool OptimizableGraph::save(ostream& os, int level) const +{ + if (! _parameters.write(os)) + return false; + set verticesToSave; + for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) { + OptimizableGraph::Edge* e = static_cast(*it); + if (e->level() == level) { + for (vector::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) { + verticesToSave.insert(static_cast(*it)); + } + } + } + + for (set::const_iterator it = verticesToSave.begin(); it != verticesToSave.end(); ++it){ + OptimizableGraph::Vertex* v = *it; + saveVertex(os, v); + } + + EdgeContainer edgesToSave; + for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) { + const OptimizableGraph::Edge* e = dynamic_cast(*it); + if (e->level() == level) + edgesToSave.push_back(const_cast(e)); + } + sort(edgesToSave.begin(), edgesToSave.end(), EdgeIDCompare()); + + for (EdgeContainer::const_iterator it = edgesToSave.begin(); it != edgesToSave.end(); ++it) { + OptimizableGraph::Edge* e = *it; + saveEdge(os, e); + } + + return os.good(); +} + + +bool OptimizableGraph::saveSubset(ostream& os, HyperGraph::VertexSet& vset, int level) +{ + if (! _parameters.write(os)) + return false; + + for (HyperGraph::VertexSet::const_iterator it=vset.begin(); it!=vset.end(); it++){ + OptimizableGraph::Vertex* v = dynamic_cast(*it); + saveVertex(os, v); + } + for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) { + OptimizableGraph::Edge* e = dynamic_cast< OptimizableGraph::Edge*>(*it); + if (e->level() != level) + continue; + + bool verticesInEdge = true; + for (vector::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) { + if (vset.find(*it) == vset.end()) { + verticesInEdge = false; + break; + } + } + if (! verticesInEdge) + continue; + + saveEdge(os, e); + } + + return os.good(); +} + +bool OptimizableGraph::saveSubset(ostream& os, HyperGraph::EdgeSet& eset) +{ + if (!_parameters.write(os)) + return false; + std::set vset; + for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end(); ++it) { + HyperGraph::Edge* e = *it; + for (vector::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) { + OptimizableGraph::Vertex* v = static_cast(*it); + vset.insert(v); + } + } + + for (std::set::const_iterator it=vset.begin(); it!=vset.end(); ++it){ + OptimizableGraph::Vertex* v = dynamic_cast(*it); + saveVertex(os, v); + } + + for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end(); ++it) { + OptimizableGraph::Edge* e = dynamic_cast< OptimizableGraph::Edge*>(*it); + saveEdge(os, e); + } + + return os.good(); +} + +void OptimizableGraph::addGraph(OptimizableGraph* g){ + for (HyperGraph::VertexIDMap::iterator it=g->vertices().begin(); it!=g->vertices().end(); ++it){ + OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*)(it->second); + if (vertex(v->id())) + continue; + OptimizableGraph::Vertex* v2=v->clone(); + v2->edges().clear(); + v2->setHessianIndex(-1); + addVertex(v2); + } + for (HyperGraph::EdgeSet::iterator it=g->edges().begin(); it!=g->edges().end(); ++it){ + OptimizableGraph::Edge* e = (OptimizableGraph::Edge*)(*it); + OptimizableGraph::Edge* en = e->clone(); + en->resize(e->vertices().size()); + int cnt = 0; + for (vector::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) { + OptimizableGraph::Vertex* v = (OptimizableGraph::Vertex*) vertex((*it)->id()); + assert(v); + en->setVertex(cnt++, v); + } + addEdge(en); + } +} + +int OptimizableGraph::maxDimension() const{ + int maxDim=0; + for (HyperGraph::VertexIDMap::const_iterator it=vertices().begin(); it!=vertices().end(); ++it){ + const OptimizableGraph::Vertex* v= static_cast< const OptimizableGraph::Vertex*>(it->second); + maxDim = (std::max)(maxDim, v->dimension()); + } + return maxDim; +} + +void OptimizableGraph::setRenamedTypesFromString(const std::string& types) +{ + Factory* factory = Factory::instance(); + vector typesMap = strSplit(types, ","); + for (size_t i = 0; i < typesMap.size(); ++i) { + vector m = strSplit(typesMap[i], "="); + if (m.size() != 2) { + cerr << __PRETTY_FUNCTION__ << ": unable to extract type map from " << typesMap[i] << endl; + continue; + } + string typeInFile = trim(m[0]); + string loadedType = trim(m[1]); + if (! factory->knowsTag(loadedType)) { + cerr << __PRETTY_FUNCTION__ << ": unknown type " << loadedType << endl; + continue; + } + + _renamedTypesLookup[typeInFile] = loadedType; + } + + cerr << "# load look up table" << endl; + for (std::map::const_iterator it = _renamedTypesLookup.begin(); it != _renamedTypesLookup.end(); ++it) { + cerr << "#\t" << it->first << " -> " << it->second << endl; + } +} + +bool OptimizableGraph::isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty, const std::set& vertDims_) const +{ + std::set auxDims; + if (vertDims_.size() == 0) { + auxDims = dimensions(); + } + const set& vertDims = vertDims_.size() == 0 ? auxDims : vertDims_; + bool suitableSolver = true; + if (vertDims.size() == 2) { + if (solverProperty.requiresMarginalize) { + suitableSolver = vertDims.count(solverProperty.poseDim) == 1 && vertDims.count(solverProperty.landmarkDim) == 1; + } + else { + suitableSolver = solverProperty.poseDim == -1; + } + } else if (vertDims.size() == 1) { + suitableSolver = vertDims.count(solverProperty.poseDim) == 1 || solverProperty.poseDim == -1; + } else { + suitableSolver = solverProperty.poseDim == -1 && !solverProperty.requiresMarginalize; + } + return suitableSolver; +} + +std::set OptimizableGraph::dimensions() const +{ + std::set auxDims; + for (VertexIDMap::const_iterator it = vertices().begin(); it != vertices().end(); ++it) { + OptimizableGraph::Vertex* v = static_cast(it->second); + auxDims.insert(v->dimension()); + } + return auxDims; +} + +void OptimizableGraph::preIteration(int iter) +{ + HyperGraphActionSet& actions = _graphActions[AT_PREITERATION]; + if (actions.size() > 0) { + HyperGraphAction::ParametersIteration params(iter); + for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it) { + (*(*it))(this, ¶ms); + } + } +} + +void OptimizableGraph::postIteration(int iter) +{ + HyperGraphActionSet& actions = _graphActions[AT_POSTITERATION]; + if (actions.size() > 0) { + HyperGraphAction::ParametersIteration params(iter); + for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it) { + (*(*it))(this, ¶ms); + } + } +} + +bool OptimizableGraph::addPostIterationAction(HyperGraphAction* action) +{ + std::pair insertResult = _graphActions[AT_POSTITERATION].insert(action); + return insertResult.second; +} + +bool OptimizableGraph::addPreIterationAction(HyperGraphAction* action) +{ + std::pair insertResult = _graphActions[AT_PREITERATION].insert(action); + return insertResult.second; +} + +bool OptimizableGraph::removePreIterationAction(HyperGraphAction* action) +{ + return _graphActions[AT_PREITERATION].erase(action) > 0; +} + +bool OptimizableGraph::removePostIterationAction(HyperGraphAction* action) +{ + return _graphActions[AT_POSTITERATION].erase(action) > 0; +} + +bool OptimizableGraph::saveVertex(std::ostream& os, OptimizableGraph::Vertex* v) const +{ + Factory* factory = Factory::instance(); + string tag = factory->tag(v); + if (tag.size() > 0) { + os << tag << " " << v->id() << " "; + v->write(os); + os << endl; + Data* d=v->userData(); + while (d) { // write the data packet for the vertex + tag = factory->tag(d); + if (tag.size() > 0) { + os << tag << " "; + d->write(os); + os << endl; + } + d=d->next(); + } + if (v->fixed()) { + os << "FIX " << v->id() << endl; + } + return os.good(); + } + return false; +} + +bool OptimizableGraph::saveEdge(std::ostream& os, OptimizableGraph::Edge* e) const +{ + Factory* factory = Factory::instance(); + string tag = factory->tag(e); + if (tag.size() > 0) { + os << tag << " "; + if (_edge_has_id) + os << e->id() << " "; + for (vector::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) { + OptimizableGraph::Vertex* v = static_cast(*it); + os << v->id() << " "; + } + e->write(os); + os << endl; + return os.good(); + } + return false; +} + +void OptimizableGraph::clearParameters() +{ + _parameters.clear(); +} + +bool OptimizableGraph::verifyInformationMatrices(bool verbose) const +{ + bool allEdgeOk = true; + SelfAdjointEigenSolver eigenSolver; + for (OptimizableGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) { + OptimizableGraph::Edge* e = static_cast(*it); + Eigen::MatrixXd::MapType information(e->informationData(), e->dimension(), e->dimension()); + // test on symmetry + bool isSymmetric = information.transpose() == information; + bool okay = isSymmetric; + if (isSymmetric) { + // compute the eigenvalues + eigenSolver.compute(information, Eigen::EigenvaluesOnly); + bool isSPD = eigenSolver.eigenvalues()(0) >= 0.; + okay = okay && isSPD; + } + allEdgeOk = allEdgeOk && okay; + if (! okay) { + if (verbose) { + if (! isSymmetric) + cerr << "Information Matrix for an edge is not symmetric:"; + else + cerr << "Information Matrix for an edge is not SPD:"; + for (size_t i = 0; i < e->vertices().size(); ++i) + cerr << " " << e->vertex(i)->id(); + if (isSymmetric) + cerr << "\teigenvalues: " << eigenSolver.eigenvalues().transpose(); + cerr << endl; + } + } + } + return allEdgeOk; +} + +bool OptimizableGraph::initMultiThreading() +{ +# if (defined G2O_OPENMP) && EIGEN_VERSION_AT_LEAST(3,1,0) + Eigen::initParallel(); +# endif + return true; +} + +} // end namespace + diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimizable_graph.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimizable_graph.h new file mode 100644 index 0000000..9d9b561 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimizable_graph.h @@ -0,0 +1,688 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_AIS_OPTIMIZABLE_GRAPH_HH_ +#define G2O_AIS_OPTIMIZABLE_GRAPH_HH_ + +#include +#include +#include +#include +#include +#include + +#include "openmp_mutex.h" +#include "hyper_graph.h" +#include "parameter.h" +#include "parameter_container.h" +#include "jacobian_workspace.h" + +#include "../stuff/macros.h" + +namespace g2o { + + class HyperGraphAction; + struct OptimizationAlgorithmProperty; + class Cache; + class CacheContainer; + class RobustKernel; + + /** + @addtogroup g2o + */ + /** + This is an abstract class that represents one optimization + problem. It specializes the general graph to contain special + vertices and edges. The vertices represent parameters that can + be optimized, while the edges represent constraints. This class + also provides basic functionalities to handle the backup/restore + of portions of the vertices. + */ + struct OptimizableGraph : public HyperGraph { + + enum ActionType { + AT_PREITERATION, AT_POSTITERATION, + AT_NUM_ELEMENTS, // keep as last element + }; + + typedef std::set HyperGraphActionSet; + + // forward declarations + class Vertex; + class Edge; + + /** + * \brief data packet for a vertex. Extend this class to store in the vertices + * the potential additional information you need (e.g. images, laser scans, ...). + */ + class Data : public HyperGraph::HyperGraphElement + { + friend struct OptimizableGraph; + public: + virtual ~Data(); + Data(); + //! read the data from a stream + virtual bool read(std::istream& is) = 0; + //! write the data to a stream + virtual bool write(std::ostream& os) const = 0; + virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_DATA;} + const Data* next() const {return _next;} + Data* next() {return _next;} + void setNext(Data* next_) { _next = next_; } + protected: + Data* _next; // linked list of multiple data; + }; + + /** + * \brief order vertices based on their ID + */ + struct VertexIDCompare { + bool operator() (const Vertex* v1, const Vertex* v2) const + { + return v1->id() < v2->id(); + } + }; + + /** + * \brief order edges based on the internal ID, which is assigned to the edge in addEdge() + */ + struct EdgeIDCompare { + bool operator() (const Edge* e1, const Edge* e2) const + { + return e1->internalId() < e2->internalId(); + } + }; + + //! vector container for vertices + typedef std::vector VertexContainer; + //! vector container for edges + typedef std::vector EdgeContainer; + + /** + * \brief A general case Vertex for optimization + */ + class Vertex : public HyperGraph::Vertex { + private: + friend struct OptimizableGraph; + public: + Vertex(); + + //! returns a deep copy of the current vertex + virtual Vertex* clone() const ; + + //! the user data associated with this vertex + const Data* userData() const { return _userData; } + Data* userData() { return _userData; } + + void setUserData(Data* obs) { _userData = obs;} + void addUserData(Data* obs) { + if (obs) { + obs->setNext(_userData); + _userData=obs; + } + } + + virtual ~Vertex(); + + //! sets the node to the origin (used in the multilevel stuff) + void setToOrigin() { setToOriginImpl(); updateCache();} + + //! get the element from the hessian matrix + virtual const double& hessian(int i, int j) const = 0; + virtual double& hessian(int i, int j) = 0; + virtual double hessianDeterminant() const = 0; + virtual double* hessianData() = 0; + + /** maps the internal matrix to some external memory location */ + virtual void mapHessianMemory(double* d) = 0; + + /** + * copies the b vector in the array b_ + * @return the number of elements copied + */ + virtual int copyB(double* b_) const = 0; + + //! get the b vector element + virtual const double& b(int i) const = 0; + virtual double& b(int i) = 0; + //! return a pointer to the b vector associated with this vertex + virtual double* bData() = 0; + + /** + * set the b vector part of this vertex to zero + */ + virtual void clearQuadraticForm() = 0; + + /** + * updates the current vertex with the direct solution x += H_ii\b_ii + * @return the determinant of the inverted hessian + */ + virtual double solveDirect(double lambda=0) = 0; + + /** + * sets the initial estimate from an array of double + * Implement setEstimateDataImpl() + * @return true on success + */ + bool setEstimateData(const double* estimate); + + /** + * sets the initial estimate from an array of double + * Implement setEstimateDataImpl() + * @return true on success + */ + bool setEstimateData(const std::vector& estimate) { +#ifndef NDEBUG + int dim = estimateDimension(); + assert((dim == -1) || (estimate.size() == std::size_t(dim))); +#endif + return setEstimateData(&estimate[0]); + }; + + /** + * writes the estimater to an array of double + * @returns true on success + */ + virtual bool getEstimateData(double* estimate) const; + + /** + * writes the estimater to an array of double + * @returns true on success + */ + virtual bool getEstimateData(std::vector& estimate) const { + int dim = estimateDimension(); + if (dim < 0) + return false; + estimate.resize(dim); + return getEstimateData(&estimate[0]); + }; + + /** + * returns the dimension of the extended representation used by get/setEstimate(double*) + * -1 if it is not supported + */ + virtual int estimateDimension() const; + + /** + * sets the initial estimate from an array of double. + * Implement setMinimalEstimateDataImpl() + * @return true on success + */ + bool setMinimalEstimateData(const double* estimate); + + /** + * sets the initial estimate from an array of double. + * Implement setMinimalEstimateDataImpl() + * @return true on success + */ + bool setMinimalEstimateData(const std::vector& estimate) { +#ifndef NDEBUG + int dim = minimalEstimateDimension(); + assert((dim == -1) || (estimate.size() == std::size_t(dim))); +#endif + return setMinimalEstimateData(&estimate[0]); + }; + + /** + * writes the estimate to an array of double + * @returns true on success + */ + virtual bool getMinimalEstimateData(double* estimate) const ; + + /** + * writes the estimate to an array of double + * @returns true on success + */ + virtual bool getMinimalEstimateData(std::vector& estimate) const { + int dim = minimalEstimateDimension(); + if (dim < 0) + return false; + estimate.resize(dim); + return getMinimalEstimateData(&estimate[0]); + }; + + /** + * returns the dimension of the extended representation used by get/setEstimate(double*) + * -1 if it is not supported + */ + virtual int minimalEstimateDimension() const; + + //! backup the position of the vertex to a stack + virtual void push() = 0; + + //! restore the position of the vertex by retrieving the position from the stack + virtual void pop() = 0; + + //! pop the last element from the stack, without restoring the current estimate + virtual void discardTop() = 0; + + //! return the stack size + virtual int stackSize() const = 0; + + /** + * Update the position of the node from the parameters in v. + * Depends on the implementation of oplusImpl in derived classes to actually carry + * out the update. + * Will also call updateCache() to update the caches of depending on the vertex. + */ + void oplus(const double* v) + { + oplusImpl(v); + updateCache(); + } + + //! temporary index of this node in the parameter vector obtained from linearization + int hessianIndex() const { return _hessianIndex;} + int G2O_ATTRIBUTE_DEPRECATED(tempIndex() const) { return hessianIndex();} + //! set the temporary index of the vertex in the parameter blocks + void setHessianIndex(int ti) { _hessianIndex = ti;} + void G2O_ATTRIBUTE_DEPRECATED(setTempIndex(int ti)) { setHessianIndex(ti);} + + //! true => this node is fixed during the optimization + bool fixed() const {return _fixed;} + //! true => this node should be considered fixed during the optimization + void setFixed(bool fixed) { _fixed = fixed;} + + //! true => this node is marginalized out during the optimization + bool marginalized() const {return _marginalized;} + //! true => this node should be marginalized out during the optimization + void setMarginalized(bool marginalized) { _marginalized = marginalized;} + + //! dimension of the estimated state belonging to this node + int dimension() const { return _dimension;} + + //! sets the id of the node in the graph be sure that the graph keeps consistent after changing the id + virtual void setId(int id) {_id = id;} + + //! set the row of this vertex in the Hessian + void setColInHessian(int c) { _colInHessian = c;} + //! get the row of this vertex in the Hessian + int colInHessian() const {return _colInHessian;} + + const OptimizableGraph* graph() const {return _graph;} + + OptimizableGraph* graph() {return _graph;} + + /** + * lock for the block of the hessian and the b vector associated with this vertex, to avoid + * race-conditions if multi-threaded. + */ + void lockQuadraticForm() { _quadraticFormMutex.lock();} + /** + * unlock the block of the hessian and the b vector associated with this vertex + */ + void unlockQuadraticForm() { _quadraticFormMutex.unlock();} + + //! read the vertex from a stream, i.e., the internal state of the vertex + virtual bool read(std::istream& is) = 0; + //! write the vertex to a stream + virtual bool write(std::ostream& os) const = 0; + + virtual void updateCache(); + + CacheContainer* cacheContainer(); + protected: + OptimizableGraph* _graph; + Data* _userData; + int _hessianIndex; + bool _fixed; + bool _marginalized; + int _dimension; + int _colInHessian; + OpenMPMutex _quadraticFormMutex; + + CacheContainer* _cacheContainer; + + /** + * update the position of the node from the parameters in v. + * Implement in your class! + */ + virtual void oplusImpl(const double* v) = 0; + + //! sets the node to the origin (used in the multilevel stuff) + virtual void setToOriginImpl() = 0; + + /** + * writes the estimater to an array of double + * @returns true on success + */ + virtual bool setEstimateDataImpl(const double* ) { return false;} + + /** + * sets the initial estimate from an array of double + * @return true on success + */ + virtual bool setMinimalEstimateDataImpl(const double* ) { return false;} + + }; + + class Edge: public HyperGraph::Edge { + private: + friend struct OptimizableGraph; + public: + Edge(); + virtual ~Edge(); + virtual Edge* clone() const; + + // indicates if all vertices are fixed + virtual bool allVerticesFixed() const = 0; + + // computes the error of the edge and stores it in an internal structure + virtual void computeError() = 0; + + //! sets the measurement from an array of double + //! @returns true on success + virtual bool setMeasurementData(const double* m); + + //! writes the measurement to an array of double + //! @returns true on success + virtual bool getMeasurementData(double* m) const; + + //! returns the dimension of the measurement in the extended representation which is used + //! by get/setMeasurement; + virtual int measurementDimension() const; + + /** + * sets the estimate to have a zero error, based on the current value of the state variables + * returns false if not supported. + */ + virtual bool setMeasurementFromState(); + + //! if NOT NULL, error of this edge will be robustifed with the kernel + RobustKernel* robustKernel() const { return _robustKernel;} + /** + * specify the robust kernel to be used in this edge + */ + void setRobustKernel(RobustKernel* ptr); + + //! returns the error vector cached after calling the computeError; + virtual const double* errorData() const = 0; + virtual double* errorData() = 0; + + //! returns the memory of the information matrix, usable for example with a Eigen::Map + virtual const double* informationData() const = 0; + virtual double* informationData() = 0; + + //! computes the chi2 based on the cached error value, only valid after computeError has been called. + virtual double chi2() const = 0; + + /** + * Linearizes the constraint in the edge. + * Makes side effect on the vertices of the graph by changing + * the parameter vector b and the hessian blocks ii and jj. + * The off diagoinal block is accesed via _hessian. + */ + virtual void constructQuadraticForm() = 0; + + /** + * maps the internal matrix to some external memory location, + * you need to provide the memory before calling constructQuadraticForm + * @param d the memory location to which we map + * @param i index of the vertex i + * @param j index of the vertex j (j > i, upper triangular fashion) + * @param rowMajor if true, will write in rowMajor order to the block. Since EIGEN is columnMajor by default, this results in writing the transposed + */ + virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor) = 0; + + /** + * Linearizes the constraint in the edge in the manifold space, and store + * the result in the given workspace + */ + virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace) = 0; + + /** set the estimate of the to vertex, based on the estimate of the from vertices in the edge. */ + virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) = 0; + + /** + * override in your class if it's possible to initialize the vertices in certain combinations. + * The return value may correspond to the cost for initiliaizng the vertex but should be positive if + * the initialization is possible and negative if not possible. + */ + virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) { (void) from; (void) to; return -1.;} + + //! returns the level of the edge + int level() const { return _level;} + //! sets the level of the edge + void setLevel(int l) { _level=l;} + + //! returns the dimensions of the error function + int dimension() const { return _dimension;} + + virtual Vertex* createFrom() {return 0;} + virtual Vertex* createTo() {return 0;} + + //! read the vertex from a stream, i.e., the internal state of the vertex + virtual bool read(std::istream& is) = 0; + //! write the vertex to a stream + virtual bool write(std::ostream& os) const = 0; + + //! the internal ID of the edge + long long internalId() const { return _internalId;} + + OptimizableGraph* graph(); + const OptimizableGraph* graph() const; + + bool setParameterId(int argNum, int paramId); + inline const Parameter* parameter(int argNo) const {return *_parameters.at(argNo);} + inline size_t numParameters() const {return _parameters.size();} + inline void resizeParameters(size_t newSize) { + _parameters.resize(newSize, 0); + _parameterIds.resize(newSize, -1); + _parameterTypes.resize(newSize, typeid(void*).name()); + } + protected: + int _dimension; + int _level; + RobustKernel* _robustKernel; + long long _internalId; + + std::vector _cacheIds; + + template + bool installParameter(ParameterType*& p, size_t argNo, int paramId=-1){ + if (argNo>=_parameters.size()) + return false; + _parameterIds[argNo] = paramId; + _parameters[argNo] = (Parameter**)&p; + _parameterTypes[argNo] = typeid(ParameterType).name(); + return true; + } + + template + void resolveCache(CacheType*& cache, OptimizableGraph::Vertex*, + const std::string& _type, + const ParameterVector& parameters); + + bool resolveParameters(); + virtual bool resolveCaches(); + + std::vector _parameterTypes; + std::vector _parameters; + std::vector _parameterIds; + }; + + //! returns the vertex number id appropriately casted + inline Vertex* vertex(int id) { return reinterpret_cast(HyperGraph::vertex(id));} + + //! returns the vertex number id appropriately casted + inline const Vertex* vertex (int id) const{ return reinterpret_cast(HyperGraph::vertex(id));} + + //! empty constructor + OptimizableGraph(); + virtual ~OptimizableGraph(); + + //! adds all edges and vertices of the graph g to this graph. + void addGraph(OptimizableGraph* g); + + /** + * adds a new vertex. The new vertex is then "taken". + * @return false if a vertex with the same id as v is already in the graph, true otherwise. + */ + virtual bool addVertex(HyperGraph::Vertex* v, Data* userData); + virtual bool addVertex(HyperGraph::Vertex* v) { return addVertex(v, 0);} + + /** + * adds a new edge. + * The edge should point to the vertices that it is connecting (setFrom/setTo). + * @return false if the insertion does not work (incompatible types of the vertices/missing vertex). true otherwise. + */ + virtual bool addEdge(HyperGraph::Edge* e); + + //! returns the chi2 of the current configuration + double chi2() const; + + //! return the maximum dimension of all vertices in the graph + int maxDimension() const; + + /** + * iterates over all vertices and returns a set of all the vertex dimensions in the graph + */ + std::set dimensions() const; + + /** + * carry out n iterations + * @return the number of performed iterations + */ + virtual int optimize(int iterations, bool online=false); + + //! called at the beginning of an iteration (argument is the number of the iteration) + virtual void preIteration(int); + //! called at the end of an iteration (argument is the number of the iteration) + virtual void postIteration(int); + + //! add an action to be executed before each iteration + bool addPreIterationAction(HyperGraphAction* action); + //! add an action to be executed after each iteration + bool addPostIterationAction(HyperGraphAction* action); + + //! remove an action that should no longer be execured before each iteration + bool removePreIterationAction(HyperGraphAction* action); + //! remove an action that should no longer be execured after each iteration + bool removePostIterationAction(HyperGraphAction* action); + + //! push the estimate of all variables onto a stack + virtual void push(); + //! pop (restore) the estimate of all variables from the stack + virtual void pop(); + //! discard the last backup of the estimate for all variables by removing it from the stack + virtual void discardTop(); + + //! load the graph from a stream. Uses the Factory singleton for creating the vertices and edges. + virtual bool load(std::istream& is, bool createEdges=true); + bool load(const char* filename, bool createEdges=true); + //! save the graph to a stream. Again uses the Factory system. + virtual bool save(std::ostream& os, int level = 0) const; + //! function provided for convenience, see save() above + bool save(const char* filename, int level = 0) const; + + + //! save a subgraph to a stream. Again uses the Factory system. + bool saveSubset(std::ostream& os, HyperGraph::VertexSet& vset, int level = 0); + + //! save a subgraph to a stream. Again uses the Factory system. + bool saveSubset(std::ostream& os, HyperGraph::EdgeSet& eset); + + //! push the estimate of a subset of the variables onto a stack + virtual void push(HyperGraph::VertexSet& vset); + //! pop (restore) the estimate a subset of the variables from the stack + virtual void pop(HyperGraph::VertexSet& vset); + //! ignore the latest stored element on the stack, remove it from the stack but do not restore the estimate + virtual void discardTop(HyperGraph::VertexSet& vset); + + //! fixes/releases a set of vertices + virtual void setFixed(HyperGraph::VertexSet& vset, bool fixed); + + /** + * set the renamed types lookup from a string, format is for example: + * VERTEX_CAM=VERTEX_SE3:EXPMAP,EDGE_PROJECT_P2MC=EDGE_PROJECT_XYZ:EXPMAP + * This will change the occurance of VERTEX_CAM in the file to VERTEX_SE3:EXPMAP + */ + void setRenamedTypesFromString(const std::string& types); + + /** + * test whether a solver is suitable for optimizing this graph. + * @param solverProperty the solver property to evaluate. + * @param vertDims should equal to the set returned by dimensions() to avoid re-evaluating. + */ + bool isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty, const std::set& vertDims = std::set()) const; + + /** + * remove the parameters of the graph + */ + virtual void clearParameters(); + + bool addParameter(Parameter* p) { + return _parameters.addParameter(p); + } + + Parameter* parameter(int id) { + return _parameters.getParameter(id); + } + + /** + * verify that all the information of the edges are semi positive definite, i.e., + * all Eigenvalues are >= 0. + * @param verbose output edges with not PSD information matrix on cerr + * @return true if all edges have PSD information matrix + */ + bool verifyInformationMatrices(bool verbose = false) const; + + // helper functions to save an individual vertex + bool saveVertex(std::ostream& os, Vertex* v) const; + + // helper functions to save an individual edge + bool saveEdge(std::ostream& os, Edge* e) const; + //! the workspace for storing the Jacobians of the graph + JacobianWorkspace& jacobianWorkspace() { return _jacobianWorkspace;} + const JacobianWorkspace& jacobianWorkspace() const { return _jacobianWorkspace;} + + /** + * Eigen starting from version 3.1 requires that we call an initialize + * function, if we perform calls to Eigen from several threads. + * Currently, this function calls Eigen::initParallel if g2o is compiled + * with OpenMP support and Eigen's version is at least 3.1 + */ + static bool initMultiThreading(); + + protected: + std::map _renamedTypesLookup; + long long _nextEdgeId; + std::vector _graphActions; + + // do not watch this. To be removed soon, or integrated in a nice way + bool _edge_has_id; + + ParameterContainer _parameters; + JacobianWorkspace _jacobianWorkspace; + }; + + /** + @} + */ + +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm.cpp new file mode 100644 index 0000000..0888176 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm.cpp @@ -0,0 +1,62 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "optimization_algorithm.h" + +using namespace std; + +namespace g2o { + +OptimizationAlgorithm::OptimizationAlgorithm() : + _optimizer(0) +{ +} + +OptimizationAlgorithm::~OptimizationAlgorithm() +{ +} + +void OptimizationAlgorithm::printProperties(std::ostream& os) const +{ + os << "------------- Algorithm Properties -------------" << endl; + for (PropertyMap::const_iterator it = _properties.begin(); it != _properties.end(); ++it) { + BaseProperty* p = it->second; + os << it->first << "\t" << p->toString() << endl; + } + os << "------------------------------------------------" << endl; +} + +bool OptimizationAlgorithm::updatePropertiesFromString(const std::string& propString) +{ + return _properties.updateMapFromString(propString); +} + +void OptimizationAlgorithm::setOptimizer(SparseOptimizer* optimizer) +{ + _optimizer = optimizer; +} + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm.h new file mode 100644 index 0000000..470a450 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm.h @@ -0,0 +1,117 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_OPTIMIZATION_ALGORITHM_H +#define G2O_OPTIMIZATION_ALGORITHM_H + +#include +#include +#include + +#include "../stuff/property.h" + +#include "hyper_graph.h" +#include "sparse_block_matrix.h" + +namespace g2o { + + class SparseOptimizer; + + /** + * \brief Generic interface for a non-linear solver operating on a graph + */ + class OptimizationAlgorithm + { + public: + enum SolverResult {Terminate=2, OK=1, Fail=-1}; + OptimizationAlgorithm(); + virtual ~OptimizationAlgorithm(); + + /** + * initialize the solver, called once before the first call to solve() + */ + virtual bool init(bool online = false) = 0; + + /** + * Solve one iteration. The SparseOptimizer running on-top will call this + * for the given number of iterations. + * @param iteration indicates the current iteration + */ + virtual SolverResult solve(int iteration, bool online = false) = 0; + + /** + * computes the block diagonal elements of the pattern specified in the input + * and stores them in given SparseBlockMatrix. + * If your solver does not support computing the marginals, return false. + */ + virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices) = 0; + + /** + * update the structures for online processing + */ + virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges) = 0; + + /** + * called by the optimizer if verbose. re-implement, if you want to print something + */ + virtual void printVerbose(std::ostream& os) const {(void) os;}; + + public: + //! return the optimizer operating on + const SparseOptimizer* optimizer() const { return _optimizer;} + SparseOptimizer* optimizer() { return _optimizer;} + + /** + * specify on which optimizer the solver should work on + */ + void setOptimizer(SparseOptimizer* optimizer); + + //! return the properties of the solver + const PropertyMap& properties() const { return _properties;} + + /** + * update the properties from a string, see PropertyMap::updateMapFromString() + */ + bool updatePropertiesFromString(const std::string& propString); + + /** + * print the properties to a stream in a human readable fashion + */ + void printProperties(std::ostream& os) const; + + protected: + SparseOptimizer* _optimizer; ///< the optimizer the solver is working on + PropertyMap _properties; ///< the properties of your solver, use this to store the parameters of your solver + + private: + // Disable the copy constructor and assignment operator + OptimizationAlgorithm(const OptimizationAlgorithm&) { } + OptimizationAlgorithm& operator= (const OptimizationAlgorithm&) { return *this; } + }; + +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.cpp new file mode 100644 index 0000000..f63e071 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.cpp @@ -0,0 +1,229 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "optimization_algorithm_dogleg.h" + +#include + +#include "../stuff/timeutil.h" + +#include "block_solver.h" +#include "sparse_optimizer.h" +#include "solver.h" +#include "batch_stats.h" +using namespace std; + +namespace g2o { + + OptimizationAlgorithmDogleg::OptimizationAlgorithmDogleg(BlockSolverBase* solver) : + OptimizationAlgorithmWithHessian(solver) + { + _userDeltaInit = _properties.makeProperty >("initialDelta", 1e4); + _maxTrialsAfterFailure = _properties.makeProperty >("maxTrialsAfterFailure", 100); + _initialLambda = _properties.makeProperty >("initialLambda", 1e-7); + _lamdbaFactor = _properties.makeProperty >("lambdaFactor", 10.); + _delta = _userDeltaInit->value(); + _lastStep = STEP_UNDEFINED; + _wasPDInAllIterations = true; + } + + OptimizationAlgorithmDogleg::~OptimizationAlgorithmDogleg() + { + } + + OptimizationAlgorithm::SolverResult OptimizationAlgorithmDogleg::solve(int iteration, bool online) + { + assert(_optimizer && "_optimizer not set"); + assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph"); + assert(dynamic_cast(_solver) && "underlying linear solver is not a block solver"); + + BlockSolverBase* blockSolver = static_cast(_solver); + + if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure + bool ok = _solver->buildStructure(); + if (! ok) { + cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl; + return OptimizationAlgorithm::Fail; + } + + // init some members to the current size of the problem + _hsd.resize(_solver->vectorSize()); + _hdl.resize(_solver->vectorSize()); + _auxVector.resize(_solver->vectorSize()); + _delta = _userDeltaInit->value(); + _currentLambda = _initialLambda->value(); + _wasPDInAllIterations = true; + } + + double t=get_monotonic_time(); + _optimizer->computeActiveErrors(); + G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats(); + if (globalStats) { + globalStats->timeResiduals = get_monotonic_time()-t; + t=get_monotonic_time(); + } + + double currentChi = _optimizer->activeRobustChi2(); + + _solver->buildSystem(); + if (globalStats) { + globalStats->timeQuadraticForm = get_monotonic_time()-t; + } + + Eigen::VectorXd::ConstMapType b(_solver->b(), _solver->vectorSize()); + + // compute alpha + _auxVector.setZero(); + blockSolver->multiplyHessian(_auxVector.data(), _solver->b()); + double bNormSquared = b.squaredNorm(); + double alpha = bNormSquared / _auxVector.dot(b); + + _hsd = alpha * b; + double hsdNorm = _hsd.norm(); + double hgnNorm = -1.; + + bool solvedGaussNewton = false; + bool goodStep = false; + int& numTries = _lastNumTries; + numTries = 0; + do { + ++numTries; + + if (! solvedGaussNewton) { + const double minLambda = 1e-12; + const double maxLambda = 1e3; + solvedGaussNewton = true; + // apply a damping factor to enforce positive definite Hessian, if the matrix appeared + // to be not positive definite in at least one iteration before. + // We apply a damping factor to obtain a PD matrix. + bool solverOk = false; + while(!solverOk) { + if (! _wasPDInAllIterations) + _solver->setLambda(_currentLambda, true); // add _currentLambda to the diagonal + solverOk = _solver->solve(); + if (! _wasPDInAllIterations) + _solver->restoreDiagonal(); + _wasPDInAllIterations = _wasPDInAllIterations && solverOk; + if (! _wasPDInAllIterations) { + // simple strategy to control the damping factor + if (solverOk) { + _currentLambda = std::max(minLambda, _currentLambda / (0.5 * _lamdbaFactor->value())); + } else { + _currentLambda *= _lamdbaFactor->value(); + if (_currentLambda > maxLambda) { + _currentLambda = maxLambda; + return Fail; + } + } + } + } + if (!solverOk) { + return Fail; + } + hgnNorm = Eigen::VectorXd::ConstMapType(_solver->x(), _solver->vectorSize()).norm(); + } + + Eigen::VectorXd::ConstMapType hgn(_solver->x(), _solver->vectorSize()); + assert(hgnNorm >= 0. && "Norm of the GN step is not computed"); + + if (hgnNorm < _delta) { + _hdl = hgn; + _lastStep = STEP_GN; + } + else if (hsdNorm > _delta) { + _hdl = _delta / hsdNorm * _hsd; + _lastStep = STEP_SD; + } else { + _auxVector = hgn - _hsd; // b - a + double c = _hsd.dot(_auxVector); + double bmaSquaredNorm = _auxVector.squaredNorm(); + double beta; + if (c <= 0.) + beta = (-c + sqrt(c*c + bmaSquaredNorm * (_delta*_delta - _hsd.squaredNorm()))) / bmaSquaredNorm; + else { + double hsdSqrNorm = _hsd.squaredNorm(); + beta = (_delta*_delta - hsdSqrNorm) / (c + sqrt(c*c + bmaSquaredNorm * (_delta*_delta - hsdSqrNorm))); + } + assert(beta > 0. && beta < 1 && "Error while computing beta"); + _hdl = _hsd + beta * (hgn - _hsd); + _lastStep = STEP_DL; + assert(_hdl.norm() < _delta + 1e-5 && "Computed step does not correspond to the trust region"); + } + + // compute the linear gain + _auxVector.setZero(); + blockSolver->multiplyHessian(_auxVector.data(), _hdl.data()); + double linearGain = -1 * (_auxVector.dot(_hdl)) + 2 * (b.dot(_hdl)); + + // apply the update and see what happens + _optimizer->push(); + _optimizer->update(_hdl.data()); + _optimizer->computeActiveErrors(); + double newChi = _optimizer-> activeRobustChi2(); + double nonLinearGain = currentChi - newChi; + if (fabs(linearGain) < 1e-12) + linearGain = 1e-12; + double rho = nonLinearGain / linearGain; + //cerr << PVAR(nonLinearGain) << " " << PVAR(linearGain) << " " << PVAR(rho) << endl; + if (rho > 0) { // step is good and will be accepted + _optimizer->discardTop(); + goodStep = true; + } else { // recover previous state + _optimizer->pop(); + } + + // update trust region based on the step quality + if (rho > 0.75) + _delta = std::max(_delta, 3. * _hdl.norm()); + else if (rho < 0.25) + _delta *= 0.5; + } while (!goodStep && numTries < _maxTrialsAfterFailure->value()); + if (numTries == _maxTrialsAfterFailure->value() || !goodStep) + return Terminate; + return OK; + } + + void OptimizationAlgorithmDogleg::printVerbose(std::ostream& os) const + { + os + << "\t Delta= " << _delta + << "\t step= " << stepType2Str(_lastStep) + << "\t tries= " << _lastNumTries; + if (! _wasPDInAllIterations) + os << "\t lambda= " << _currentLambda; + } + + const char* OptimizationAlgorithmDogleg::stepType2Str(int stepType) + { + switch (stepType) { + case STEP_SD: return "Descent"; + case STEP_GN: return "GN"; + case STEP_DL: return "Dogleg"; + default: return "Undefined"; + } + } + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.h new file mode 100644 index 0000000..e49761c --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.h @@ -0,0 +1,89 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H +#define G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H + +#include "optimization_algorithm_with_hessian.h" + +namespace g2o { + + class BlockSolverBase; + + /** + * \brief Implementation of Powell's Dogleg Algorithm + */ + class OptimizationAlgorithmDogleg : public OptimizationAlgorithmWithHessian + { + public: + /** \brief type of the step to take */ + enum { + STEP_UNDEFINED, + STEP_SD, STEP_GN, STEP_DL + }; + + public: + /** + * construct the Dogleg algorithm, which will use the given Solver for solving the + * linearized system. + */ + explicit OptimizationAlgorithmDogleg(BlockSolverBase* solver); + virtual ~OptimizationAlgorithmDogleg(); + + virtual SolverResult solve(int iteration, bool online = false); + + virtual void printVerbose(std::ostream& os) const; + + //! return the type of the last step taken by the algorithm + int lastStep() const { return _lastStep;} + //! return the diameter of the trust region + double trustRegion() const { return _delta;} + + //! convert the type into an integer + static const char* stepType2Str(int stepType); + + protected: + // parameters + Property* _maxTrialsAfterFailure; + Property* _userDeltaInit; + // damping to enforce positive definite matrix + Property* _initialLambda; + Property* _lamdbaFactor; + + Eigen::VectorXd _hsd; ///< steepest decent step + Eigen::VectorXd _hdl; ///< final dogleg step + Eigen::VectorXd _auxVector; ///< auxilary vector used to perform multiplications or other stuff + + double _currentLambda; ///< the damping factor to force positive definite matrix + double _delta; ///< trust region + int _lastStep; ///< type of the step taken by the algorithm + bool _wasPDInAllIterations; ///< the matrix we solve was positive definite in all iterations -> if not apply damping + int _lastNumTries; + }; + +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_factory.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_factory.cpp new file mode 100644 index 0000000..a9be96e --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_factory.cpp @@ -0,0 +1,137 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "optimization_algorithm_factory.h" + +#include +#include +#include + +using namespace std; + +namespace g2o { + + AbstractOptimizationAlgorithmCreator::AbstractOptimizationAlgorithmCreator(const OptimizationAlgorithmProperty& p) : + _property(p) + { + } + + OptimizationAlgorithmFactory* OptimizationAlgorithmFactory::factoryInstance = 0; + + OptimizationAlgorithmFactory::OptimizationAlgorithmFactory() + { + } + + OptimizationAlgorithmFactory::~OptimizationAlgorithmFactory() + { + for (CreatorList::iterator it = _creator.begin(); it != _creator.end(); ++it) + delete *it; + } + + OptimizationAlgorithmFactory* OptimizationAlgorithmFactory::instance() + { + if (factoryInstance == 0) { + factoryInstance = new OptimizationAlgorithmFactory; + } + return factoryInstance; + } + + void OptimizationAlgorithmFactory::registerSolver(AbstractOptimizationAlgorithmCreator* c) + { + const string& name = c->property().name; + CreatorList::iterator foundIt = findSolver(name); + if (foundIt != _creator.end()) { + _creator.erase(foundIt); + cerr << "SOLVER FACTORY WARNING: Overwriting Solver creator " << name << endl; + assert(0); + } + _creator.push_back(c); + } + + void OptimizationAlgorithmFactory::unregisterSolver(AbstractOptimizationAlgorithmCreator* c) + { + const string& name = c->property().name; + CreatorList::iterator foundIt = findSolver(name); + if (foundIt != _creator.end()) { + delete *foundIt; + _creator.erase(foundIt); + } + } + + OptimizationAlgorithm* OptimizationAlgorithmFactory::construct(const std::string& name, OptimizationAlgorithmProperty& solverProperty) const + { + CreatorList::const_iterator foundIt = findSolver(name); + if (foundIt != _creator.end()) { + solverProperty = (*foundIt)->property(); + return (*foundIt)->construct(); + } + cerr << "SOLVER FACTORY WARNING: Unable to create solver " << name << endl; + return 0; + } + + void OptimizationAlgorithmFactory::destroy() + { + delete factoryInstance; + factoryInstance = 0; + } + + void OptimizationAlgorithmFactory::listSolvers(std::ostream& os) const + { + size_t solverNameColumnLength = 0; + for (CreatorList::const_iterator it = _creator.begin(); it != _creator.end(); ++it) + solverNameColumnLength = std::max(solverNameColumnLength, (*it)->property().name.size()); + solverNameColumnLength += 4; + + for (CreatorList::const_iterator it = _creator.begin(); it != _creator.end(); ++it) { + const OptimizationAlgorithmProperty& sp = (*it)->property(); + os << sp.name; + for (size_t i = sp.name.size(); i < solverNameColumnLength; ++i) + os << ' '; + os << sp.desc << endl; + } + } + + OptimizationAlgorithmFactory::CreatorList::const_iterator OptimizationAlgorithmFactory::findSolver(const std::string& name) const + { + for (CreatorList::const_iterator it = _creator.begin(); it != _creator.end(); ++it) { + const OptimizationAlgorithmProperty& sp = (*it)->property(); + if (sp.name == name) + return it; + } + return _creator.end(); + } + + OptimizationAlgorithmFactory::CreatorList::iterator OptimizationAlgorithmFactory::findSolver(const std::string& name) + { + for (CreatorList::iterator it = _creator.begin(); it != _creator.end(); ++it) { + const OptimizationAlgorithmProperty& sp = (*it)->property(); + if (sp.name == name) + return it; + } + return _creator.end(); + } + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_factory.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_factory.h new file mode 100644 index 0000000..b8bf26e --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_factory.h @@ -0,0 +1,167 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_OPTMIZATION_ALGORITHM_PROPERTY_H +#define G2O_OPTMIZATION_ALGORITHM_PROPERTY_H + +#include "../../config.h" +#include "../stuff/misc.h" +#include "optimization_algorithm_property.h" + +#include +#include +#include + + +// define to get some verbose output +//#define G2O_DEBUG_OPTIMIZATION_ALGORITHM_FACTORY + +namespace g2o { + + // forward decl + class OptimizationAlgorithm; + class SparseOptimizer; + + /** + * \brief base for allocating an optimization algorithm + * + * Allocating a solver for a given optimizer. The method construct() has to be + * implemented in your derived class to allocate the desired solver. + */ + class AbstractOptimizationAlgorithmCreator + { + public: + AbstractOptimizationAlgorithmCreator(const OptimizationAlgorithmProperty& p); + virtual ~AbstractOptimizationAlgorithmCreator() { } + //! allocate a solver operating on optimizer, re-implement for your creator + virtual OptimizationAlgorithm* construct() = 0; + //! return the properties of the solver + const OptimizationAlgorithmProperty& property() const { return _property;} + protected: + OptimizationAlgorithmProperty _property; + }; + + /** + * \brief create solvers based on their short name + * + * Factory to allocate solvers based on their short name. + * The Factory is implemented as a sigleton and the single + * instance can be accessed via the instance() function. + */ + class OptimizationAlgorithmFactory + { + public: + typedef std::list CreatorList; + + //! return the instance + static OptimizationAlgorithmFactory* instance(); + + //! free the instance + static void destroy(); + + /** + * register a specific creator for allocating a solver + */ + void registerSolver(AbstractOptimizationAlgorithmCreator* c); + + /** + * unregister a specific creator for allocating a solver + */ + void unregisterSolver(AbstractOptimizationAlgorithmCreator* c); + + /** + * construct a solver based on its name, e.g., var, fix3_2_cholmod + */ + OptimizationAlgorithm* construct(const std::string& tag, OptimizationAlgorithmProperty& solverProperty) const; + + //! list the known solvers into a stream + void listSolvers(std::ostream& os) const; + + //! return the underlying list of creators + const CreatorList& creatorList() const { return _creator;} + + protected: + OptimizationAlgorithmFactory(); + ~OptimizationAlgorithmFactory(); + + CreatorList _creator; + + CreatorList::const_iterator findSolver(const std::string& name) const; + CreatorList::iterator findSolver(const std::string& name); + + private: + static OptimizationAlgorithmFactory* factoryInstance; + }; + + class RegisterOptimizationAlgorithmProxy + { + public: + RegisterOptimizationAlgorithmProxy(AbstractOptimizationAlgorithmCreator* c) + { + _creator = c; +#ifdef G2O_DEBUG_OPTIMIZATION_ALGORITHM_FACTORY + std::cout << __FUNCTION__ << ": Registering " << _creator->property().name << " of type " << typeid(*_creator).name() << std::endl; +#endif + OptimizationAlgorithmFactory::instance()->registerSolver(c); + } + + ~RegisterOptimizationAlgorithmProxy() + { +#ifdef G2O_DEBUG_OPTIMIZATION_ALGORITHM_FACTORY + std::cout << __FUNCTION__ << ": Unregistering " << _creator->property().name << std::endl; +#endif + OptimizationAlgorithmFactory::instance()->unregisterSolver(_creator); + } + private: + AbstractOptimizationAlgorithmCreator* _creator; + }; + +} + +#if defined _MSC_VER && defined G2O_SHARED_LIBS +# define G2O_OAF_EXPORT __declspec(dllexport) +# define G2O_OAF_IMPORT __declspec(dllimport) +#else +# define G2O_OAF_EXPORT +# define G2O_OAF_IMPORT +#endif + +#define G2O_REGISTER_OPTIMIZATION_LIBRARY(libraryname) \ + extern "C" void G2O_OAF_EXPORT g2o_optimization_library_##libraryname(void) {} + +#define G2O_USE_OPTIMIZATION_LIBRARY(libraryname) \ + extern "C" void G2O_OAF_IMPORT g2o_optimization_library_##libraryname(void); \ + static g2o::ForceLinker g2o_force_optimization_algorithm_library_##libraryname(g2o_optimization_library_##libraryname); + +#define G2O_REGISTER_OPTIMIZATION_ALGORITHM(optimizername, instance) \ + extern "C" void G2O_OAF_EXPORT g2o_optimization_algorithm_##optimizername(void) {} \ + static g2o::RegisterOptimizationAlgorithmProxy g_optimization_algorithm_proxy_##optimizername(instance); + +#define G2O_USE_OPTIMIZATION_ALGORITHM(optimizername) \ + extern "C" void G2O_OAF_IMPORT g2o_optimization_algorithm_##optimizername(void); \ + static g2o::ForceLinker g2o_force_optimization_algorithm_link_##optimizername(g2o_optimization_algorithm_##optimizername); + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.cpp new file mode 100644 index 0000000..04ca445 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.cpp @@ -0,0 +1,101 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "optimization_algorithm_gauss_newton.h" + +#include + +#include "../stuff/timeutil.h" +#include "../stuff/macros.h" + +#include "solver.h" +#include "batch_stats.h" +#include "sparse_optimizer.h" +using namespace std; + +namespace g2o { + + OptimizationAlgorithmGaussNewton::OptimizationAlgorithmGaussNewton(Solver* solver) : + OptimizationAlgorithmWithHessian(solver) + { + } + + OptimizationAlgorithmGaussNewton::~OptimizationAlgorithmGaussNewton() + { + } + + OptimizationAlgorithm::SolverResult OptimizationAlgorithmGaussNewton::solve(int iteration, bool online) + { + assert(_optimizer && "_optimizer not set"); + assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph"); + bool ok = true; + + //here so that correct component for max-mixtures can be computed before the build structure + double t=get_monotonic_time(); + _optimizer->computeActiveErrors(); + G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats(); + if (globalStats) { + globalStats->timeResiduals = get_monotonic_time()-t; + } + + if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure + ok = _solver->buildStructure(); + if (! ok) { + cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl; + return OptimizationAlgorithm::Fail; + } + } + + t=get_monotonic_time(); + _solver->buildSystem(); + if (globalStats) { + globalStats->timeQuadraticForm = get_monotonic_time()-t; + t=get_monotonic_time(); + } + + ok = _solver->solve(); + if (globalStats) { + globalStats->timeLinearSolution = get_monotonic_time()-t; + t=get_monotonic_time(); + } + + _optimizer->update(_solver->x()); + if (globalStats) { + globalStats->timeUpdate = get_monotonic_time()-t; + } + if (ok) + return OK; + else + return Fail; + } + + void OptimizationAlgorithmGaussNewton::printVerbose(std::ostream& os) const + { + os + << "\t schur= " << _solver->schur(); + } + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h new file mode 100644 index 0000000..409aa27 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h @@ -0,0 +1,54 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H +#define G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H + +#include "optimization_algorithm_with_hessian.h" + +namespace g2o { + + /** + * \brief Implementation of the Gauss Newton Algorithm + */ + class OptimizationAlgorithmGaussNewton : public OptimizationAlgorithmWithHessian + { + public: + /** + * construct the Gauss Newton algorithm, which use the given Solver for solving the + * linearized system. + */ + explicit OptimizationAlgorithmGaussNewton(Solver* solver); + virtual ~OptimizationAlgorithmGaussNewton(); + + virtual SolverResult solve(int iteration, bool online = false); + + virtual void printVerbose(std::ostream& os) const; + }; + +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.cpp new file mode 100644 index 0000000..067678f --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.cpp @@ -0,0 +1,214 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +// Modified Raul Mur Artal (2014) +// - Stop criterium (solve function) + +#include "optimization_algorithm_levenberg.h" + +#include + +#include "../stuff/timeutil.h" + +#include "sparse_optimizer.h" +#include "solver.h" +#include "batch_stats.h" +using namespace std; + +namespace g2o { + + OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(Solver* solver) : + OptimizationAlgorithmWithHessian(solver) + { + _currentLambda = -1.; + _tau = 1e-5; // Carlos: originally 1e-5 + _goodStepUpperScale = 2./3.; + _goodStepLowerScale = 1./3.; + _userLambdaInit = _properties.makeProperty >("initialLambda", 0.); + _maxTrialsAfterFailure = _properties.makeProperty >("maxTrialsAfterFailure", 10); // Carlos: Originally 10 iterations + _ni=2.; + _levenbergIterations = 0; + _nBad = 0; + } + + OptimizationAlgorithmLevenberg::~OptimizationAlgorithmLevenberg() + { + } + + OptimizationAlgorithm::SolverResult OptimizationAlgorithmLevenberg::solve(int iteration, bool online) + { + assert(_optimizer && "_optimizer not set"); + assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph"); + + if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure + bool ok = _solver->buildStructure(); + if (! ok) { + cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl; + return OptimizationAlgorithm::Fail; + } + } + + double t=get_monotonic_time(); + _optimizer->computeActiveErrors(); + G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats(); + if (globalStats) { + globalStats->timeResiduals = get_monotonic_time()-t; + t=get_monotonic_time(); + } + + double currentChi = _optimizer->activeRobustChi2(); + double tempChi=currentChi; + + double iniChi = currentChi; + + _solver->buildSystem(); + if (globalStats) { + globalStats->timeQuadraticForm = get_monotonic_time()-t; + } + + // core part of the Levenbarg algorithm + if (iteration == 0) { + _currentLambda = computeLambdaInit(); + _ni = 2; + _nBad = 0; + } + + double rho=0; + int& qmax = _levenbergIterations; + qmax = 0; + do { + _optimizer->push(); + if (globalStats) { + globalStats->levenbergIterations++; + t=get_monotonic_time(); + } + // update the diagonal of the system matrix + _solver->setLambda(_currentLambda, true); + bool ok2 = _solver->solve(); + if (globalStats) { + globalStats->timeLinearSolution+=get_monotonic_time()-t; + t=get_monotonic_time(); + } + _optimizer->update(_solver->x()); + if (globalStats) { + globalStats->timeUpdate = get_monotonic_time()-t; + } + + // restore the diagonal + _solver->restoreDiagonal(); + + _optimizer->computeActiveErrors(); + tempChi = _optimizer->activeRobustChi2(); + // cout << "tempChi: " << tempChi << endl; + if (! ok2) + tempChi=std::numeric_limits::max(); + + rho = (currentChi-tempChi); + double scale = computeScale(); + scale += 1e-3; // make sure it's non-zero :) + rho /= scale; + + if (rho>0 && g2o_isfinite(tempChi)){ // last step was good + double alpha = 1.-pow((2*rho-1),3); + // crop lambda between minimum and maximum factors + alpha = (std::min)(alpha, _goodStepUpperScale); + double scaleFactor = (std::max)(_goodStepLowerScale, alpha); + _currentLambda *= scaleFactor; + _ni = 2; + currentChi=tempChi; + _optimizer->discardTop(); + } else { + _currentLambda*=_ni; + _ni*=2; + _optimizer->pop(); // restore the last state before trying to optimize + } + qmax++; + } while (rho<0 && qmax < _maxTrialsAfterFailure->value() && ! _optimizer->terminate()); + + if (qmax == _maxTrialsAfterFailure->value() || rho==0) + { + // cout << "qmax = " << qmax << " rho = " << rho << endl; + return Terminate; + } + + //Stop criterium (Raul) + if((iniChi-currentChi)*1e3=3) + { + return Terminate; + } + + return OK; + } + + double OptimizationAlgorithmLevenberg::computeLambdaInit() const + { + if (_userLambdaInit->value() > 0) + return _userLambdaInit->value(); + double maxDiagonal=0.; + for (size_t k = 0; k < _optimizer->indexMapping().size(); k++) { + OptimizableGraph::Vertex* v = _optimizer->indexMapping()[k]; + assert(v); + int dim = v->dimension(); + for (int j = 0; j < dim; ++j){ + maxDiagonal = std::max(fabs(v->hessian(j,j)),maxDiagonal); + } + } + return _tau*maxDiagonal; + } + + double OptimizationAlgorithmLevenberg::computeScale() const + { + double scale = 0.; + for (size_t j=0; j < _solver->vectorSize(); j++){ + scale += _solver->x()[j] * (_currentLambda * _solver->x()[j] + _solver->b()[j]); + } + return scale; + } + + void OptimizationAlgorithmLevenberg::setMaxTrialsAfterFailure(int max_trials) + { + _maxTrialsAfterFailure->setValue(max_trials); + } + + void OptimizationAlgorithmLevenberg::setUserLambdaInit(double lambda) + { + _userLambdaInit->setValue(lambda); + } + + void OptimizationAlgorithmLevenberg::printVerbose(std::ostream& os) const + { + os + << "\t schur= " << _solver->schur() + << "\t lambda= " << FIXED(_currentLambda) + << "\t levenbergIter= " << _levenbergIterations; + } + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h new file mode 100644 index 0000000..bc4a4a0 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h @@ -0,0 +1,92 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_SOLVER_LEVENBERG_H +#define G2O_SOLVER_LEVENBERG_H + +#include "optimization_algorithm_with_hessian.h" + +namespace g2o { + + /** + * \brief Implementation of the Levenberg Algorithm + */ + class OptimizationAlgorithmLevenberg : public OptimizationAlgorithmWithHessian + { + public: + /** + * construct the Levenberg algorithm, which will use the given Solver for solving the + * linearized system. + */ + explicit OptimizationAlgorithmLevenberg(Solver* solver); + virtual ~OptimizationAlgorithmLevenberg(); + + virtual SolverResult solve(int iteration, bool online = false); + + virtual void printVerbose(std::ostream& os) const; + + //! return the currently used damping factor + double currentLambda() const { return _currentLambda;} + + //! the number of internal iteration if an update step increases chi^2 within Levenberg-Marquardt + void setMaxTrialsAfterFailure(int max_trials); + + //! get the number of inner iterations for Levenberg-Marquardt + int maxTrialsAfterFailure() const { return _maxTrialsAfterFailure->value();} + + //! return the lambda set by the user, if < 0 the SparseOptimizer will compute the initial lambda + double userLambdaInit() {return _userLambdaInit->value();} + //! specify the initial lambda used for the first iteraion, if not given the SparseOptimizer tries to compute a suitable value + void setUserLambdaInit(double lambda); + + //! return the number of levenberg iterations performed in the last round + int levenbergIteration() { return _levenbergIterations;} + + protected: + // Levenberg parameters + Property* _maxTrialsAfterFailure; + Property* _userLambdaInit; + double _currentLambda; + double _tau; + double _goodStepLowerScale; ///< lower bound for lambda decrease if a good LM step + double _goodStepUpperScale; ///< upper bound for lambda decrease if a good LM step + double _ni; + int _levenbergIterations; ///< the numer of levenberg iterations performed to accept the last step + //RAUL + int _nBad; + + /** + * helper for Levenberg, this function computes the initial damping factor, if the user did not + * specify an own value, see setUserLambdaInit() + */ + double computeLambdaInit() const; + double computeScale() const; + + }; + +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_property.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_property.h new file mode 100644 index 0000000..62abb0e --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_property.h @@ -0,0 +1,57 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H +#define G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H + +#include + +namespace g2o { + +/** + * \brief describe the properties of a solver + */ +struct OptimizationAlgorithmProperty +{ + std::string name; ///< name of the solver, e.g., var + std::string desc; ///< short description of the solver + std::string type; ///< type of solver, e.g., "CSparse Cholesky", "PCG" + bool requiresMarginalize; ///< whether the solver requires marginalization of landmarks + int poseDim; ///< dimension of the pose vertices (-1 if variable) + int landmarkDim; ///< dimension of the landmar vertices (-1 if variable) + OptimizationAlgorithmProperty() : + name(), desc(), type(), requiresMarginalize(false), poseDim(-1), landmarkDim(-1) + { + } + OptimizationAlgorithmProperty(const std::string& name_, const std::string& desc_, const std::string& type_, bool requiresMarginalize_, int poseDim_, int landmarkDim_) : + name(name_), desc(desc_), type(type_), requiresMarginalize(requiresMarginalize_), poseDim(poseDim_), landmarkDim(landmarkDim_) + { + } +}; + +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.cpp new file mode 100644 index 0000000..5c23a78 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.cpp @@ -0,0 +1,101 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "optimization_algorithm_with_hessian.h" + +#include "solver.h" +#include "optimizable_graph.h" +#include "sparse_optimizer.h" + +#include +using namespace std; + +namespace g2o { + + OptimizationAlgorithmWithHessian::OptimizationAlgorithmWithHessian(Solver* solver) : + OptimizationAlgorithm(), + _solver(solver) + { + _writeDebug = _properties.makeProperty >("writeDebug", true); + } + + OptimizationAlgorithmWithHessian::~OptimizationAlgorithmWithHessian() + { + delete _solver; + } + + bool OptimizationAlgorithmWithHessian::init(bool online) + { + assert(_optimizer && "_optimizer not set"); + assert(_solver && "Solver not set"); + _solver->setWriteDebug(_writeDebug->value()); + bool useSchur=false; + for (OptimizableGraph::VertexContainer::const_iterator it=_optimizer->activeVertices().begin(); it!=_optimizer->activeVertices().end(); ++it) { + OptimizableGraph::Vertex* v= *it; + if (v->marginalized()){ + useSchur=true; + break; + } + } + if (useSchur){ + if (_solver->supportsSchur()) + _solver->setSchur(true); + } else { + if (_solver->supportsSchur()) + _solver->setSchur(false); + } + + bool initState = _solver->init(_optimizer, online); + return initState; + } + + bool OptimizationAlgorithmWithHessian::computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices) + { + return _solver ? _solver->computeMarginals(spinv, blockIndices) : false; + } + + bool OptimizationAlgorithmWithHessian::buildLinearStructure() + { + return _solver ? _solver->buildStructure() : false; + } + + void OptimizationAlgorithmWithHessian::updateLinearSystem() + { + if (_solver) + _solver->buildSystem(); + } + + bool OptimizationAlgorithmWithHessian::updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges) + { + return _solver ? _solver->updateStructure(vset, edges) : false; + } + + void OptimizationAlgorithmWithHessian::setWriteDebug(bool writeDebug) + { + _writeDebug->setValue(writeDebug); + } + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h new file mode 100644 index 0000000..cf5da58 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h @@ -0,0 +1,72 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H +#define G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H + +#include "optimization_algorithm.h" + +namespace g2o { + + class Solver; + + /** + * \brief Base for solvers operating on the approximated Hessian, e.g., Gauss-Newton, Levenberg + */ + class OptimizationAlgorithmWithHessian : public OptimizationAlgorithm + { + public: + explicit OptimizationAlgorithmWithHessian(Solver* solver); + virtual ~OptimizationAlgorithmWithHessian(); + + virtual bool init(bool online = false); + + virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices); + + virtual bool buildLinearStructure(); + + virtual void updateLinearSystem(); + + virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges); + + //! return the underlying solver used to solve the linear system + Solver* solver() { return _solver;} + + /** + * write debug output of the Hessian if system is not positive definite + */ + virtual void setWriteDebug(bool writeDebug); + virtual bool writeDebug() const { return _writeDebug->value();} + + protected: + Solver* _solver; + Property* _writeDebug; + + }; + +}// end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/parameter.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/parameter.cpp new file mode 100644 index 0000000..345f9c0 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/parameter.cpp @@ -0,0 +1,40 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "parameter.h" + +namespace g2o { + + Parameter::Parameter() : _id(-1) + { + } + + void Parameter::setId(int id_) + { + _id = id_; + } + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/parameter.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/parameter.h new file mode 100644 index 0000000..6def664 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/parameter.h @@ -0,0 +1,56 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_GRAPH_PARAMETER_HH_ +#define G2O_GRAPH_PARAMETER_HH_ + +#include + +#include "hyper_graph.h" + +namespace g2o { + + class Parameter : public HyperGraph::HyperGraphElement + { + public: + Parameter(); + virtual ~Parameter() {}; + //! read the data from a stream + virtual bool read(std::istream& is) = 0; + //! write the data to a stream + virtual bool write(std::ostream& os) const = 0; + int id() const {return _id;} + void setId(int id_); + virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_PARAMETER;} + protected: + int _id; + }; + + typedef std::vector ParameterVector; + +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/parameter_container.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/parameter_container.cpp new file mode 100644 index 0000000..d012331 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/parameter_container.cpp @@ -0,0 +1,142 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "parameter_container.h" + +#include + +#include "factory.h" +#include "parameter.h" + +#include "../stuff/macros.h" +#include "../stuff/color_macros.h" +#include "../stuff/string_tools.h" + +namespace g2o { + + using namespace std; + + ParameterContainer::ParameterContainer(bool isMainStorage_) : + _isMainStorage(isMainStorage_) + { + } + + void ParameterContainer::clear() { + if (!_isMainStorage) + return; + for (iterator it = begin(); it!=end(); it++){ + delete it->second; + } + BaseClass::clear(); + } + + ParameterContainer::~ParameterContainer(){ + clear(); + } + + bool ParameterContainer::addParameter(Parameter* p){ + if (p->id()<0) + return false; + iterator it=find(p->id()); + if (it!=end()) + return false; + insert(make_pair(p->id(), p)); + return true; + } + + Parameter* ParameterContainer::getParameter(int id) { + iterator it=find(id); + if (it==end()) + return 0; + return it->second; + } + + Parameter* ParameterContainer::detachParameter(int id){ + iterator it=find(id); + if (it==end()) + return 0; + Parameter* p=it->second; + erase(it); + return p; + } + + bool ParameterContainer::write(std::ostream& os) const{ + Factory* factory = Factory::instance(); + for (const_iterator it=begin(); it!=end(); it++){ + os << factory->tag(it->second) << " "; + os << it->second->id() << " "; + it->second->write(os); + os << endl; + } + return true; + } + + bool ParameterContainer::read(std::istream& is, const std::map* _renamedTypesLookup){ + stringstream currentLine; + string token; + + Factory* factory = Factory::instance(); + HyperGraph::GraphElemBitset elemBitset; + elemBitset[HyperGraph::HGET_PARAMETER] = 1; + + while (1) { + int bytesRead = readLine(is, currentLine); + if (bytesRead == -1) + break; + currentLine >> token; + if (bytesRead == 0 || token.size() == 0 || token[0] == '#') + continue; + if (_renamedTypesLookup && _renamedTypesLookup->size()>0){ + map::const_iterator foundIt = _renamedTypesLookup->find(token); + if (foundIt != _renamedTypesLookup->end()) { + token = foundIt->second; + } + } + + HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset); + if (! element) // not a parameter or otherwise unknown tag + continue; + assert(element->elementType() == HyperGraph::HGET_PARAMETER && "Should be a param"); + + Parameter* p = static_cast(element); + int pid; + currentLine >> pid; + p->setId(pid); + bool r = p->read(currentLine); + if (! r) { + cerr << __PRETTY_FUNCTION__ << ": Error reading data " << token << " for parameter " << pid << endl; + delete p; + } else { + if (! addParameter(p) ){ + cerr << __PRETTY_FUNCTION__ << ": Parameter of type:" << token << " id:" << pid << " already defined" << endl; + } + } + } // while read line + + return true; + } + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/parameter_container.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/parameter_container.h new file mode 100644 index 0000000..eef6c2f --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/parameter_container.h @@ -0,0 +1,74 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_GRAPH_PARAMETER_CONTAINER_HH_ +#define G2O_GRAPH_PARAMETER_CONTAINER_HH_ + +#include +#include +#include + +namespace g2o { + + class Parameter; + + /** + * \brief map id to parameters + */ + class ParameterContainer : protected std::map + { + public: + typedef std::map BaseClass; + + /** + * create a container for the parameters. + * @param isMainStorage_ pointers to the parameters are owned by this container, i.e., freed in its constructor + */ + ParameterContainer(bool isMainStorage_=true); + virtual ~ParameterContainer(); + //! add parameter to the container + bool addParameter(Parameter* p); + //! return a parameter based on its ID + Parameter* getParameter(int id); + //! remove a parameter from the container, i.e., the user now owns the pointer + Parameter* detachParameter(int id); + //! read parameters from a stream + virtual bool read(std::istream& is, const std::map* renamedMap =0); + //! write the data to a stream + virtual bool write(std::ostream& os) const; + bool isMainStorage() const {return _isMainStorage;} + void clear(); + + // stuff of the base class that should re-appear + using BaseClass::size; + + protected: + bool _isMainStorage; + }; + +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/robust_kernel.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/robust_kernel.cpp new file mode 100644 index 0000000..ac6776d --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/robust_kernel.cpp @@ -0,0 +1,46 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "robust_kernel.h" + +namespace g2o { + +RobustKernel::RobustKernel() : + _delta(1.) +{ +} + +RobustKernel::RobustKernel(double delta) : + _delta(delta) +{ +} + +void RobustKernel::setDelta(double delta) +{ + _delta = delta; +} + +} // end namespace g2o diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/robust_kernel.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/robust_kernel.h new file mode 100644 index 0000000..29e1394 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/robust_kernel.h @@ -0,0 +1,81 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_ROBUST_KERNEL_H +#define G2O_ROBUST_KERNEL_H + +#ifdef _MSC_VER +#include +#else +#include +#endif +#include + + +namespace g2o { + + /** + * \brief base for all robust cost functions + * + * Note in all the implementations for the other cost functions the e in the + * funtions corresponds to the sqaured errors, i.e., the robustification + * functions gets passed the squared error. + * + * e.g. the robustified least squares function is + * + * chi^2 = sum_{e} rho( e^T Omega e ) + */ + class RobustKernel + { + public: + RobustKernel(); + explicit RobustKernel(double delta); + virtual ~RobustKernel() {} + /** + * compute the scaling factor for a error: + * The error is e^T Omega e + * The output rho is + * rho[0]: The actual scaled error value + * rho[1]: First derivative of the scaling function + * rho[2]: Second derivative of the scaling function + */ + virtual void robustify(double squaredError, Eigen::Vector3d& rho) const = 0; + + /** + * set the window size of the error. A squared error above delta^2 is considered + * as outlier in the data. + */ + virtual void setDelta(double delta); + double delta() const { return _delta;} + + protected: + double _delta; + }; + typedef std::tr1::shared_ptr RobustKernelPtr; + +} // end namespace g2o + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/robust_kernel_factory.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/robust_kernel_factory.cpp new file mode 100644 index 0000000..1d028cf --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/robust_kernel_factory.cpp @@ -0,0 +1,111 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "robust_kernel_factory.h" +#include "robust_kernel.h" + +#include + +using namespace std; + +namespace g2o { + +RobustKernelFactory* RobustKernelFactory::factoryInstance = 0; + +RobustKernelFactory::RobustKernelFactory() +{ +} + +RobustKernelFactory::~RobustKernelFactory() +{ + for (CreatorMap::iterator it = _creator.begin(); it != _creator.end(); ++it) { + delete it->second; + } + _creator.clear(); +} + +RobustKernelFactory* RobustKernelFactory::instance() +{ + if (factoryInstance == 0) { + factoryInstance = new RobustKernelFactory; + } + + return factoryInstance; +} + +void RobustKernelFactory::registerRobustKernel(const std::string& tag, AbstractRobustKernelCreator* c) +{ + CreatorMap::const_iterator foundIt = _creator.find(tag); + if (foundIt != _creator.end()) { + cerr << "RobustKernelFactory WARNING: Overwriting robust kernel tag " << tag << endl; + assert(0); + } + + _creator[tag] = c; +} + +void RobustKernelFactory::unregisterType(const std::string& tag) +{ + CreatorMap::iterator tagPosition = _creator.find(tag); + if (tagPosition != _creator.end()) { + AbstractRobustKernelCreator* c = tagPosition->second; + delete c; + _creator.erase(tagPosition); + } +} + +RobustKernel* RobustKernelFactory::construct(const std::string& tag) const +{ + CreatorMap::const_iterator foundIt = _creator.find(tag); + if (foundIt != _creator.end()) { + return foundIt->second->construct(); + } + return 0; +} + +AbstractRobustKernelCreator* RobustKernelFactory::creator(const std::string& tag) const +{ + CreatorMap::const_iterator foundIt = _creator.find(tag); + if (foundIt != _creator.end()) { + return foundIt->second; + } + return 0; +} + +void RobustKernelFactory::fillKnownKernels(std::vector& types) const +{ + types.clear(); + for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it) + types.push_back(it->first); +} + +void RobustKernelFactory::destroy() +{ + delete factoryInstance; + factoryInstance = 0; +} + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/robust_kernel_factory.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/robust_kernel_factory.h new file mode 100644 index 0000000..bcdec07 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/robust_kernel_factory.h @@ -0,0 +1,151 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_ROBUST_KERNEL_FACTORY_H +#define G2O_ROBUST_KERNEL_FACTORY_H + +#include "../stuff/misc.h" + +#include +#include +#include +#include + +namespace g2o { + + class RobustKernel; + + /** + * \brief Abstract interface for allocating a robust kernel + */ + class AbstractRobustKernelCreator + { + public: + /** + * create a hyper graph element. Has to implemented in derived class. + */ + virtual RobustKernel* construct() = 0; + virtual ~AbstractRobustKernelCreator() { } + }; + + /** + * \brief templatized creator class which creates graph elements + */ + template + class RobustKernelCreator : public AbstractRobustKernelCreator + { + public: + RobustKernel* construct() { return new T;} + }; + + /** + * \brief create robust kernels based on their human readable name + */ + class RobustKernelFactory + { + public: + + //! return the instance + static RobustKernelFactory* instance(); + + //! free the instance + static void destroy(); + + /** + * register a tag for a specific creator + */ + void registerRobustKernel(const std::string& tag, AbstractRobustKernelCreator* c); + + /** + * unregister a tag for a specific creator + */ + void unregisterType(const std::string& tag); + + /** + * construct a robust kernel based on its tag + */ + RobustKernel* construct(const std::string& tag) const; + + /** + * return the creator for a specific tag + */ + AbstractRobustKernelCreator* creator(const std::string& tag) const; + + /** + * get a list of all known robust kernels + */ + void fillKnownKernels(std::vector& types) const; + + protected: + + typedef std::map CreatorMap; + RobustKernelFactory(); + ~RobustKernelFactory(); + + CreatorMap _creator; ///< look-up map for the existing creators + + private: + static RobustKernelFactory* factoryInstance; + }; + + template + class RegisterRobustKernelProxy + { + public: + RegisterRobustKernelProxy(const std::string& name) : _name(name) + { + RobustKernelFactory::instance()->registerRobustKernel(_name, new RobustKernelCreator()); + } + + ~RegisterRobustKernelProxy() + { + RobustKernelFactory::instance()->unregisterType(_name); + } + + private: + std::string _name; + }; + +#if defined _MSC_VER && defined G2O_SHARED_LIBS +# define G2O_ROBUST_KERNEL_FACTORY_EXPORT __declspec(dllexport) +# define G2O_ROBUST_KERNEL_FACTORY_IMPORT __declspec(dllimport) +#else +# define G2O_ROBUST_KERNEL_FACTORY_EXPORT +# define G2O_ROBUST_KERNEL_FACTORY_IMPORT +#endif + + // These macros are used to automate registering of robust kernels and forcing linkage +#define G2O_REGISTER_ROBUST_KERNEL(name, classname) \ + extern "C" void G2O_ROBUST_KERNEL_FACTORY_EXPORT g2o_robust_kernel_##classname(void) {} \ + static g2o::RegisterRobustKernelProxy g_robust_kernel_proxy_##classname(#name); + +#define G2O_USE_ROBUST_KERNEL(classname) \ + extern "C" void G2O_ROBUST_KERNEL_FACTORY_IMPORT g2o_robust_kernel_##classname(void); \ + static g2o::TypeFunctionProxy proxy_##classname(g2o_robust_kernel_##classname); + +} // end namespace g2o + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/robust_kernel_impl.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/robust_kernel_impl.cpp new file mode 100644 index 0000000..e446efc --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/robust_kernel_impl.cpp @@ -0,0 +1,173 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "robust_kernel_impl.h" +#include "robust_kernel_factory.h" + +#include + +namespace g2o { + +RobustKernelScaleDelta::RobustKernelScaleDelta(const RobustKernelPtr& kernel, double delta) : + RobustKernel(delta), + _kernel(kernel) +{ +} + +RobustKernelScaleDelta::RobustKernelScaleDelta(double delta) : + RobustKernel(delta) +{ +} + +void RobustKernelScaleDelta::setKernel(const RobustKernelPtr& ptr) +{ + _kernel = ptr; +} + +void RobustKernelScaleDelta::robustify(double error, Eigen::Vector3d& rho) const +{ + if (_kernel.get()) { + double dsqr = _delta * _delta; + double dsqrReci = 1. / dsqr; + _kernel->robustify(dsqrReci * error, rho); + rho[0] *= dsqr; + rho[2] *= dsqrReci; + } else { // no robustification + rho[0] = error; + rho[1] = 1.; + rho[2] = 0.; + } +} + +void RobustKernelHuber::setDelta(double delta) +{ + dsqr = delta*delta; + _delta = delta; +} + + +void RobustKernelHuber::setDeltaSqr(const double &delta, const double &deltaSqr) +{ + dsqr = deltaSqr; + _delta = delta; +} + +void RobustKernelHuber::robustify(double e, Eigen::Vector3d& rho) const +{ + //dsqr = _delta * _delta; + if (e <= dsqr) { // inlier + rho[0] = e; + rho[1] = 1.; + rho[2] = 0.; + } else { // outlier + double sqrte = sqrt(e); // absolut value of the error + rho[0] = 2*sqrte*_delta - dsqr; // rho(e) = 2 * delta * e^(1/2) - delta^2 + rho[1] = _delta / sqrte; // rho'(e) = delta / sqrt(e) + rho[2] = - 0.5 * rho[1] / e; // rho''(e) = -1 / (2*e^(3/2)) = -1/2 * (delta/e) / e + } +} + +void RobustKernelTukey::setDeltaSqr(const double &deltaSqr, const double &inv) +{ + _deltaSqr = deltaSqr; + _invDeltaSqr = inv; + +} + +void RobustKernelTukey::robustify(double e, Eigen::Vector3d& rho) const +{ + if (e <= _deltaSqr) { // inlier + double factor = e*_invDeltaSqr; + double d = 1-factor; + double dd = d*d; + rho[0] = _deltaSqr*(1-dd*d); + rho[1] = 3*dd; + rho[2] = -6*_invDeltaSqr*d; + } else { // outlier + rho[0] = _deltaSqr; // rho(e) = delta^2 + rho[1] = 0.; + rho[2] = 0.; + } +} + +void RobustKernelPseudoHuber::robustify(double e2, Eigen::Vector3d& rho) const +{ + double dsqr = _delta * _delta; + double dsqrReci = 1. / dsqr; + double aux1 = dsqrReci * e2 + 1.0; + double aux2 = sqrt(aux1); + rho[0] = 2 * dsqr * (aux2 - 1); + rho[1] = 1. / aux2; + rho[2] = -0.5 * dsqrReci * rho[1] / aux1; +} + +void RobustKernelCauchy::robustify(double e2, Eigen::Vector3d& rho) const +{ + double dsqr = _delta * _delta; + double dsqrReci = 1. / dsqr; + double aux = dsqrReci * e2 + 1.0; + rho[0] = dsqr * log(aux); + rho[1] = 1. / aux; + rho[2] = -dsqrReci * std::pow(rho[1], 2); +} + +void RobustKernelSaturated::robustify(double e2, Eigen::Vector3d& rho) const +{ + double dsqr = _delta * _delta; + if (e2 <= dsqr) { // inlier + rho[0] = e2; + rho[1] = 1.; + rho[2] = 0.; + } else { // outlier + rho[0] = dsqr; + rho[1] = 0.; + rho[2] = 0.; + } +} + +//delta is used as $phi$ +void RobustKernelDCS::robustify(double e2, Eigen::Vector3d& rho) const +{ + const double& phi = _delta; + double scale = (2.0*phi)/(phi+e2); + if(scale>=1.0) + scale = 1.0; + + rho[0] = scale*e2*scale; + rho[1] = (scale*scale); + rho[2] = 0; +} + + +// register the kernel to their factory +G2O_REGISTER_ROBUST_KERNEL(Huber, RobustKernelHuber) +G2O_REGISTER_ROBUST_KERNEL(Tukey, RobustKernelTukey) +G2O_REGISTER_ROBUST_KERNEL(PseudoHuber, RobustKernelPseudoHuber) +G2O_REGISTER_ROBUST_KERNEL(Cauchy, RobustKernelCauchy) +G2O_REGISTER_ROBUST_KERNEL(Saturated, RobustKernelSaturated) +G2O_REGISTER_ROBUST_KERNEL(DCS, RobustKernelDCS) + +} // end namespace g2o diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/robust_kernel_impl.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/robust_kernel_impl.h new file mode 100644 index 0000000..066cc37 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/robust_kernel_impl.h @@ -0,0 +1,167 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_ROBUST_KERNEL_IMPL_H +#define G2O_ROBUST_KERNEL_IMPL_H + +#include "robust_kernel.h" + +namespace g2o { + + /** + * \brief scale a robust kernel to another delta (window size) + * + * Scales a robust kernel to another window size. Useful, in case if + * one implements a kernel which only is designed for a fixed window + * size. + */ + class RobustKernelScaleDelta : public RobustKernel + { + public: + /** + * construct the scaled kernel ontop of another kernel which might be shared accross + * several scaled kernels + */ + explicit RobustKernelScaleDelta(const RobustKernelPtr& kernel, double delta = 1.); + explicit RobustKernelScaleDelta(double delta = 1.); + + //! return the underlying kernel + const RobustKernelPtr kernel() const { return _kernel;} + //! use another kernel for the underlying operation + void setKernel(const RobustKernelPtr& ptr); + + void robustify(double error, Eigen::Vector3d& rho) const; + + protected: + RobustKernelPtr _kernel; + }; + + /** + * \brief Huber Cost Function + * + * Loss function as described by Huber + * See http://en.wikipedia.org/wiki/Huber_loss_function + * + * If e^(1/2) < d + * rho(e) = e + * + * else + * + * 1/2 2 + * rho(e) = 2 d e - d + */ + class RobustKernelHuber : public RobustKernel + { + public: + virtual void setDelta(double delta); + virtual void setDeltaSqr(const double &delta, const double &deltaSqr); + virtual void robustify(double e2, Eigen::Vector3d& rho) const; + + private: + float dsqr; + }; + + /** + * \brief Tukey Cost Function + * + * + * If e^(1/2) < d + * rho(e) = delta2(1-(1-e/delta2)^3) + * + * else + * + * rho(e) = delta2 + */ + class RobustKernelTukey : public RobustKernel + { + public: + + virtual void setDeltaSqr(const double &deltaSqr, const double &inv); + virtual void robustify(double e2, Eigen::Vector3d& rho) const; + private: + float _deltaSqr; + float _invDeltaSqr; + }; + + + /** + * \brief Pseudo Huber Cost Function + * + * The smooth pseudo huber cost function: + * See http://en.wikipedia.org/wiki/Huber_loss_function + * + * 2 e + * 2 d (sqrt(-- + 1) - 1) + * 2 + * d + */ + class RobustKernelPseudoHuber : public RobustKernel + { + public: + virtual void robustify(double e2, Eigen::Vector3d& rho) const; + }; + + /** + * \brief Cauchy cost function + * + * 2 e + * d log(-- + 1) + * 2 + * d + */ + class RobustKernelCauchy : public RobustKernel + { + public: + virtual void robustify(double e2, Eigen::Vector3d& rho) const; + }; + + /** + * \brief Saturated cost function. + * + * The error is at most delta^2 + */ + class RobustKernelSaturated : public RobustKernel + { + public: + virtual void robustify(double e2, Eigen::Vector3d& rho) const; + }; + + /** + * \brief Dynamic covariance scaling - DCS + * + * See paper Robust Map Optimization from Agarwal et al. ICRA 2013 + * + * delta is used as $phi$ + */ + class RobustKernelDCS : public RobustKernel + { + public: + virtual void robustify(double e2, Eigen::Vector3d& rho) const; + }; + +} // end namespace g2o + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/solver.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/solver.cpp new file mode 100644 index 0000000..cdddfac --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/solver.cpp @@ -0,0 +1,87 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "solver.h" + +#include +#include + +namespace g2o { + +Solver::Solver() : + _optimizer(0), _x(0), _b(0), _xSize(0), _maxXSize(0), + _isLevenberg(false), _additionalVectorSpace(0) +{ +} + +Solver::~Solver() +{ + delete[] _x; + delete[] _b; +} + +void Solver::resizeVector(size_t sx) +{ + size_t oldSize = _xSize; + _xSize = sx; + sx += _additionalVectorSpace; // allocate some additional space if requested + if (_maxXSize < sx) { + _maxXSize = 2*sx; + delete[] _x; + _x = new double[_maxXSize]; +#ifndef NDEBUG + memset(_x, 0, _maxXSize * sizeof(double)); +#endif + if (_b) { // backup the former b, might still be needed for online processing + memcpy(_x, _b, oldSize * sizeof(double)); + delete[] _b; + _b = new double[_maxXSize]; + std::swap(_b, _x); + } else { + _b = new double[_maxXSize]; +#ifndef NDEBUG + memset(_b, 0, _maxXSize * sizeof(double)); +#endif + } + } +} + +void Solver::setOptimizer(SparseOptimizer* optimizer) +{ + _optimizer = optimizer; +} + +void Solver::setLevenberg(bool levenberg) +{ + _isLevenberg = levenberg; +} + +void Solver::setAdditionalVectorSpace(size_t s) +{ + _additionalVectorSpace = s; +} + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/solver.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/solver.h new file mode 100644 index 0000000..a801d31 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/solver.h @@ -0,0 +1,151 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_SOLVER_H +#define G2O_SOLVER_H + +#include "hyper_graph.h" +#include "batch_stats.h" +#include "sparse_block_matrix.h" +#include + +namespace g2o { + + + class SparseOptimizer; + + /** + * \brief Generic interface for a sparse solver operating on a graph which solves one iteration of the linearized objective function + */ + class Solver + { + public: + Solver(); + virtual ~Solver(); + + public: + /** + * initialize the solver, called once before the first iteration + */ + virtual bool init(SparseOptimizer* optimizer, bool online = false) = 0; + + /** + * build the structure of the system + */ + virtual bool buildStructure(bool zeroBlocks = false) = 0; + /** + * update the structures for online processing + */ + virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges) = 0; + /** + * build the current system + */ + virtual bool buildSystem() = 0; + + /** + * solve Ax = b + */ + virtual bool solve() = 0; + + /** + * computes the block diagonal elements of the pattern specified in the input + * and stores them in given SparseBlockMatrix + */ + virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices) = 0; + + /** + * update the system while performing Levenberg, i.e., modifying the diagonal + * components of A by doing += lambda along the main diagonal of the Matrix. + * Note that this function may be called with a positive and a negative lambda. + * The latter is used to undo a former modification. + * If backup is true, then the solver should store a backup of the diagonal, which + * can be restored by restoreDiagonal() + */ + virtual bool setLambda(double lambda, bool backup = false) = 0; + + /** + * restore a previosly made backup of the diagonal + */ + virtual void restoreDiagonal() = 0; + + //! return x, the solution vector + double* x() { return _x;} + const double* x() const { return _x;} + //! return b, the right hand side of the system + double* b() { return _b;} + const double* b() const { return _b;} + + //! return the size of the solution vector (x) and b + size_t vectorSize() const { return _xSize;} + + //! the optimizer (graph) on which the solver works + SparseOptimizer* optimizer() const { return _optimizer;} + void setOptimizer(SparseOptimizer* optimizer); + + //! the system is Levenberg-Marquardt + bool levenberg() const { return _isLevenberg;} + void setLevenberg(bool levenberg); + + /** + * does this solver support the Schur complement for solving a system consisting of poses and + * landmarks. Re-implemement in a derived solver, if your solver supports it. + */ + virtual bool supportsSchur() {return false;} + + //! should the solver perform the schur complement or not + virtual bool schur()=0; + virtual void setSchur(bool s)=0; + + size_t additionalVectorSpace() const { return _additionalVectorSpace;} + void setAdditionalVectorSpace(size_t s); + + /** + * write debug output of the Hessian if system is not positive definite + */ + virtual void setWriteDebug(bool) = 0; + virtual bool writeDebug() const = 0; + + //! write the hessian to disk using the specified file name + virtual bool saveHessian(const std::string& /*fileName*/) const = 0; + + protected: + SparseOptimizer* _optimizer; + double* _x; + double* _b; + size_t _xSize, _maxXSize; + bool _isLevenberg; ///< the system we gonna solve is a Levenberg-Marquardt system + size_t _additionalVectorSpace; + + void resizeVector(size_t sx); + + private: + // Disable the copy constructor and assignment operator + Solver(const Solver&) { } + Solver& operator= (const Solver&) { return *this; } + }; +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_block_matrix.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_block_matrix.h new file mode 100644 index 0000000..8e9b5ef --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_block_matrix.h @@ -0,0 +1,231 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_SPARSE_BLOCK_MATRIX_ +#define G2O_SPARSE_BLOCK_MATRIX_ + +#include +#include +#include +#include +#include +#include +#include + +#include "sparse_block_matrix_ccs.h" +#include "matrix_structure.h" +#include "matrix_operations.h" +#include "../../config.h" + +namespace g2o { + using namespace Eigen; +/** + * \brief Sparse matrix which uses blocks + * + * Template class that specifies a sparse block matrix. A block + * matrix is a sparse matrix made of dense blocks. These blocks + * cannot have a random pattern, but follow a (variable) grid + * structure. This structure is specified by a partition of the rows + * and the columns of the matrix. The blocks are represented by the + * Eigen::Matrix structure, thus they can be statically or dynamically + * allocated. For efficiency reasons it is convenient to allocate them + * statically, when possible. A static block matrix has all blocks of + * the same size, and the size of the block is specified by the + * template argument. If this is not the case, and you have different + * block sizes than you have to use a dynamic-block matrix (default + * template argument). + */ +template +class SparseBlockMatrix { + + public: + //! this is the type of the elementary block, it is an Eigen::Matrix. + typedef MatrixType SparseMatrixBlock; + + //! columns of the matrix + inline int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;} + //! rows of the matrix + inline int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;} + + typedef std::map IntBlockMap; + + /** + * constructs a sparse block matrix having a specific layout + * @param rbi: array of int containing the row layout of the blocks. + * the component i of the array should contain the index of the first row of the block i+1. + * @param rbi: array of int containing the column layout of the blocks. + * the component i of the array should contain the index of the first col of the block i+1. + * @param rb: number of row blocks + * @param cb: number of col blocks + * @param hasStorage: set it to true if the matrix "owns" the blocks, thus it deletes it on destruction. + * if false the matrix is only a "view" over an existing structure. + */ + SparseBlockMatrix( const int * rbi, const int* cbi, int rb, int cb, bool hasStorage=true); + + SparseBlockMatrix(); + + ~SparseBlockMatrix(); + + + //! this zeroes all the blocks. If dealloc=true the blocks are removed from memory + void clear(bool dealloc=false) ; + + //! returns the block at location r,c. if alloc=true he block is created if it does not exist + SparseMatrixBlock* block(int r, int c, bool alloc=false); + //! returns the block at location r,c + const SparseMatrixBlock* block(int r, int c) const; + + //! how many rows does the block at block-row r has? + inline int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; } + + //! how many cols does the block at block-col c has? + inline int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; } + + //! where does the row at block-row r starts? + inline int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; } + + //! where does the col at block-col r starts? + inline int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; } + + //! number of non-zero elements + size_t nonZeros() const; + //! number of allocated blocks + size_t nonZeroBlocks() const; + + //! deep copy of a sparse-block-matrix; + SparseBlockMatrix* clone() const ; + + /** + * returns a view or a copy of the block matrix + * @param rmin: starting block row + * @param rmax: ending block row + * @param cmin: starting block col + * @param cmax: ending block col + * @param alloc: if true it makes a deep copy, if false it creates a view. + */ + SparseBlockMatrix* slice(int rmin, int rmax, int cmin, int cmax, bool alloc=true) const; + + //! transposes a block matrix, The transposed type should match the argument false on failure + template + bool transpose(SparseBlockMatrix*& dest) const; + + //! adds the current matrix to the destination + bool add(SparseBlockMatrix*& dest) const ; + + //! dest = (*this) * M + template + bool multiply(SparseBlockMatrix *& dest, const SparseBlockMatrix* M) const; + + //! dest = (*this) * src + void multiply(double*& dest, const double* src) const; + + /** + * compute dest = (*this) * src + * However, assuming that this is a symmetric matrix where only the upper triangle is stored + */ + void multiplySymmetricUpperTriangle(double*& dest, const double* src) const; + + //! dest = M * (*this) + void rightMultiply(double*& dest, const double* src) const; + + //! *this *= a + void scale( double a); + + /** + * writes in dest a block permutaton specified by pinv. + * @param pinv: array such that new_block[i] = old_block[pinv[i]] + */ + bool symmPermutation(SparseBlockMatrix*& dest, const int* pinv, bool onlyUpper=false) const; + + /** + * fill the CCS arrays of a matrix, arrays have to be allocated beforehand + */ + int fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle = false) const; + + /** + * fill the CCS arrays of a matrix, arrays have to be allocated beforehand. This function only writes + * the values and assumes that column and row structures have already been written. + */ + int fillCCS(double* Cx, bool upperTriangle = false) const; + + //! exports the non zero blocks in the structure matrix ms + void fillBlockStructure(MatrixStructure& ms) const; + + //! the block matrices per block-column + const std::vector& blockCols() const { return _blockCols;} + std::vector& blockCols() { return _blockCols;} + + //! indices of the row blocks + const std::vector& rowBlockIndices() const { return _rowBlockIndices;} + std::vector& rowBlockIndices() { return _rowBlockIndices;} + + //! indices of the column blocks + const std::vector& colBlockIndices() const { return _colBlockIndices;} + std::vector& colBlockIndices() { return _colBlockIndices;} + + /** + * write the content of this matrix to a stream loadable by Octave + * @param upperTriangle does this matrix store only the upper triangular blocks + */ + bool writeOctave(const char* filename, bool upperTriangle = true) const; + + /** + * copy into CCS structure + * @return number of processed blocks, -1 on error + */ + int fillSparseBlockMatrixCCS(SparseBlockMatrixCCS& blockCCS) const; + + /** + * copy as transposed into a CCS structure + * @return number of processed blocks, -1 on error + */ + int fillSparseBlockMatrixCCSTransposed(SparseBlockMatrixCCS& blockCCS) const; + + /** + * take over the memory and matrix pattern from a hash matrix. + * The structure of the hash matrix will be cleared. + */ + void takePatternFromHash(SparseBlockMatrixHashMap& hashMatrix); + + protected: + std::vector _rowBlockIndices; ///< vector of the indices of the blocks along the rows. + std::vector _colBlockIndices; ///< vector of the indices of the blocks along the cols + //! array of maps of blocks. The index of the array represent a block column of the matrix + //! and the block column is stored as a map row_block -> matrix_block_ptr. + std::vector _blockCols; + bool _hasStorage; +}; + +template < class MatrixType > +std::ostream& operator << (std::ostream&, const SparseBlockMatrix& m); + + typedef SparseBlockMatrix SparseBlockMatrixXd; + +} //end namespace + +#include "sparse_block_matrix.hpp" + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp new file mode 100644 index 0000000..8dfa99c --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp @@ -0,0 +1,657 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +namespace g2o { + using namespace Eigen; + + namespace { + struct TripletEntry + { + int r, c; + double x; + TripletEntry(int r_, int c_, double x_) : r(r_), c(c_), x(x_) {} + }; + struct TripletColSort + { + bool operator()(const TripletEntry& e1, const TripletEntry& e2) const + { + return e1.c < e2.c || (e1.c == e2.c && e1.r < e2.r); + } + }; + /** Helper class to sort pair based on first elem */ + template > + struct CmpPairFirst { + bool operator()(const std::pair& left, const std::pair& right) { + return Pred()(left.first, right.first); + } + }; + } + + template + SparseBlockMatrix::SparseBlockMatrix( const int * rbi, const int* cbi, int rb, int cb, bool hasStorage): + _rowBlockIndices(rbi,rbi+rb), + _colBlockIndices(cbi,cbi+cb), + _blockCols(cb), _hasStorage(hasStorage) + { + } + + template + SparseBlockMatrix::SparseBlockMatrix( ): + _blockCols(0), _hasStorage(true) + { + } + + template + void SparseBlockMatrix::clear(bool dealloc) { +# ifdef G2O_OPENMP +# pragma omp parallel for default (shared) if (_blockCols.size() > 100) +# endif + for (int i=0; i < static_cast(_blockCols.size()); ++i) { + for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ + typename SparseBlockMatrix::SparseMatrixBlock* b=it->second; + if (_hasStorage && dealloc) + delete b; + else + b->setZero(); + } + if (_hasStorage && dealloc) + _blockCols[i].clear(); + } + } + + template + SparseBlockMatrix::~SparseBlockMatrix(){ + if (_hasStorage) + clear(true); + } + + template + typename SparseBlockMatrix::SparseMatrixBlock* SparseBlockMatrix::block(int r, int c, bool alloc) { + typename SparseBlockMatrix::IntBlockMap::iterator it =_blockCols[c].find(r); + typename SparseBlockMatrix::SparseMatrixBlock* _block=0; + if (it==_blockCols[c].end()){ + if (!_hasStorage && ! alloc ) + return 0; + else { + int rb=rowsOfBlock(r); + int cb=colsOfBlock(c); + _block=new typename SparseBlockMatrix::SparseMatrixBlock(rb,cb); + _block->setZero(); + std::pair < typename SparseBlockMatrix::IntBlockMap::iterator, bool> result + =_blockCols[c].insert(std::make_pair(r,_block)); (void) result; + assert (result.second); + } + } else { + _block=it->second; + } + return _block; + } + + template + const typename SparseBlockMatrix::SparseMatrixBlock* SparseBlockMatrix::block(int r, int c) const { + typename SparseBlockMatrix::IntBlockMap::const_iterator it =_blockCols[c].find(r); + if (it==_blockCols[c].end()) + return 0; + return it->second; + } + + + template + SparseBlockMatrix* SparseBlockMatrix::clone() const { + SparseBlockMatrix* ret= new SparseBlockMatrix(&_rowBlockIndices[0], &_colBlockIndices[0], _rowBlockIndices.size(), _colBlockIndices.size()); + for (size_t i=0; i<_blockCols.size(); ++i){ + for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ + typename SparseBlockMatrix::SparseMatrixBlock* b=new typename SparseBlockMatrix::SparseMatrixBlock(*it->second); + ret->_blockCols[i].insert(std::make_pair(it->first, b)); + } + } + ret->_hasStorage=true; + return ret; + } + + + template + template + bool SparseBlockMatrix::transpose(SparseBlockMatrix*& dest) const { + if (! dest){ + dest=new SparseBlockMatrix(&_colBlockIndices[0], &_rowBlockIndices[0], _colBlockIndices.size(), _rowBlockIndices.size()); + } else { + if (! dest->_hasStorage) + return false; + if(_rowBlockIndices.size()!=dest->_colBlockIndices.size()) + return false; + if (_colBlockIndices.size()!=dest->_rowBlockIndices.size()) + return false; + for (size_t i=0; i<_rowBlockIndices.size(); ++i){ + if(_rowBlockIndices[i]!=dest->_colBlockIndices[i]) + return false; + } + for (size_t i=0; i<_colBlockIndices.size(); ++i){ + if(_colBlockIndices[i]!=dest->_rowBlockIndices[i]) + return false; + } + } + + for (size_t i=0; i<_blockCols.size(); ++i){ + for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ + typename SparseBlockMatrix::SparseMatrixBlock* s=it->second; + typename SparseBlockMatrix::SparseMatrixBlock* d=dest->block(i,it->first,true); + *d = s->transpose(); + } + } + return true; + } + + template + bool SparseBlockMatrix::add(SparseBlockMatrix*& dest) const { + if (! dest){ + dest=new SparseBlockMatrix(&_rowBlockIndices[0], &_colBlockIndices[0], _rowBlockIndices.size(), _colBlockIndices.size()); + } else { + if (! dest->_hasStorage) + return false; + if(_rowBlockIndices.size()!=dest->_rowBlockIndices.size()) + return false; + if (_colBlockIndices.size()!=dest->_colBlockIndices.size()) + return false; + for (size_t i=0; i<_rowBlockIndices.size(); ++i){ + if(_rowBlockIndices[i]!=dest->_rowBlockIndices[i]) + return false; + } + for (size_t i=0; i<_colBlockIndices.size(); ++i){ + if(_colBlockIndices[i]!=dest->_colBlockIndices[i]) + return false; + } + } + for (size_t i=0; i<_blockCols.size(); ++i){ + for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ + typename SparseBlockMatrix::SparseMatrixBlock* s=it->second; + typename SparseBlockMatrix::SparseMatrixBlock* d=dest->block(it->first,i,true); + (*d)+=*s; + } + } + return true; + } + + template + template < class MatrixResultType, class MatrixFactorType > + bool SparseBlockMatrix::multiply(SparseBlockMatrix*& dest, const SparseBlockMatrix * M) const { + // sanity check + if (_colBlockIndices.size()!=M->_rowBlockIndices.size()) + return false; + for (size_t i=0; i<_colBlockIndices.size(); ++i){ + if (_colBlockIndices[i]!=M->_rowBlockIndices[i]) + return false; + } + if (! dest) { + dest=new SparseBlockMatrix(&_rowBlockIndices[0],&(M->_colBlockIndices[0]), _rowBlockIndices.size(), M->_colBlockIndices.size() ); + } + if (! dest->_hasStorage) + return false; + for (size_t i=0; i_blockCols.size(); ++i){ + for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=M->_blockCols[i].begin(); it!=M->_blockCols[i].end(); ++it){ + // look for a non-zero block in a row of column it + int colM=i; + const typename SparseBlockMatrix::SparseMatrixBlock *b=it->second; + typename SparseBlockMatrix::IntBlockMap::const_iterator rbt=_blockCols[it->first].begin(); + while(rbt!=_blockCols[it->first].end()){ + //int colA=it->first; + int rowA=rbt->first; + typename SparseBlockMatrix::SparseMatrixBlock *a=rbt->second; + typename SparseBlockMatrix::SparseMatrixBlock *c=dest->block(rowA,colM,true); + assert (c->rows()==a->rows()); + assert (c->cols()==b->cols()); + ++rbt; + (*c)+=(*a)*(*b); + } + } + } + return false; + } + + template + void SparseBlockMatrix::multiply(double*& dest, const double* src) const { + if (! dest){ + dest=new double [_rowBlockIndices[_rowBlockIndices.size()-1] ]; + memset(dest,0, _rowBlockIndices[_rowBlockIndices.size()-1]*sizeof(double)); + } + + // map the memory by Eigen + Eigen::Map destVec(dest, rows()); + const Eigen::Map srcVec(src, cols()); + + for (size_t i=0; i<_blockCols.size(); ++i){ + int srcOffset = i ? _colBlockIndices[i-1] : 0; + + for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ + const typename SparseBlockMatrix::SparseMatrixBlock* a=it->second; + int destOffset = it->first ? _rowBlockIndices[it->first - 1] : 0; + // destVec += *a * srcVec (according to the sub-vector parts) + internal::axpy(*a, srcVec, srcOffset, destVec, destOffset); + } + } + } + + template + void SparseBlockMatrix::multiplySymmetricUpperTriangle(double*& dest, const double* src) const + { + if (! dest){ + dest=new double [_rowBlockIndices[_rowBlockIndices.size()-1] ]; + memset(dest,0, _rowBlockIndices[_rowBlockIndices.size()-1]*sizeof(double)); + } + + // map the memory by Eigen + Eigen::Map destVec(dest, rows()); + const Eigen::Map srcVec(src, cols()); + + for (size_t i=0; i<_blockCols.size(); ++i){ + int srcOffset = colBaseOfBlock(i); + for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ + const typename SparseBlockMatrix::SparseMatrixBlock* a=it->second; + int destOffset = rowBaseOfBlock(it->first); + if (destOffset > srcOffset) // only upper triangle + break; + // destVec += *a * srcVec (according to the sub-vector parts) + internal::axpy(*a, srcVec, srcOffset, destVec, destOffset); + if (destOffset < srcOffset) + internal::atxpy(*a, srcVec, destOffset, destVec, srcOffset); + } + } + } + + template + void SparseBlockMatrix::rightMultiply(double*& dest, const double* src) const { + int destSize=cols(); + + if (! dest){ + dest=new double [ destSize ]; + memset(dest,0, destSize*sizeof(double)); + } + + // map the memory by Eigen + Eigen::Map destVec(dest, destSize); + Eigen::Map srcVec(src, rows()); + +# ifdef G2O_OPENMP +# pragma omp parallel for default (shared) schedule(dynamic, 10) +# endif + for (int i=0; i < static_cast(_blockCols.size()); ++i){ + int destOffset = colBaseOfBlock(i); + for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); + it!=_blockCols[i].end(); + ++it){ + const typename SparseBlockMatrix::SparseMatrixBlock* a=it->second; + int srcOffset = rowBaseOfBlock(it->first); + // destVec += *a.transpose() * srcVec (according to the sub-vector parts) + internal::atxpy(*a, srcVec, srcOffset, destVec, destOffset); + } + } + + } + + template + void SparseBlockMatrix::scale(double a_) { + for (size_t i=0; i<_blockCols.size(); ++i){ + for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ + typename SparseBlockMatrix::SparseMatrixBlock* a=it->second; + *a *= a_; + } + } + } + + template + SparseBlockMatrix* SparseBlockMatrix::slice(int rmin, int rmax, int cmin, int cmax, bool alloc) const { + int m=rmax-rmin; + int n=cmax-cmin; + int rowIdx [m]; + rowIdx[0] = rowsOfBlock(rmin); + for (int i=1; i::SparseBlockMatrix* s=new SparseBlockMatrix(rowIdx, colIdx, m, n, true); + for (int i=0; i::IntBlockMap::const_iterator it=_blockCols[mc].begin(); it!=_blockCols[mc].end(); ++it){ + if (it->first >= rmin && it->first < rmax){ + typename SparseBlockMatrix::SparseMatrixBlock* b = alloc ? new typename SparseBlockMatrix::SparseMatrixBlock (* (it->second) ) : it->second; + s->_blockCols[i].insert(std::make_pair(it->first-rmin, b)); + } + } + } + s->_hasStorage=alloc; + return s; + } + + template + size_t SparseBlockMatrix::nonZeroBlocks() const { + size_t count=0; + for (size_t i=0; i<_blockCols.size(); ++i) + count+=_blockCols[i].size(); + return count; + } + + template + size_t SparseBlockMatrix::nonZeros() const{ + if (MatrixType::SizeAtCompileTime != Eigen::Dynamic) { + size_t nnz = nonZeroBlocks() * MatrixType::SizeAtCompileTime; + return nnz; + } else { + size_t count=0; + for (size_t i=0; i<_blockCols.size(); ++i){ + for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ + const typename SparseBlockMatrix::SparseMatrixBlock* a=it->second; + count += a->cols()*a->rows(); + } + } + return count; + } + } + + template + std::ostream& operator << (std::ostream& os, const SparseBlockMatrix& m){ + os << "RBI: " << m.rowBlockIndices().size(); + for (size_t i=0; i::IntBlockMap::const_iterator it=m.blockCols()[i].begin(); it!=m.blockCols()[i].end(); ++it){ + const typename SparseBlockMatrix::SparseMatrixBlock* b=it->second; + os << "BLOCK: " << it->first << " " << i << std::endl; + os << *b << std::endl; + } + } + return os; + } + + template + bool SparseBlockMatrix::symmPermutation(SparseBlockMatrix*& dest, const int* pinv, bool upperTriangle) const{ + // compute the permuted version of the new row/column layout + size_t n=_rowBlockIndices.size(); + // computed the block sizes + int blockSizes[_rowBlockIndices.size()]; + blockSizes[0]=_rowBlockIndices[0]; + for (size_t i=1; i_rowBlockIndices.size()!=n) + return false; + if (dest->_colBlockIndices.size()!=n) + return false; + for (size_t i=0; i_rowBlockIndices[i]!=pBlockIndices[i]) + return false; + if (dest->_colBlockIndices[i]!=pBlockIndices[i]) + return false; + } + dest->clear(); + } + // now ready to permute the columns + for (size_t i=0; i::IntBlockMap::const_iterator it=_blockCols[i].begin(); + it!=_blockCols[i].end(); ++it){ + int pj=pinv[it->first]; + + const typename SparseBlockMatrix::SparseMatrixBlock* s=it->second; + + typename SparseBlockMatrix::SparseMatrixBlock* b=0; + if (! upperTriangle || pj<=pi) { + b=dest->block(pj,pi,true); + assert(b->cols()==s->cols()); + assert(b->rows()==s->rows()); + *b=*s; + } else { + b=dest->block(pi,pj,true); + assert(b); + assert(b->rows()==s->cols()); + assert(b->cols()==s->rows()); + *b=s->transpose(); + } + } + //cerr << endl; + // within each row, + } + return true; + + } + + template + int SparseBlockMatrix::fillCCS(double* Cx, bool upperTriangle) const + { + assert(Cx && "Target destination is NULL"); + double* CxStart = Cx; + for (size_t i=0; i<_blockCols.size(); ++i){ + int cstart=i ? _colBlockIndices[i-1] : 0; + int csize=colsOfBlock(i); + for (int c=0; c::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ + const typename SparseBlockMatrix::SparseMatrixBlock* b=it->second; + int rstart=it->first ? _rowBlockIndices[it->first-1] : 0; + + int elemsToCopy = b->rows(); + if (upperTriangle && rstart == cstart) + elemsToCopy = c + 1; + memcpy(Cx, b->data() + c*b->rows(), elemsToCopy * sizeof(double)); + Cx += elemsToCopy; + + } + } + } + return Cx - CxStart; + } + + template + int SparseBlockMatrix::fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle) const + { + assert(Cp && Ci && Cx && "Target destination is NULL"); + int nz=0; + for (size_t i=0; i<_blockCols.size(); ++i){ + int cstart=i ? _colBlockIndices[i-1] : 0; + int csize=colsOfBlock(i); + for (int c=0; c::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ + const typename SparseBlockMatrix::SparseMatrixBlock* b=it->second; + int rstart=it->first ? _rowBlockIndices[it->first-1] : 0; + + int elemsToCopy = b->rows(); + if (upperTriangle && rstart == cstart) + elemsToCopy = c + 1; + for (int r=0; r + void SparseBlockMatrix::fillBlockStructure(MatrixStructure& ms) const + { + int n = _colBlockIndices.size(); + int nzMax = (int)nonZeroBlocks(); + + ms.alloc(n, nzMax); + ms.m = _rowBlockIndices.size(); + + int nz = 0; + int* Cp = ms.Ap; + int* Ci = ms.Aii; + for (int i = 0; i < static_cast(_blockCols.size()); ++i){ + *Cp = nz; + const int& c = i; + for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it) { + const int& r = it->first; + if (r <= c) { + *Ci++ = r; + ++nz; + } + } + Cp++; + } + *Cp=nz; + assert(nz <= nzMax); + } + + template + bool SparseBlockMatrix::writeOctave(const char* filename, bool upperTriangle) const + { + std::string name = filename; + std::string::size_type lastDot = name.find_last_of('.'); + if (lastDot != std::string::npos) + name = name.substr(0, lastDot); + + std::vector entries; + for (size_t i = 0; i<_blockCols.size(); ++i){ + const int& c = i; + for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it) { + const int& r = it->first; + const MatrixType& m = *(it->second); + for (int cc = 0; cc < m.cols(); ++cc) + for (int rr = 0; rr < m.rows(); ++rr) { + int aux_r = rowBaseOfBlock(r) + rr; + int aux_c = colBaseOfBlock(c) + cc; + entries.push_back(TripletEntry(aux_r, aux_c, m(rr, cc))); + if (upperTriangle && r != c) { + entries.push_back(TripletEntry(aux_c, aux_r, m(rr, cc))); + } + } + } + } + + int nz = entries.size(); + std::sort(entries.begin(), entries.end(), TripletColSort()); + + std::ofstream fout(filename); + fout << "# name: " << name << std::endl; + fout << "# type: sparse matrix" << std::endl; + fout << "# nnz: " << nz << std::endl; + fout << "# rows: " << rows() << std::endl; + fout << "# columns: " << cols() << std::endl; + fout << std::setprecision(9) << std::fixed << std::endl; + + for (std::vector::const_iterator it = entries.begin(); it != entries.end(); ++it) { + const TripletEntry& entry = *it; + fout << entry.r+1 << " " << entry.c+1 << " " << entry.x << std::endl; + } + return fout.good(); + } + + template + int SparseBlockMatrix::fillSparseBlockMatrixCCS(SparseBlockMatrixCCS& blockCCS) const + { + blockCCS.blockCols().resize(blockCols().size()); + int numblocks = 0; + for (size_t i = 0; i < blockCols().size(); ++i) { + const IntBlockMap& row = blockCols()[i]; + typename SparseBlockMatrixCCS::SparseColumn& dest = blockCCS.blockCols()[i]; + dest.clear(); + dest.reserve(row.size()); + for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end(); ++it) { + dest.push_back(typename SparseBlockMatrixCCS::RowBlock(it->first, it->second)); + ++numblocks; + } + } + return numblocks; + } + + template + int SparseBlockMatrix::fillSparseBlockMatrixCCSTransposed(SparseBlockMatrixCCS& blockCCS) const + { + blockCCS.blockCols().clear(); + blockCCS.blockCols().resize(_rowBlockIndices.size()); + int numblocks = 0; + for (size_t i = 0; i < blockCols().size(); ++i) { + const IntBlockMap& row = blockCols()[i]; + for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end(); ++it) { + typename SparseBlockMatrixCCS::SparseColumn& dest = blockCCS.blockCols()[it->first]; + dest.push_back(typename SparseBlockMatrixCCS::RowBlock(i, it->second)); + ++numblocks; + } + } + return numblocks; + } + + template + void SparseBlockMatrix::takePatternFromHash(SparseBlockMatrixHashMap& hashMatrix) + { + // sort the sparse columns and add them to the map structures by + // exploiting that we are inserting a sorted structure + typedef std::pair SparseColumnPair; + typedef typename SparseBlockMatrixHashMap::SparseColumn HashSparseColumn; + for (size_t i = 0; i < hashMatrix.blockCols().size(); ++i) { + // prepare a temporary vector for sorting + HashSparseColumn& column = hashMatrix.blockCols()[i]; + if (column.size() == 0) + continue; + std::vector sparseRowSorted; // temporary structure + sparseRowSorted.reserve(column.size()); + for (typename HashSparseColumn::const_iterator it = column.begin(); it != column.end(); ++it) + sparseRowSorted.push_back(*it); + std::sort(sparseRowSorted.begin(), sparseRowSorted.end(), CmpPairFirst()); + // try to free some memory early + HashSparseColumn aux; + swap(aux, column); + // now insert sorted vector to the std::map structure + IntBlockMap& destColumnMap = blockCols()[i]; + destColumnMap.insert(sparseRowSorted[0]); + for (size_t j = 1; j < sparseRowSorted.size(); ++j) { + typename SparseBlockMatrix::IntBlockMap::iterator hint = destColumnMap.end(); + --hint; // cppreference says the element goes after the hint (until C++11) + destColumnMap.insert(hint, sparseRowSorted[j]); + } + } + } + +}// end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h new file mode 100644 index 0000000..eb9042c --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h @@ -0,0 +1,282 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_SPARSE_BLOCK_MATRIX_CCS_H +#define G2O_SPARSE_BLOCK_MATRIX_CCS_H + +#include +#include +#include + +#include "../../config.h" +#include "matrix_operations.h" + +#ifdef _MSC_VER +#include +#else +#include +#endif + +namespace g2o { + + /** + * \brief Sparse matrix which uses blocks + * + * This class is used as a const view on a SparseBlockMatrix + * which allows a faster iteration over the elements of the + * matrix. + */ + template + class SparseBlockMatrixCCS + { + public: + //! this is the type of the elementary block, it is an Eigen::Matrix. + typedef MatrixType SparseMatrixBlock; + + //! columns of the matrix + int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;} + //! rows of the matrix + int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;} + + /** + * \brief A block within a column + */ + struct RowBlock + { + int row; ///< row of the block + MatrixType* block; ///< matrix pointer for the block + RowBlock() : row(-1), block(0) {} + RowBlock(int r, MatrixType* b) : row(r), block(b) {} + bool operator<(const RowBlock& other) const { return row < other.row;} + }; + typedef std::vector SparseColumn; + + SparseBlockMatrixCCS(const std::vector& rowIndices, const std::vector& colIndices) : + _rowBlockIndices(rowIndices), _colBlockIndices(colIndices) + {} + + //! how many rows does the block at block-row r has? + int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; } + + //! how many cols does the block at block-col c has? + int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; } + + //! where does the row at block-row r start? + int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; } + + //! where does the col at block-col r start? + int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; } + + //! the block matrices per block-column + const std::vector& blockCols() const { return _blockCols;} + std::vector& blockCols() { return _blockCols;} + + //! indices of the row blocks + const std::vector& rowBlockIndices() const { return _rowBlockIndices;} + + //! indices of the column blocks + const std::vector& colBlockIndices() const { return _colBlockIndices;} + + void rightMultiply(double*& dest, const double* src) const + { + int destSize=cols(); + + if (! dest){ + dest=new double [ destSize ]; + memset(dest,0, destSize*sizeof(double)); + } + + // map the memory by Eigen + Eigen::Map destVec(dest, destSize); + Eigen::Map srcVec(src, rows()); + +# ifdef G2O_OPENMP +# pragma omp parallel for default (shared) schedule(dynamic, 10) +# endif + for (int i=0; i < static_cast(_blockCols.size()); ++i){ + int destOffset = colBaseOfBlock(i); + for (typename SparseColumn::const_iterator it = _blockCols[i].begin(); it!=_blockCols[i].end(); ++it) { + const SparseMatrixBlock* a = it->block; + int srcOffset = rowBaseOfBlock(it->row); + // destVec += *a.transpose() * srcVec (according to the sub-vector parts) + internal::atxpy(*a, srcVec, srcOffset, destVec, destOffset); + } + } + } + + /** + * sort the blocks in each column + */ + void sortColumns() + { + for (int i=0; i < static_cast(_blockCols.size()); ++i){ + std::sort(_blockCols[i].begin(), _blockCols[i].end()); + } + } + + /** + * fill the CCS arrays of a matrix, arrays have to be allocated beforehand + */ + int fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle = false) const + { + assert(Cp && Ci && Cx && "Target destination is NULL"); + int nz=0; + for (size_t i=0; i<_blockCols.size(); ++i){ + int cstart=i ? _colBlockIndices[i-1] : 0; + int csize=colsOfBlock(i); + for (int c=0; cblock; + int rstart=it->row ? _rowBlockIndices[it->row-1] : 0; + + int elemsToCopy = b->rows(); + if (upperTriangle && rstart == cstart) + elemsToCopy = c + 1; + for (int r=0; rblock; + int rstart = it->row ? _rowBlockIndices[it->row-1] : 0; + + int elemsToCopy = b->rows(); + if (upperTriangle && rstart == cstart) + elemsToCopy = c + 1; + memcpy(Cx, b->data() + c*b->rows(), elemsToCopy * sizeof(double)); + Cx += elemsToCopy; + + } + } + cstart = _colBlockIndices[i]; + } + return Cx - CxStart; + } + + protected: + const std::vector& _rowBlockIndices; ///< vector of the indices of the blocks along the rows. + const std::vector& _colBlockIndices; ///< vector of the indices of the blocks along the cols + std::vector _blockCols; ///< the matrices stored in CCS order + }; + + + + /** + * \brief Sparse matrix which uses blocks based on hash structures + * + * This class is used to construct the pattern of a sparse block matrix + */ + template + class SparseBlockMatrixHashMap + { + public: + //! this is the type of the elementary block, it is an Eigen::Matrix. + typedef MatrixType SparseMatrixBlock; + + //! columns of the matrix + int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;} + //! rows of the matrix + int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;} + + typedef std::tr1::unordered_map SparseColumn; + + SparseBlockMatrixHashMap(const std::vector& rowIndices, const std::vector& colIndices) : + _rowBlockIndices(rowIndices), _colBlockIndices(colIndices) + {} + + //! how many rows does the block at block-row r has? + int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; } + + //! how many cols does the block at block-col c has? + int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; } + + //! where does the row at block-row r start? + int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; } + + //! where does the col at block-col r start? + int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; } + + //! the block matrices per block-column + const std::vector& blockCols() const { return _blockCols;} + std::vector& blockCols() { return _blockCols;} + + //! indices of the row blocks + const std::vector& rowBlockIndices() const { return _rowBlockIndices;} + + //! indices of the column blocks + const std::vector& colBlockIndices() const { return _colBlockIndices;} + + /** + * add a block to the pattern, return a pointer to the added block + */ + MatrixType* addBlock(int r, int c, bool zeroBlock = false) + { + assert(c <(int)_blockCols.size() && "accessing column which is not available"); + SparseColumn& sparseColumn = _blockCols[c]; + typename SparseColumn::iterator foundIt = sparseColumn.find(r); + if (foundIt == sparseColumn.end()) { + int rb = rowsOfBlock(r); + int cb = colsOfBlock(c); + MatrixType* m = new MatrixType(rb, cb); + if (zeroBlock) + m->setZero(); + sparseColumn[r] = m; + return m; + } + return foundIt->second; + } + + protected: + const std::vector& _rowBlockIndices; ///< vector of the indices of the blocks along the rows. + const std::vector& _colBlockIndices; ///< vector of the indices of the blocks along the cols + std::vector _blockCols; ///< the matrices stored in CCS order + }; + +} //end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_block_matrix_diagonal.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_block_matrix_diagonal.h new file mode 100644 index 0000000..7b13b9f --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_block_matrix_diagonal.h @@ -0,0 +1,108 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_SPARSE_BLOCK_MATRIX_DIAGONAL_H +#define G2O_SPARSE_BLOCK_MATRIX_DIAGONAL_H + +#include +#include +#include + +#include "../../config.h" +#include "matrix_operations.h" + +namespace g2o { + + /** + * \brief Sparse matrix which uses blocks on the diagonal + * + * This class is used as a const view on a SparseBlockMatrix + * which allows a faster iteration over the elements of the + * matrix. + */ + template + class SparseBlockMatrixDiagonal + { + public: + //! this is the type of the elementary block, it is an Eigen::Matrix. + typedef MatrixType SparseMatrixBlock; + + //! columns of the matrix + int cols() const {return _blockIndices.size() ? _blockIndices.back() : 0;} + //! rows of the matrix + int rows() const {return _blockIndices.size() ? _blockIndices.back() : 0;} + + typedef std::vector > DiagonalVector; + + SparseBlockMatrixDiagonal(const std::vector& blockIndices) : + _blockIndices(blockIndices) + {} + + //! how many rows/cols does the block at block-row / block-column r has? + inline int dimOfBlock(int r) const { return r ? _blockIndices[r] - _blockIndices[r-1] : _blockIndices[0] ; } + + //! where does the row /col at block-row / block-column r starts? + inline int baseOfBlock(int r) const { return r ? _blockIndices[r-1] : 0 ; } + + //! the block matrices per block-column + const DiagonalVector& diagonal() const { return _diagonal;} + DiagonalVector& diagonal() { return _diagonal;} + + //! indices of the row blocks + const std::vector& blockIndices() const { return _blockIndices;} + + void multiply(double*& dest, const double* src) const + { + int destSize=cols(); + if (! dest) { + dest=new double[destSize]; + memset(dest,0, destSize*sizeof(double)); + } + + // map the memory by Eigen + Eigen::Map destVec(dest, destSize); + Eigen::Map srcVec(src, rows()); + +# ifdef G2O_OPENMP +# pragma omp parallel for default (shared) schedule(dynamic, 10) +# endif + for (int i=0; i < static_cast(_diagonal.size()); ++i){ + int destOffset = baseOfBlock(i); + int srcOffset = destOffset; + const SparseMatrixBlock& A = _diagonal[i]; + // destVec += *A.transpose() * srcVec (according to the sub-vector parts) + internal::axpy(A, srcVec, srcOffset, destVec, destOffset); + } + } + + protected: + const std::vector& _blockIndices; ///< vector of the indices of the blocks along the diagonal + DiagonalVector _diagonal; + }; + +} //end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_block_matrix_test.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_block_matrix_test.cpp new file mode 100644 index 0000000..42a4560 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_block_matrix_test.cpp @@ -0,0 +1,107 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "sparse_block_matrix.h" +#include + +using namespace std; +using namespace g2o; +using namespace Eigen; + +typedef SparseBlockMatrix< MatrixXd > +SparseBlockMatrixX; + +std::ostream& operator << (std::ostream& os, const SparseBlockMatrixX::SparseMatrixBlock& m) { + for (int i=0; iblock(0,0, true); + cerr << b->rows() << " " << b->cols() << endl; + for (int i=0; irows(); ++i) + for (int j=0; jcols(); ++j){ + (*b)(i,j)=i*b->cols()+j; + } + + + cerr << "block access 2" << endl; + b=M->block(0,2, true); + cerr << b->rows() << " " << b->cols() << endl; + for (int i=0; irows(); ++i) + for (int j=0; jcols(); ++j){ + (*b)(i,j)=i*b->cols()+j; + } + + b=M->block(3,2, true); + cerr << b->rows() << " " << b->cols() << endl; + for (int i=0; irows(); ++i) + for (int j=0; jcols(); ++j){ + (*b)(i,j)=i*b->cols()+j; + } + + cerr << *M << endl; + + cerr << "SUM" << endl; + + SparseBlockMatrixX* Ms=0; + M->add(Ms); + M->add(Ms); + cerr << *Ms; + + SparseBlockMatrixX* Mt=0; + M->transpose(Mt); + cerr << *Mt << endl; + + SparseBlockMatrixX* Mp=0; + M->multiply(Mp, Mt); + cerr << *Mp << endl; + + int iperm[]={3,2,1,0}; + SparseBlockMatrixX* PMp=0; + + Mp->symmPermutation(PMp,iperm, false); + cerr << *PMp << endl; + + PMp->clear(true); + Mp->block(3,0)->fill(0.); + Mp->symmPermutation(PMp,iperm, true); + cerr << *PMp << endl; + + + +} diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_optimizer.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_optimizer.cpp new file mode 100644 index 0000000..7c316e6 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_optimizer.cpp @@ -0,0 +1,615 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "sparse_optimizer.h" + +#include +#include +#include +#include +#include +#include + +#include "estimate_propagator.h" +#include "optimization_algorithm.h" +#include "batch_stats.h" +#include "hyper_graph_action.h" +#include "robust_kernel.h" +#include "../stuff/timeutil.h" +#include "../stuff/macros.h" +#include "../stuff/misc.h" +#include "../../config.h" + +namespace g2o{ + using namespace std; + + + SparseOptimizer::SparseOptimizer() : + _forceStopFlag(0), _verbose(false), _algorithm(0), _computeBatchStatistics(false) + { + _graphActions.resize(AT_NUM_ELEMENTS); + } + + SparseOptimizer::~SparseOptimizer(){ + delete _algorithm; + G2OBatchStatistics::setGlobalStats(0); + } + + void SparseOptimizer::computeActiveErrors() + { + // call the callbacks in case there is something registered + HyperGraphActionSet& actions = _graphActions[AT_COMPUTEACTIVERROR]; + if (actions.size() > 0) { + for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it) + (*(*it))(this); + } + +# ifdef G2O_OPENMP +# pragma omp parallel for default (shared) if (_activeEdges.size() > 50) +# endif + for (int k = 0; k < static_cast(_activeEdges.size()); ++k) { + OptimizableGraph::Edge* e = _activeEdges[k]; + e->computeError(); + } + +# ifndef NDEBUG + for (int k = 0; k < static_cast(_activeEdges.size()); ++k) { + OptimizableGraph::Edge* e = _activeEdges[k]; + bool hasNan = arrayHasNaN(e->errorData(), e->dimension()); + if (hasNan) { + cerr << "computeActiveErrors(): found NaN in error for edge " << e << endl; + } + } +# endif + + } + + double SparseOptimizer::activeChi2( ) const + { + double chi = 0.0; + for (EdgeContainer::const_iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) { + const OptimizableGraph::Edge* e = *it; + chi += e->chi2(); + } + return chi; + } + + double SparseOptimizer::activeRobustChi2() const + { + Eigen::Vector3d rho; + double chi = 0.0; + for (EdgeContainer::const_iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) { + const OptimizableGraph::Edge* e = *it; + if (e->robustKernel()) { + e->robustKernel()->robustify(e->chi2(), rho); + chi += rho[0]; + } + else + chi += e->chi2(); + } + return chi; + } + + OptimizableGraph::Vertex* SparseOptimizer::findGauge(){ + if (vertices().empty()) + return 0; + + int maxDim=0; + for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){ + OptimizableGraph::Vertex* v=static_cast(it->second); + maxDim=std::max(maxDim,v->dimension()); + } + + OptimizableGraph::Vertex* rut=0; + for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){ + OptimizableGraph::Vertex* v=static_cast(it->second); + if (v->dimension()==maxDim){ + rut=v; + break; + } + } + return rut; + } + + bool SparseOptimizer::gaugeFreedom() + { + if (vertices().empty()) + return false; + + int maxDim=0; + for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){ + OptimizableGraph::Vertex* v=static_cast(it->second); + maxDim = std::max(maxDim,v->dimension()); + } + + for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){ + OptimizableGraph::Vertex* v=static_cast(it->second); + if (v->dimension() == maxDim) { + // test for fixed vertex + if (v->fixed()) { + return false; + } + // test for full dimension prior + for (HyperGraph::EdgeSet::const_iterator eit = v->edges().begin(); eit != v->edges().end(); ++eit) { + OptimizableGraph::Edge* e = static_cast(*eit); + if (e->vertices().size() == 1 && e->dimension() == maxDim) + return false; + } + } + } + return true; + } + + bool SparseOptimizer::buildIndexMapping(SparseOptimizer::VertexContainer& vlist){ + if (! vlist.size()){ + _ivMap.clear(); + return false; + } + + _ivMap.resize(vlist.size()); + size_t i = 0; + for (int k=0; k<2; k++) + for (VertexContainer::iterator it=vlist.begin(); it!=vlist.end(); ++it){ + OptimizableGraph::Vertex* v = *it; + if (! v->fixed()){ + if (static_cast(v->marginalized()) == k){ + v->setHessianIndex(i); + _ivMap[i]=v; + i++; + } + } + else { + v->setHessianIndex(-1); + } + } + _ivMap.resize(i); + return true; + } + + void SparseOptimizer::clearIndexMapping(){ + for (size_t i=0; i<_ivMap.size(); ++i){ + _ivMap[i]->setHessianIndex(-1); + _ivMap[i]=0; + } + } + + bool SparseOptimizer::initializeOptimization(int level){ + HyperGraph::VertexSet vset; + for (VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it) + vset.insert(it->second); + return initializeOptimization(vset,level); + } + + bool SparseOptimizer::initializeOptimization(HyperGraph::VertexSet& vset, int level){ + if (edges().size() == 0) { + cerr << __PRETTY_FUNCTION__ << ": Attempt to initialize an empty graph" << endl; + return false; + } + bool workspaceAllocated = _jacobianWorkspace.allocate(); (void) workspaceAllocated; + assert(workspaceAllocated && "Error while allocating memory for the Jacobians"); + clearIndexMapping(); + _activeVertices.clear(); + _activeVertices.reserve(vset.size()); + _activeEdges.clear(); + set auxEdgeSet; // temporary structure to avoid duplicates + for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it){ + OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*) *it; + const OptimizableGraph::EdgeSet& vEdges=v->edges(); + // count if there are edges in that level. If not remove from the pool + int levelEdges=0; + for (OptimizableGraph::EdgeSet::const_iterator it=vEdges.begin(); it!=vEdges.end(); ++it){ + OptimizableGraph::Edge* e=reinterpret_cast(*it); + if (level < 0 || e->level() == level) { + + bool allVerticesOK = true; + for (vector::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) { + if (vset.find(*vit) == vset.end()) { + allVerticesOK = false; + break; + } + } + if (allVerticesOK && !e->allVerticesFixed()) { + auxEdgeSet.insert(e); + levelEdges++; + } + + } + } + if (levelEdges){ + _activeVertices.push_back(v); + + // test for NANs in the current estimate if we are debugging +# ifndef NDEBUG + int estimateDim = v->estimateDimension(); + if (estimateDim > 0) { + Eigen::VectorXd estimateData(estimateDim); + if (v->getEstimateData(estimateData.data()) == true) { + int k; + bool hasNan = arrayHasNaN(estimateData.data(), estimateDim, &k); + if (hasNan) + cerr << __PRETTY_FUNCTION__ << ": Vertex " << v->id() << " contains a nan entry at index " << k << endl; + } + } +# endif + + } + } + + _activeEdges.reserve(auxEdgeSet.size()); + for (set::iterator it = auxEdgeSet.begin(); it != auxEdgeSet.end(); ++it) + _activeEdges.push_back(*it); + + sortVectorContainers(); + return buildIndexMapping(_activeVertices); + } + + bool SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset){ + bool workspaceAllocated = _jacobianWorkspace.allocate(); (void) workspaceAllocated; + assert(workspaceAllocated && "Error while allocating memory for the Jacobians"); + clearIndexMapping(); + _activeVertices.clear(); + _activeEdges.clear(); + _activeEdges.reserve(eset.size()); + set auxVertexSet; // temporary structure to avoid duplicates + for (HyperGraph::EdgeSet::iterator it=eset.begin(); it!=eset.end(); ++it){ + OptimizableGraph::Edge* e=(OptimizableGraph::Edge*)(*it); + for (vector::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) { + auxVertexSet.insert(static_cast(*vit)); + } + _activeEdges.push_back(reinterpret_cast(*it)); + } + + _activeVertices.reserve(auxVertexSet.size()); + for (set::iterator it = auxVertexSet.begin(); it != auxVertexSet.end(); ++it) + _activeVertices.push_back(*it); + + sortVectorContainers(); + return buildIndexMapping(_activeVertices); + } + + void SparseOptimizer::setToOrigin(){ + for (VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it) { + OptimizableGraph::Vertex* v = static_cast(it->second); + v->setToOrigin(); + } + } + + void SparseOptimizer::computeInitialGuess() + { + EstimatePropagator::PropagateCost costFunction(this); + computeInitialGuess(costFunction); + } + + void SparseOptimizer::computeInitialGuess(EstimatePropagatorCost& costFunction) + { + OptimizableGraph::VertexSet emptySet; + std::set backupVertices; + HyperGraph::VertexSet fixedVertices; // these are the root nodes where to start the initialization + for (EdgeContainer::iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) { + OptimizableGraph::Edge* e = *it; + for (size_t i = 0; i < e->vertices().size(); ++i) { + OptimizableGraph::Vertex* v = static_cast(e->vertex(i)); + if (v->fixed()) + fixedVertices.insert(v); + else { // check for having a prior which is able to fully initialize a vertex + for (EdgeSet::const_iterator vedgeIt = v->edges().begin(); vedgeIt != v->edges().end(); ++vedgeIt) { + OptimizableGraph::Edge* vedge = static_cast(*vedgeIt); + if (vedge->vertices().size() == 1 && vedge->initialEstimatePossible(emptySet, v) > 0.) { + //cerr << "Initialize with prior for " << v->id() << endl; + vedge->initialEstimate(emptySet, v); + fixedVertices.insert(v); + } + } + } + if (v->hessianIndex() == -1) { + std::set::const_iterator foundIt = backupVertices.find(v); + if (foundIt == backupVertices.end()) { + v->push(); + backupVertices.insert(v); + } + } + } + } + + EstimatePropagator estimatePropagator(this); + estimatePropagator.propagate(fixedVertices, costFunction); + + // restoring the vertices that should not be initialized + for (std::set::iterator it = backupVertices.begin(); it != backupVertices.end(); ++it) { + Vertex* v = *it; + v->pop(); + } + if (verbose()) { + computeActiveErrors(); + cerr << "iteration= -1\t chi2= " << activeChi2() + << "\t time= 0.0" + << "\t cumTime= 0.0" + << "\t (using initial guess from " << costFunction.name() << ")" << endl; + } + } + + int SparseOptimizer::optimize(int iterations, bool online) + { + if (_ivMap.size() == 0) { + cerr << __PRETTY_FUNCTION__ << ": 0 vertices to optimize, maybe forgot to call initializeOptimization()" << endl; + return -1; + } + + int cjIterations=0; + double cumTime=0; + bool ok=true; + + ok = _algorithm->init(online); + if (! ok) { + cerr << __PRETTY_FUNCTION__ << " Error while initializing" << endl; + return -1; + } + + _batchStatistics.clear(); + if (_computeBatchStatistics) + _batchStatistics.resize(iterations); + + OptimizationAlgorithm::SolverResult result = OptimizationAlgorithm::OK; + for (int i=0; isolve(i, online); + ok = ( result == OptimizationAlgorithm::OK ); + + bool errorComputed = false; + if (_computeBatchStatistics) { + computeActiveErrors(); + errorComputed = true; + _batchStatistics[i].chi2 = activeRobustChi2(); + _batchStatistics[i].timeIteration = get_monotonic_time()-ts; + } + + if (verbose()){ + double dts = get_monotonic_time()-ts; + cumTime += dts; + if (! errorComputed) + computeActiveErrors(); + cerr << "iteration= " << i + << "\t chi2= " << FIXED(activeRobustChi2()) + << "\t time= " << dts + << "\t cumTime= " << cumTime + << "\t edges= " << _activeEdges.size(); + _algorithm->printVerbose(cerr); + cerr << endl; + } + ++cjIterations; + postIteration(i); + } + if (result == OptimizationAlgorithm::Fail) { + return 0; + } + return cjIterations; + } + + + void SparseOptimizer::update(const double* update) + { + // update the graph by calling oplus on the vertices + for (size_t i=0; i < _ivMap.size(); ++i) { + OptimizableGraph::Vertex* v= _ivMap[i]; +#ifndef NDEBUG + bool hasNan = arrayHasNaN(update, v->dimension()); + if (hasNan) + cerr << __PRETTY_FUNCTION__ << ": Update contains a nan for vertex " << v->id() << endl; +#endif + v->oplus(update); + update += v->dimension(); + } + } + + void SparseOptimizer::setComputeBatchStatistics(bool computeBatchStatistics) + { + if ((_computeBatchStatistics == true) && (computeBatchStatistics == false)) { + G2OBatchStatistics::setGlobalStats(0); + _batchStatistics.clear(); + } + _computeBatchStatistics = computeBatchStatistics; + } + + bool SparseOptimizer::updateInitialization(HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset) + { + std::vector newVertices; + newVertices.reserve(vset.size()); + _activeVertices.reserve(_activeVertices.size() + vset.size()); + _activeEdges.reserve(_activeEdges.size() + eset.size()); + for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end(); ++it) { + OptimizableGraph::Edge* e = static_cast(*it); + if (!e->allVerticesFixed()) _activeEdges.push_back(e); + } + + // update the index mapping + size_t next = _ivMap.size(); + for (HyperGraph::VertexSet::iterator it = vset.begin(); it != vset.end(); ++it) { + OptimizableGraph::Vertex* v=static_cast(*it); + if (! v->fixed()){ + if (! v->marginalized()){ + v->setHessianIndex(next); + _ivMap.push_back(v); + newVertices.push_back(v); + _activeVertices.push_back(v); + next++; + } + else // not supported right now + abort(); + } + else { + v->setHessianIndex(-1); + } + } + + //if (newVertices.size() != vset.size()) + //cerr << __PRETTY_FUNCTION__ << ": something went wrong " << PVAR(vset.size()) << " " << PVAR(newVertices.size()) << endl; + return _algorithm->updateStructure(newVertices, eset); + } + + void SparseOptimizer::sortVectorContainers() + { + // sort vector structures to get deterministic ordering based on IDs + sort(_activeVertices.begin(), _activeVertices.end(), VertexIDCompare()); + sort(_activeEdges.begin(), _activeEdges.end(), EdgeIDCompare()); + } + + void SparseOptimizer::clear() { + _ivMap.clear(); + _activeVertices.clear(); + _activeEdges.clear(); + OptimizableGraph::clear(); + } + + SparseOptimizer::VertexContainer::const_iterator SparseOptimizer::findActiveVertex(const OptimizableGraph::Vertex* v) const + { + VertexContainer::const_iterator lower = lower_bound(_activeVertices.begin(), _activeVertices.end(), v, VertexIDCompare()); + if (lower == _activeVertices.end()) + return _activeVertices.end(); + if ((*lower) == v) + return lower; + return _activeVertices.end(); + } + + SparseOptimizer::EdgeContainer::const_iterator SparseOptimizer::findActiveEdge(const OptimizableGraph::Edge* e) const + { + EdgeContainer::const_iterator lower = lower_bound(_activeEdges.begin(), _activeEdges.end(), e, EdgeIDCompare()); + if (lower == _activeEdges.end()) + return _activeEdges.end(); + if ((*lower) == e) + return lower; + return _activeEdges.end(); + } + + void SparseOptimizer::push(SparseOptimizer::VertexContainer& vlist) + { + for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it) + (*it)->push(); + } + + void SparseOptimizer::pop(SparseOptimizer::VertexContainer& vlist) + { + for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it) + (*it)->pop(); + } + + void SparseOptimizer::push(HyperGraph::VertexSet& vlist) + { + for (HyperGraph::VertexSet::iterator it = vlist.begin(); it != vlist.end(); ++it) { + OptimizableGraph::Vertex* v = dynamic_cast(*it); + if (v) + v->push(); + else + cerr << __FUNCTION__ << ": FATAL PUSH SET" << endl; + } + } + + void SparseOptimizer::pop(HyperGraph::VertexSet& vlist) + { + for (HyperGraph::VertexSet::iterator it = vlist.begin(); it != vlist.end(); ++it){ + OptimizableGraph::Vertex* v = dynamic_cast (*it); + if (v) + v->pop(); + else + cerr << __FUNCTION__ << ": FATAL POP SET" << endl; + } + } + + void SparseOptimizer::discardTop(SparseOptimizer::VertexContainer& vlist) + { + for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it) + (*it)->discardTop(); + } + + void SparseOptimizer::setVerbose(bool verbose) + { + _verbose = verbose; + } + + void SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm) + { + if (_algorithm) // reset the optimizer for the formerly used solver + _algorithm->setOptimizer(0); + _algorithm = algorithm; + if (_algorithm) + _algorithm->setOptimizer(this); + } + + bool SparseOptimizer::computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices){ + return _algorithm->computeMarginals(spinv, blockIndices); + } + + void SparseOptimizer::setForceStopFlag(bool* flag) + { + _forceStopFlag=flag; + } + + bool SparseOptimizer::removeVertex(HyperGraph::Vertex* v) + { + OptimizableGraph::Vertex* vv = static_cast(v); + if (vv->hessianIndex() >= 0) { + clearIndexMapping(); + _ivMap.clear(); + } + return HyperGraph::removeVertex(v); + } + + bool SparseOptimizer::addComputeErrorAction(HyperGraphAction* action) + { + std::pair insertResult = _graphActions[AT_COMPUTEACTIVERROR].insert(action); + return insertResult.second; + } + + bool SparseOptimizer::removeComputeErrorAction(HyperGraphAction* action) + { + return _graphActions[AT_COMPUTEACTIVERROR].erase(action) > 0; + } + + void SparseOptimizer::push() + { + push(_activeVertices); + } + + void SparseOptimizer::pop() + { + pop(_activeVertices); + } + + void SparseOptimizer::discardTop() + { + discardTop(_activeVertices); + } + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_optimizer.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_optimizer.h new file mode 100644 index 0000000..3b09f5a --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/core/sparse_optimizer.h @@ -0,0 +1,312 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_GRAPH_OPTIMIZER_CHOL_H_ +#define G2O_GRAPH_OPTIMIZER_CHOL_H_ + +#include "../stuff/macros.h" + +#include "optimizable_graph.h" +#include "sparse_block_matrix.h" +#include "batch_stats.h" + +#include + +namespace g2o { + + // forward declaration + class ActivePathCostFunction; + class OptimizationAlgorithm; + class EstimatePropagatorCost; + + class SparseOptimizer : public OptimizableGraph { + + public: + enum { + AT_COMPUTEACTIVERROR = OptimizableGraph::AT_NUM_ELEMENTS, + AT_NUM_ELEMENTS, // keep as last element + }; + + friend class ActivePathCostFunction; + + // Attention: _solver & _statistics is own by SparseOptimizer and will be + // deleted in its destructor. + SparseOptimizer(); + virtual ~SparseOptimizer(); + + // new interface for the optimizer + // the old functions will be dropped + /** + * Initializes the structures for optimizing a portion of the graph specified by a subset of edges. + * Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the + * schur complement or to set as fixed during the optimization. + * @param eset: the subgraph to be optimized. + * @returns false if somethings goes wrong + */ + virtual bool initializeOptimization(HyperGraph::EdgeSet& eset); + + /** + * Initializes the structures for optimizing a portion of the graph specified by a subset of vertices. + * Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the + * schur complement or to set as fixed during the optimization. + * @param vset: the subgraph to be optimized. + * @param level: is the level (in multilevel optimization) + * @returns false if somethings goes wrong + */ + virtual bool initializeOptimization(HyperGraph::VertexSet& vset, int level=0); + + /** + * Initializes the structures for optimizing the whole graph. + * Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the + * schur complement or to set as fixed during the optimization. + * @param level: is the level (in multilevel optimization) + * @returns false if somethings goes wrong + */ + virtual bool initializeOptimization(int level=0); + + /** + * HACK updating the internal structures for online processing + */ + virtual bool updateInitialization(HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset); + + /** + * Propagates an initial guess from the vertex specified as origin. + * It should be called after initializeOptimization(...), as it relies on the _activeVertices/_edges structures. + * It constructs a set of trees starting from the nodes in the graph which are fixed and eligible for preforming optimization. + * The trees are constructed by utlizing a cost-function specified. + * @param costFunction: the cost function used + * @patam maxDistance: the distance where to stop the search + */ + virtual void computeInitialGuess(); + + /** + * Same as above but using a specific propagator + */ + virtual void computeInitialGuess(EstimatePropagatorCost& propagator); + + /** + * sets all vertices to their origin. + */ + virtual void setToOrigin(); + + + /** + * starts one optimization run given the current configuration of the graph, + * and the current settings stored in the class instance. + * It can be called only after initializeOptimization + */ + int optimize(int iterations, bool online = false); + + /** + * computes the blocks of the inverse of the specified pattern. + * the pattern is given via pairs of the blocks in the hessian + * @param blockIndices: the pattern + * @param spinv: the sparse block matrix with the result + * @returns false if the operation is not supported by the solver + */ + bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices); + + /** + * computes the inverse of the specified vertex. + * @param vertex: the vertex whose state is to be marginalised + * @param spinv: the sparse block matrix with the result + * @returns false if the operation is not supported by the solver + */ + bool computeMarginals(SparseBlockMatrix& spinv, const Vertex* vertex) { + if (vertex->hessianIndex() < 0) { + return false; + } + std::vector > index; + index.push_back(std::pair(vertex->hessianIndex(), vertex->hessianIndex())); + return computeMarginals(spinv, index); + } + + /** + * computes the inverse of the set specified vertices, assembled into a single covariance matrix. + * @param vertex: the pattern + * @param spinv: the sparse block matrix with the result + * @returns false if the operation is not supported by the solver + */ + bool computeMarginals(SparseBlockMatrix& spinv, const VertexContainer& vertices) { + std::vector > indices; + for (VertexContainer::const_iterator it = vertices.begin(); it != vertices.end(); ++it) { + indices.push_back(std::pair((*it)->hessianIndex(),(*it)->hessianIndex())); + } + return computeMarginals(spinv, indices); + } + + //! finds a gauge in the graph to remove the undefined dof. + // The gauge should be fixed() and then the optimization can work (if no additional dof are in + // the system. The default implementation returns a node with maximum dimension. + virtual Vertex* findGauge(); + + bool gaugeFreedom(); + + /**returns the cached chi2 of the active portion of the graph*/ + double activeChi2() const; + /** + * returns the cached chi2 of the active portion of the graph. + * In contrast to activeChi2() this functions considers the weighting + * of the error according to the robustification of the error functions. + */ + double activeRobustChi2() const; + + //! verbose information during optimization + bool verbose() const {return _verbose;} + void setVerbose(bool verbose); + + /** + * sets a variable checked at every iteration to force a user stop. The iteration exits when the variable is true; + */ + void setForceStopFlag(bool* flag); + bool* forceStopFlag() const { return _forceStopFlag;}; + + //! if external stop flag is given, return its state. False otherwise + bool terminate() {return _forceStopFlag ? (*_forceStopFlag) : false; } + + //! the index mapping of the vertices + const VertexContainer& indexMapping() const {return _ivMap;} + //! the vertices active in the current optimization + const VertexContainer& activeVertices() const { return _activeVertices;} + //! the edges active in the current optimization + const EdgeContainer& activeEdges() const { return _activeEdges;} + + /** + * Remove a vertex. If the vertex is contained in the currently active set + * of vertices, then the internal temporary structures are cleaned, e.g., the index + * mapping is erased. In case you need the index mapping for manipulating the + * graph, you have to store it in your own copy. + */ + virtual bool removeVertex(HyperGraph::Vertex* v); + + /** + * search for an edge in _activeVertices and return the iterator pointing to it + * getActiveVertices().end() if not found + */ + VertexContainer::const_iterator findActiveVertex(const OptimizableGraph::Vertex* v) const; + /** + * search for an edge in _activeEdges and return the iterator pointing to it + * getActiveEdges().end() if not found + */ + EdgeContainer::const_iterator findActiveEdge(const OptimizableGraph::Edge* e) const; + + //! the solver used by the optimizer + const OptimizationAlgorithm* algorithm() const { return _algorithm;} + OptimizationAlgorithm* solver() { return _algorithm;} + void setAlgorithm(OptimizationAlgorithm* algorithm); + + //! push the estimate of a subset of the variables onto a stack + void push(SparseOptimizer::VertexContainer& vlist); + //! push the estimate of a subset of the variables onto a stack + void push(HyperGraph::VertexSet& vlist); + //! push all the active vertices onto a stack + void push(); + //! pop (restore) the estimate a subset of the variables from the stack + void pop(SparseOptimizer::VertexContainer& vlist); + //! pop (restore) the estimate a subset of the variables from the stack + void pop(HyperGraph::VertexSet& vlist); + //! pop (restore) the estimate of the active vertices from the stack + void pop(); + + //! ignore the latest stored element on the stack, remove it from the stack but do not restore the estimate + void discardTop(SparseOptimizer::VertexContainer& vlist); + //! same as above, but for the active vertices + void discardTop(); + using OptimizableGraph::discardTop; + + /** + * clears the graph, and polishes some intermediate structures + * Note that this only removes nodes / edges. Parameters can be removed + * with clearParameters(). + */ + virtual void clear(); + + /** + * computes the error vectors of all edges in the activeSet, and caches them + */ + void computeActiveErrors(); + + /** + * Linearizes the system by computing the Jacobians for the nodes + * and edges in the graph + */ + G2O_ATTRIBUTE_DEPRECATED(void linearizeSystem()) + { + // nothing needed, linearization is now done inside the solver + } + + /** + * update the estimate of the active vertices + * @param update: the double vector containing the stacked + * elements of the increments on the vertices. + */ + void update(const double* update); + + /** + returns the set of batch statistics about the optimisation + */ + const BatchStatisticsContainer& batchStatistics() const { return _batchStatistics;} + /** + returns the set of batch statistics about the optimisation + */ + BatchStatisticsContainer& batchStatistics() { return _batchStatistics;} + + void setComputeBatchStatistics(bool computeBatchStatistics); + + bool computeBatchStatistics() const { return _computeBatchStatistics;} + + /**** callbacks ****/ + //! add an action to be executed before the error vectors are computed + bool addComputeErrorAction(HyperGraphAction* action); + //! remove an action that should no longer be execured before computing the error vectors + bool removeComputeErrorAction(HyperGraphAction* action); + + + + protected: + bool* _forceStopFlag; + bool _verbose; + + VertexContainer _ivMap; + VertexContainer _activeVertices; ///< sorted according to VertexIDCompare + EdgeContainer _activeEdges; ///< sorted according to EdgeIDCompare + + void sortVectorContainers(); + + OptimizationAlgorithm* _algorithm; + + /** + * builds the mapping of the active vertices to the (block) row / column in the Hessian + */ + bool buildIndexMapping(SparseOptimizer::VertexContainer& vlist); + void clearIndexMapping(); + + BatchStatisticsContainer _batchStatistics; ///< global statistics of the optimizer, e.g., timing, num-non-zeros + bool _computeBatchStatistics; + }; +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/solvers/linear_solver_dense.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/solvers/linear_solver_dense.h new file mode 100644 index 0000000..b34674c --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/solvers/linear_solver_dense.h @@ -0,0 +1,125 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 H. Strasdat +// Copyright (C) 2012 R. Kümmerle +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_LINEAR_SOLVER_DENSE_H +#define G2O_LINEAR_SOLVER_DENSE_H + +#include "../core/linear_solver.h" +#include "../core/batch_stats.h" + +#include +#include +#include +#include + + +namespace g2o { + + /** + * \brief linear solver using dense cholesky decomposition + */ + template + class LinearSolverDense : public LinearSolver + { + public: + LinearSolverDense() : + LinearSolver(), + _reset(true) + { + } + + virtual ~LinearSolverDense() + { + } + + virtual bool init() + { + _reset = true; + return true; + } + + bool solve(const SparseBlockMatrix& A, double* x, double* b) + { + int n = A.cols(); + int m = A.cols(); + + Eigen::MatrixXd& H = _H; + if (H.cols() != n) { + H.resize(n, m); + _reset = true; + } + if (_reset) { + _reset = false; + H.setZero(); + } + + // copy the sparse block matrix into a dense matrix + int c_idx = 0; + for (size_t i = 0; i < A.blockCols().size(); ++i) { + int c_size = A.colsOfBlock(i); + assert(c_idx == A.colBaseOfBlock(i) && "mismatch in block indices"); + + const typename SparseBlockMatrix::IntBlockMap& col = A.blockCols()[i]; + if (col.size() > 0) { + typename SparseBlockMatrix::IntBlockMap::const_iterator it; + for (it = col.begin(); it != col.end(); ++it) { + int r_idx = A.rowBaseOfBlock(it->first); + // only the upper triangular block is processed + if (it->first <= (int)i) { + int r_size = A.rowsOfBlock(it->first); + H.block(r_idx, c_idx, r_size, c_size) = *(it->second); + if (r_idx != c_idx) // write the lower triangular block + H.block(c_idx, r_idx, c_size, r_size) = it->second->transpose(); + } + } + } + + c_idx += c_size; + } + + // solving via Cholesky decomposition + Eigen::VectorXd::MapType xvec(x, m); + Eigen::VectorXd::ConstMapType bvec(b, n); + _cholesky.compute(H); + if (_cholesky.isPositive()) { + xvec = _cholesky.solve(bvec); + return true; + } + return false; + } + + protected: + bool _reset; + Eigen::MatrixXd _H; + Eigen::LDLT _cholesky; + + }; + + +}// end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h new file mode 100644 index 0000000..492aea9 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h @@ -0,0 +1,237 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_LINEAR_SOLVER_EIGEN_H +#define G2O_LINEAR_SOLVER_EIGEN_H + +#include +#include + +#include "../core/linear_solver.h" +#include "../core/batch_stats.h" +#include "../stuff/timeutil.h" + +#include "../core/eigen_types.h" + +#include +#include + +namespace g2o { + +/** + * \brief linear solver which uses the sparse Cholesky solver from Eigen + * + * Has no dependencies except Eigen. Hence, should compile almost everywhere + * without to much issues. Performance should be similar to CSparse, I guess. + */ +template +class LinearSolverEigen: public LinearSolver +{ + public: + typedef Eigen::SparseMatrix SparseMatrix; + typedef Eigen::Triplet Triplet; + typedef Eigen::PermutationMatrix PermutationMatrix; + /** + * \brief Sub-classing Eigen's SimplicialLDLT to perform ordering with a given ordering + */ + class CholeskyDecomposition : public Eigen::SimplicialLDLT + { + public: + CholeskyDecomposition() : Eigen::SimplicialLDLT() {} + using Eigen::SimplicialLDLT< SparseMatrix, Eigen::Upper>::analyzePattern_preordered; + + void analyzePatternWithPermutation(SparseMatrix& a, const PermutationMatrix& permutation) + { + m_Pinv = permutation; + m_P = permutation.inverse(); + int size = a.cols(); + SparseMatrix ap(size, size); + ap.selfadjointView() = a.selfadjointView().twistedBy(m_P); + analyzePattern_preordered(ap, true); + } + }; + + public: + LinearSolverEigen() : + LinearSolver(), + _init(true), _blockOrdering(false), _writeDebug(false) + { + } + + virtual ~LinearSolverEigen() + { + } + + virtual bool init() + { + _init = true; + return true; + } + + bool solve(const SparseBlockMatrix& A, double* x, double* b) + { + if (_init) + _sparseMatrix.resize(A.rows(), A.cols()); + fillSparseMatrix(A, !_init); + if (_init) // compute the symbolic composition once + computeSymbolicDecomposition(A); + _init = false; + + double t=get_monotonic_time(); + _cholesky.factorize(_sparseMatrix); + if (_cholesky.info() != Eigen::Success) { // the matrix is not positive definite + if (_writeDebug) { + std::cerr << "Cholesky failure, writing debug.txt (Hessian loadable by Octave)" << std::endl; + A.writeOctave("debug.txt"); + } + return false; + } + + // Solving the system + VectorXD::MapType xx(x, _sparseMatrix.cols()); + VectorXD::ConstMapType bb(b, _sparseMatrix.cols()); + xx = _cholesky.solve(bb); + G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats(); + if (globalStats) { + globalStats->timeNumericDecomposition = get_monotonic_time() - t; + globalStats->choleskyNNZ = _cholesky.matrixL().nestedExpression().nonZeros() + _sparseMatrix.cols(); // the elements of D + } + + return true; + } + + //! do the AMD ordering on the blocks or on the scalar matrix + bool blockOrdering() const { return _blockOrdering;} + void setBlockOrdering(bool blockOrdering) { _blockOrdering = blockOrdering;} + + //! write a debug dump of the system matrix if it is not SPD in solve + virtual bool writeDebug() const { return _writeDebug;} + virtual void setWriteDebug(bool b) { _writeDebug = b;} + + protected: + bool _init; + bool _blockOrdering; + bool _writeDebug; + SparseMatrix _sparseMatrix; + CholeskyDecomposition _cholesky; + + /** + * compute the symbolic decompostion of the matrix only once. + * Since A has the same pattern in all the iterations, we only + * compute the fill-in reducing ordering once and re-use for all + * the following iterations. + */ + void computeSymbolicDecomposition(const SparseBlockMatrix& A) + { + double t=get_monotonic_time(); + if (! _blockOrdering) { + _cholesky.analyzePattern(_sparseMatrix); + } else { + // block ordering with the Eigen Interface + // This is really ugly currently, as it calls internal functions from Eigen + // and modifies the SparseMatrix class + Eigen::PermutationMatrix blockP; + { + // prepare a block structure matrix for calling AMD + std::vector triplets; + for (size_t c = 0; c < A.blockCols().size(); ++c){ + const typename SparseBlockMatrix::IntBlockMap& column = A.blockCols()[c]; + for (typename SparseBlockMatrix::IntBlockMap::const_iterator it = column.begin(); it != column.end(); ++it) { + const int& r = it->first; + if (r > static_cast(c)) // only upper triangle + break; + triplets.push_back(Triplet(r, c, 0.)); + } + } + + // call the AMD ordering on the block matrix. + // Relies on Eigen's internal stuff, probably bad idea + SparseMatrix auxBlockMatrix(A.blockCols().size(), A.blockCols().size()); + auxBlockMatrix.setFromTriplets(triplets.begin(), triplets.end()); + typename CholeskyDecomposition::CholMatrixType C; + C = auxBlockMatrix.selfadjointView(); + Eigen::internal::minimum_degree_ordering(C, blockP); + } + + int rows = A.rows(); + assert(rows == A.cols() && "Matrix A is not square"); + + // Adapt the block permutation to the scalar matrix + PermutationMatrix scalarP; + scalarP.resize(rows); + int scalarIdx = 0; + for (int i = 0; i < blockP.size(); ++i) { + const int& p = blockP.indices()(i); + int base = A.colBaseOfBlock(p); + int nCols = A.colsOfBlock(p); + for (int j = 0; j < nCols; ++j) + scalarP.indices()(scalarIdx++) = base++; + } + assert(scalarIdx == rows && "did not completely fill the permutation matrix"); + // analyze with the scalar permutation + _cholesky.analyzePatternWithPermutation(_sparseMatrix, scalarP); + + } + G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats(); + if (globalStats) + globalStats->timeSymbolicDecomposition = get_monotonic_time() - t; + } + + void fillSparseMatrix(const SparseBlockMatrix& A, bool onlyValues) + { + if (onlyValues) { + A.fillCCS(_sparseMatrix.valuePtr(), true); + } else { + + // create from triplet structure + std::vector triplets; + triplets.reserve(A.nonZeros()); + for (size_t c = 0; c < A.blockCols().size(); ++c) { + int colBaseOfBlock = A.colBaseOfBlock(c); + const typename SparseBlockMatrix::IntBlockMap& column = A.blockCols()[c]; + for (typename SparseBlockMatrix::IntBlockMap::const_iterator it = column.begin(); it != column.end(); ++it) { + int rowBaseOfBlock = A.rowBaseOfBlock(it->first); + const MatrixType& m = *(it->second); + for (int cc = 0; cc < m.cols(); ++cc) { + int aux_c = colBaseOfBlock + cc; + for (int rr = 0; rr < m.rows(); ++rr) { + int aux_r = rowBaseOfBlock + rr; + if (aux_r > aux_c) + break; + triplets.push_back(Triplet(aux_r, aux_c, m(rr, cc))); + } + } + } + } + _sparseMatrix.setFromTriplets(triplets.begin(), triplets.end()); + + } + } +}; + +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/color_macros.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/color_macros.h new file mode 100644 index 0000000..7115bbe --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/color_macros.h @@ -0,0 +1,65 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_COLOR_MACROS_H +#define G2O_COLOR_MACROS_H + +// font attributes +#define FT_BOLD "\033[1m" +#define FT_UNDERLINE "\033[4m" + +//background color +#define BG_BLACK "\033[40m" +#define BG_RED "\033[41m" +#define BG_GREEN "\033[42m" +#define BG_YELLOW "\033[43m" +#define BG_LIGHTBLUE "\033[44m" +#define BG_MAGENTA "\033[45m" +#define BG_BLUE "\033[46m" +#define BG_WHITE "\033[47m" + +// font color +#define CL_BLACK(s) "\033[30m" << s << "\033[0m" +#define CL_RED(s) "\033[31m" << s << "\033[0m" +#define CL_GREEN(s) "\033[32m" << s << "\033[0m" +#define CL_YELLOW(s) "\033[33m" << s << "\033[0m" +#define CL_LIGHTBLUE(s) "\033[34m" << s << "\033[0m" +#define CL_MAGENTA(s) "\033[35m" << s << "\033[0m" +#define CL_BLUE(s) "\033[36m" << s << "\033[0m" +#define CL_WHITE(s) "\033[37m" << s << "\033[0m" + +#define FG_BLACK "\033[30m" +#define FG_RED "\033[31m" +#define FG_GREEN "\033[32m" +#define FG_YELLOW "\033[33m" +#define FG_LIGHTBLUE "\033[34m" +#define FG_MAGENTA "\033[35m" +#define FG_BLUE "\033[36m" +#define FG_WHITE "\033[37m" + +#define FG_NORM "\033[0m" + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/macros.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/macros.h new file mode 100644 index 0000000..be4a245 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/macros.h @@ -0,0 +1,134 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_MACROS_H +#define G2O_MACROS_H + +#ifndef DEG2RAD +#define DEG2RAD(x) ((x) * 0.01745329251994329575) +#endif + +#ifndef RAD2DEG +#define RAD2DEG(x) ((x) * 57.29577951308232087721) +#endif + +// GCC the one and only +#if defined(__GNUC__) +# define G2O_ATTRIBUTE_CONSTRUCTOR(func) \ + static void func(void)__attribute__ ((constructor)); \ + static void func(void) + +# define G2O_ATTRIBUTE_UNUSED __attribute__((unused)) +# define G2O_ATTRIBUTE_FORMAT12 __attribute__ ((format (printf, 1, 2))) +# define G2O_ATTRIBUTE_FORMAT23 __attribute__ ((format (printf, 2, 3))) +# define G2O_ATTRIBUTE_WARNING(func) func __attribute__((warning)) +# define G2O_ATTRIBUTE_DEPRECATED(func) func __attribute__((deprecated)) + +#ifdef ANDROID +# define g2o_isnan(x) isnan(x) +# define g2o_isinf(x) isinf(x) +# define g2o_isfinite(x) isfinite(x) +#else +# define g2o_isnan(x) std::isnan(x) +# define g2o_isinf(x) std::isinf(x) +# define g2o_isfinite(x) std::isfinite(x) +#endif + +// MSVC on Windows +#elif defined _MSC_VER +# define __PRETTY_FUNCTION__ __FUNCTION__ + +/** +Modified by Mark Pupilli from: + + "Initializer/finalizer sample for MSVC and GCC. + 2010 Joe Lowe. Released into the public domain." + + "For MSVC, places a ptr to the function in the user initializer section (.CRT$XCU), basically the same thing the compiler does for the constructor calls for static C++ objects. For GCC, uses a constructor attribute." + + (As posted on Stack OVerflow) +*/ +# define G2O_ATTRIBUTE_CONSTRUCTOR(f) \ + __pragma(section(".CRT$XCU",read)) \ + static void __cdecl f(void); \ + __declspec(allocate(".CRT$XCU")) void (__cdecl*f##_)(void) = f; \ + static void __cdecl f(void) + +# define G2O_ATTRIBUTE_UNUSED +# define G2O_ATTRIBUTE_FORMAT12 +# define G2O_ATTRIBUTE_FORMAT23 +# define G2O_ATTRIBUTE_WARNING(func) func +# define G2O_ATTRIBUTE_DEPRECATED(func) func + +#include + +# define g2o_isnan(x) _isnan(x) +# define g2o_isinf(x) (_finite(x) == 0) +# define g2o_isfinite(x) (_finite(x) != 0) + +// unknown compiler +#else +# ifndef __PRETTY_FUNCTION__ +# define __PRETTY_FUNCTION__ "" +# endif +# define G2O_ATTRIBUTE_CONSTRUCTOR(func) func +# define G2O_ATTRIBUTE_UNUSED +# define G2O_ATTRIBUTE_FORMAT12 +# define G2O_ATTRIBUTE_FORMAT23 +# define G2O_ATTRIBUTE_WARNING(func) func +# define G2O_ATTRIBUTE_DEPRECATED(func) func + +#include +#define g2o_isnan(x) isnan(x) +#define g2o_isinf(x) isinf(x) +#define g2o_isfinite(x) isfinite(x) + +#endif + +// some macros that are only useful for c++ +#ifdef __cplusplus + +#define G2O_FSKIP_LINE(f) \ + {char c=' ';while(c != '\n' && f.good() && !(f).eof()) (f).get(c);} + +#ifndef PVAR + #define PVAR(s) \ + #s << " = " << (s) << std::flush +#endif + +#ifndef PVARA +#define PVARA(s) \ + #s << " = " << RAD2DEG(s) << "deg" << std::flush +#endif + +#ifndef FIXED +#define FIXED(s) \ + std::fixed << s << std::resetiosflags(std::ios_base::fixed) +#endif + +#endif // __cplusplus + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/misc.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/misc.h new file mode 100644 index 0000000..4fa4ff3 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/misc.h @@ -0,0 +1,206 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_STUFF_MISC_H +#define G2O_STUFF_MISC_H + +#include "macros.h" +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +/** @addtogroup utils **/ +// @{ + +/** \file misc.h + * \brief some general case utility functions + * + * This file specifies some general case utility functions + **/ + +namespace g2o { + +/** + * return the square value + */ +template +inline T square(T x) +{ + return x*x; +} + +/** + * return the hypot of x and y + */ +template +inline T hypot(T x, T y) +{ + return (T) (sqrt(x*x + y*y)); +} + +/** + * return the squared hypot of x and y + */ +template +inline T hypot_sqr(T x, T y) +{ + return x*x + y*y; +} + +/** + * convert from degree to radian + */ +inline double deg2rad(double degree) +{ + return degree * 0.01745329251994329576; +} + +/** + * convert from radian to degree + */ +inline double rad2deg(double rad) +{ + return rad * 57.29577951308232087721; +} + +/** + * normalize the angle + */ +inline double normalize_theta(double theta) +{ + if (theta >= -M_PI && theta < M_PI) + return theta; + + double multiplier = floor(theta / (2*M_PI)); + theta = theta - multiplier*2*M_PI; + if (theta >= M_PI) + theta -= 2*M_PI; + if (theta < -M_PI) + theta += 2*M_PI; + + return theta; +} + +/** + * inverse of an angle, i.e., +180 degree + */ +inline double inverse_theta(double th) +{ + return normalize_theta(th + M_PI); +} + +/** + * average two angles + */ +inline double average_angle(double theta1, double theta2) +{ + double x, y; + + x = cos(theta1) + cos(theta2); + y = sin(theta1) + sin(theta2); + if(x == 0 && y == 0) + return 0; + else + return std::atan2(y, x); +} + +/** + * sign function. + * @return the sign of x. +1 for x > 0, -1 for x < 0, 0 for x == 0 + */ +template +inline int sign(T x) +{ + if (x > 0) + return 1; + else if (x < 0) + return -1; + else + return 0; +} + +/** + * clamp x to the interval [l, u] + */ +template +inline T clamp(T l, T x, T u) +{ + if (x < l) + return l; + if (x > u) + return u; + return x; +} + +/** + * wrap x to be in the interval [l, u] + */ +template +inline T wrap(T l, T x, T u) +{ + T intervalWidth = u - l; + while (x < l) + x += intervalWidth; + while (x > u) + x -= intervalWidth; + return x; +} + +/** + * tests whether there is a NaN in the array + */ +inline bool arrayHasNaN(const double* array, int size, int* nanIndex = 0) +{ + for (int i = 0; i < size; ++i) + if (g2o_isnan(array[i])) { + if (nanIndex) + *nanIndex = i; + return true; + } + return false; +} + +/** + * The following two functions are used to force linkage with static libraries. + */ +extern "C" +{ + typedef void (* ForceLinkFunction) (void); +} + +struct ForceLinker +{ + ForceLinker(ForceLinkFunction function) { (function)(); } +}; + + +} // end namespace + +// @} + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/os_specific.c b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/os_specific.c new file mode 100644 index 0000000..85f9368 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/os_specific.c @@ -0,0 +1,64 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "os_specific.h" + +#ifdef WINDOWS + +int vasprintf(char** strp, const char* fmt, va_list ap) +{ + int n; + int size = 100; + char* p; + char* np; + + if ((p = (char*)malloc(size * sizeof(char))) == NULL) + return -1; + + while (1) { +#ifdef _MSC_VER + n = vsnprintf_s(p, size, size - 1, fmt, ap); +#else + n = vsnprintf(p, size, fmt, ap); +#endif + if (n > -1 && n < size) { + *strp = p; + return n; + } + if (n > -1) + size = n+1; + else + size *= 2; + if ((np = (char*)realloc (p, size * sizeof(char))) == NULL) { + free(p); + return -1; + } else + p = np; + } +} + + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/os_specific.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/os_specific.h new file mode 100644 index 0000000..89dd0cc --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/os_specific.h @@ -0,0 +1,56 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_OS_SPECIFIC_HH_ +#define G2O_OS_SPECIFIC_HH_ + +#ifdef WINDOWS +#include +#include +#include +#ifndef _WINDOWS +#include +#endif +#define drand48() ((double) rand()/(double)RAND_MAX) + +#ifdef __cplusplus +extern "C" { +#endif + +int vasprintf(char** strp, const char* fmt, va_list ap); + +#ifdef __cplusplus +} +#endif + +#endif + +#ifdef UNIX +#include +// nothing to do on real operating systems +#endif + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/property.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/property.cpp new file mode 100644 index 0000000..f13016e --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/property.cpp @@ -0,0 +1,105 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "property.h" + +#include +#include + +#include "macros.h" + +#include "string_tools.h" +using namespace std; + +namespace g2o { + + BaseProperty::BaseProperty(const std::string name_) :_name(name_){ + } + + BaseProperty::~BaseProperty(){} + + bool PropertyMap::addProperty(BaseProperty* p) { + std::pair result = insert(make_pair(p->name(), p)); + return result.second; + } + + bool PropertyMap::eraseProperty(const std::string& name) { + PropertyMapIterator it=find(name); + if (it==end()) + return false; + delete it->second; + erase(it); + return true; + } + + PropertyMap::~PropertyMap() { + for (PropertyMapIterator it=begin(); it!=end(); it++){ + if (it->second) + delete it->second; + } + } + + bool PropertyMap::updatePropertyFromString(const std::string& name, const std::string& value) + { + PropertyMapIterator it = find(name); + if (it == end()) + return false; + it->second->fromString(value); + return true; + } + + void PropertyMap::writeToCSV(std::ostream& os) const + { + for (PropertyMapConstIterator it=begin(); it!=end(); it++){ + BaseProperty* p =it->second; + os << p->name() << ", "; + } + os << std::endl; + for (PropertyMapConstIterator it=begin(); it!=end(); it++){ + BaseProperty* p =it->second; + os << p->toString() << ", "; + } + os << std::endl; + } + + bool PropertyMap::updateMapFromString(const std::string& values) + { + bool status = true; + vector valuesMap = strSplit(values, ","); + for (size_t i = 0; i < valuesMap.size(); ++i) { + vector m = strSplit(valuesMap[i], "="); + if (m.size() != 2) { + cerr << __PRETTY_FUNCTION__ << ": unable to extract name=value pair from " << valuesMap[i] << endl; + continue; + } + string name = trim(m[0]); + string value = trim(m[1]); + status = status && updatePropertyFromString(name, value); + } + return status; + } + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/property.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/property.h new file mode 100644 index 0000000..7b63878 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/property.h @@ -0,0 +1,158 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_PROPERTY_H_ +#define G2O_PROPERTY_H_ + +#include +#include +#include + +#include "string_tools.h" + +namespace g2o { + + class BaseProperty { + public: + BaseProperty(const std::string name_); + virtual ~BaseProperty(); + const std::string& name() {return _name;} + virtual std::string toString() const = 0; + virtual bool fromString(const std::string& s) = 0; + protected: + std::string _name; + }; + + template + class Property: public BaseProperty { + public: + typedef T ValueType; + Property(const std::string& name_): BaseProperty(name_){} + Property(const std::string& name_, const T& v): BaseProperty(name_), _value(v){} + void setValue(const T& v) {_value = v; } + const T& value() const {return _value;} + virtual std::string toString() const + { + std::stringstream sstr; + sstr << _value; + return sstr.str(); + } + virtual bool fromString(const std::string& s) + { + bool status = convertString(s, _value); + return status; + } + protected: + T _value; + }; + + /** + * \brief a collection of properties mapping from name to the property itself + */ + class PropertyMap : protected std::map + { + public: + typedef std::map BaseClass; + typedef BaseClass::iterator PropertyMapIterator; + typedef BaseClass::const_iterator PropertyMapConstIterator; + + ~PropertyMap(); + + /** + * add a property to the map + */ + bool addProperty(BaseProperty* p); + + /** + * remove a property from the map + */ + bool eraseProperty(const std::string& name_); + + /** + * return a property by its name + */ + template + P* getProperty(const std::string& name_) + { + PropertyMapIterator it=find(name_); + if (it==end()) + return 0; + return dynamic_cast(it->second); + } + template + const P* getProperty(const std::string& name_) const + { + PropertyMapConstIterator it=find(name_); + if (it==end()) + return 0; + return dynamic_cast(it->second); + } + + /** + * create a property and insert it + */ + template + P* makeProperty(const std::string& name_, const typename P::ValueType& v) + { + PropertyMapIterator it=find(name_); + if (it==end()){ + P* p=new P(name_, v); + addProperty(p); + return p; + } else + return dynamic_cast(it->second); + } + + /** + * update a specfic property with a new value + * @return true if the params is stored and update was carried out + */ + bool updatePropertyFromString(const std::string& name, const std::string& value); + + /** + * update the map based on a name=value string, e.g., name1=val1,name2=val2 + * @return true, if it was possible to update all parameters + */ + bool updateMapFromString(const std::string& values); + + void writeToCSV(std::ostream& os) const; + + using BaseClass::size; + using BaseClass::begin; + using BaseClass::end; + using BaseClass::iterator; + using BaseClass::const_iterator; + + }; + + typedef Property IntProperty; + typedef Property BoolProperty; + typedef Property FloatProperty; + typedef Property DoubleProperty; + typedef Property StringProperty; + +} // end namespace +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/string_tools.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/string_tools.cpp new file mode 100644 index 0000000..0a4f60a --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/string_tools.cpp @@ -0,0 +1,185 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "string_tools.h" +#include "os_specific.h" +#include "macros.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#if (defined (UNIX) || defined(CYGWIN)) && !defined(ANDROID) +#include +#endif + +namespace g2o { + +using namespace std; + +std::string trim(const std::string& s) +{ + if(s.length() == 0) + return s; + string::size_type b = s.find_first_not_of(" \t\n"); + string::size_type e = s.find_last_not_of(" \t\n"); + if(b == string::npos) + return ""; + return std::string(s, b, e - b + 1); +} + +std::string trimLeft(const std::string& s) +{ + if(s.length() == 0) + return s; + string::size_type b = s.find_first_not_of(" \t\n"); + string::size_type e = s.length() - 1; + if(b == string::npos) + return ""; + return std::string(s, b, e - b + 1); +} + +std::string trimRight(const std::string& s) +{ + if(s.length() == 0) + return s; + string::size_type b = 0; + string::size_type e = s.find_last_not_of(" \t\n"); + if(b == string::npos) + return ""; + return std::string(s, b, e - b + 1); +} + +std::string strToLower(const std::string& s) +{ + string ret; + std::transform(s.begin(), s.end(), back_inserter(ret), (int(*)(int)) std::tolower); + return ret; +} + +std::string strToUpper(const std::string& s) +{ + string ret; + std::transform(s.begin(), s.end(), back_inserter(ret), (int(*)(int)) std::toupper); + return ret; +} + +std::string formatString(const char* fmt, ...) +{ + char* auxPtr = NULL; + va_list arg_list; + va_start(arg_list, fmt); + int numChar = vasprintf(&auxPtr, fmt, arg_list); + va_end(arg_list); + string retString; + if (numChar != -1) + retString = auxPtr; + else { + cerr << __PRETTY_FUNCTION__ << ": Error while allocating memory" << endl; + } + free(auxPtr); + return retString; +} + +int strPrintf(std::string& str, const char* fmt, ...) +{ + char* auxPtr = NULL; + va_list arg_list; + va_start(arg_list, fmt); + int numChars = vasprintf(&auxPtr, fmt, arg_list); + va_end(arg_list); + str = auxPtr; + free(auxPtr); + return numChars; +} + +std::string strExpandFilename(const std::string& filename) +{ +#if (defined (UNIX) || defined(CYGWIN)) && !defined(ANDROID) + string result = filename; + wordexp_t p; + + wordexp(filename.c_str(), &p, 0); + if(p.we_wordc > 0) { + result = p.we_wordv[0]; + } + wordfree(&p); + return result; +#else + (void) filename; + std::cerr << "WARNING: " << __PRETTY_FUNCTION__ << " not implemented" << std::endl; + return std::string(); +#endif +} + +std::vector strSplit(const std::string& str, const std::string& delimiters) +{ + std::vector tokens; + string::size_type lastPos = 0; + string::size_type pos = 0; + + do { + pos = str.find_first_of(delimiters, lastPos); + tokens.push_back(str.substr(lastPos, pos - lastPos)); + lastPos = pos + 1; + } while (string::npos != pos); + + return tokens; +} + +bool strStartsWith(const std::string& s, const std::string& start) +{ + if (s.size() < start.size()) + return false; + return equal(start.begin(), start.end(), s.begin()); +} + +bool strEndsWith(const std::string& s, const std::string& end) +{ + if (s.size() < end.size()) + return false; + return equal(end.rbegin(), end.rend(), s.rbegin()); +} + +int readLine(std::istream& is, std::stringstream& currentLine) +{ + if (is.eof()) + return -1; + currentLine.str(""); + currentLine.clear(); + is.get(*currentLine.rdbuf()); + if (is.fail()) // fail is set on empty lines + is.clear(); + G2O_FSKIP_LINE(is); // read \n not read by get() + return static_cast(currentLine.str().size()); +} + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/string_tools.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/string_tools.h new file mode 100644 index 0000000..226208f --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/string_tools.h @@ -0,0 +1,176 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_STRING_TOOLS_H +#define G2O_STRING_TOOLS_H + +#include +#include +#include +#include + +#include "macros.h" + +namespace g2o { + +/** @addtogroup utils **/ +// @{ + +/** \file stringTools.h + * \brief utility functions for handling strings + */ + +/** + * remove whitespaces from the start/end of a string + */ + std::string trim(const std::string& s); + +/** + * remove whitespaces from the left side of the string + */ + std::string trimLeft(const std::string& s); + +/** + * remove whitespaced from the right side of the string + */ + std::string trimRight(const std::string& s); + +/** + * convert the string to lower case + */ + std::string strToLower(const std::string& s); + +/** + * convert a string to upper case + */ + std::string strToUpper(const std::string& s); + +/** + * read integer values (seperated by spaces) from a string and store + * them in the given OutputIterator. + */ +template +OutputIterator readInts(const char* str, OutputIterator out) +{ + char* cl = (char*)str; + char* cle = cl; + while (1) { + int id = strtol(cl, &cle, 10); + if (cl == cle) + break; + *out++ = id; + cl = cle; + } + return out; +} + +/** + * read float values (seperated by spaces) from a string and store + * them in the given OutputIterator. + */ +template +OutputIterator readFloats(const char* str, OutputIterator out) +{ + char* cl = (char*)str; + char* cle = cl; + while (1) { + double val = strtod(cl, &cle); + if (cl == cle) + break; + *out++ = val; + cl = cle; + } + return out; +} + +/** + * format a string and return a std::string. + * Format is just like printf, see man 3 printf + */ + std::string formatString(const char* fmt, ...) G2O_ATTRIBUTE_FORMAT12; + +/** + * replacement function for sprintf which fills a std::string instead of a char* + */ + int strPrintf(std::string& str, const char* fmt, ...) G2O_ATTRIBUTE_FORMAT23; + +/** + * convert a string into an other type. + */ +template +bool convertString(const std::string& s, T& x, bool failIfLeftoverChars = true) +{ + std::istringstream i(s); + char c; + if (!(i >> x) || (failIfLeftoverChars && i.get(c))) + return false; + return true; +} + +/** + * convert a string into an other type. + * Return the converted value. Throw error if parsing is wrong. + */ +template +T stringToType(const std::string& s, bool failIfLeftoverChars = true) +{ + T x; + convertString(s, x, failIfLeftoverChars); + return x; +} + +/** + * return true, if str starts with substr + */ + bool strStartsWith(const std::string & str, const std::string& substr); + +/** + * return true, if str ends with substr + */ + bool strEndsWith(const std::string & str, const std::string& substr); + +/** + * expand the given filename like a posix shell, e.g., ~ $CARMEN_HOME and other will get expanded. + * Also command substitution, e.g. `pwd` will give the current directory. + */ + std::string strExpandFilename(const std::string& filename); + +/** + * split a string into token based on the characters given in delim + */ + std::vector strSplit(const std::string& s, const std::string& delim); + +/** + * read a line from is into currentLine. + * @return the number of characters read into currentLine (excluding newline), -1 on eof() + */ + int readLine(std::istream& is, std::stringstream& currentLine); + +// @} + +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/timeutil.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/timeutil.cpp new file mode 100644 index 0000000..ec19051 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/timeutil.cpp @@ -0,0 +1,124 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "timeutil.h" +#include + +#ifdef _WINDOWS +#include +#include +#endif + +#ifdef UNIX +#include +#endif + +namespace g2o { + +#ifdef _WINDOWS +#if defined(_MSC_VER) || defined(_MSC_EXTENSIONS) + #define DELTA_EPOCH_IN_MICROSECS 11644473600000000Ui64 +#else + #define DELTA_EPOCH_IN_MICROSECS 11644473600000000ULL +#endif + +struct timezone +{ + int tz_minuteswest; /* minutes W of Greenwich */ + int tz_dsttime; /* type of dst correction */ +}; + +int gettimeofday(struct timeval *tv, struct timezone *tz) +{ +// Define a structure to receive the current Windows filetime + FILETIME ft; + +// Initialize the present time to 0 and the timezone to UTC + unsigned __int64 tmpres = 0; + static int tzflag = 0; + + if (NULL != tv) + { + GetSystemTimeAsFileTime(&ft); + +// The GetSystemTimeAsFileTime returns the number of 100 nanosecond +// intervals since Jan 1, 1601 in a structure. Copy the high bits to +// the 64 bit tmpres, shift it left by 32 then or in the low 32 bits. + tmpres |= ft.dwHighDateTime; + tmpres <<= 32; + tmpres |= ft.dwLowDateTime; + +// Convert to microseconds by dividing by 10 + tmpres /= 10; + +// The Unix epoch starts on Jan 1 1970. Need to subtract the difference +// in seconds from Jan 1 1601. + tmpres -= DELTA_EPOCH_IN_MICROSECS; + +// Finally change microseconds to seconds and place in the seconds value. +// The modulus picks up the microseconds. + tv->tv_sec = (long)(tmpres / 1000000UL); + tv->tv_usec = (long)(tmpres % 1000000UL); + } + + if (NULL != tz) { + if (!tzflag) { + _tzset(); + tzflag++; + } + + long sec; + int hours; + _get_timezone(&sec); + _get_daylight(&hours); + +// Adjust for the timezone west of Greenwich + tz->tz_minuteswest = sec / 60; + tz->tz_dsttime = hours; + } + + return 0; +} +#endif + +ScopeTime::ScopeTime(const char* title) : _title(title), _startTime(get_monotonic_time()) {} + +ScopeTime::~ScopeTime() { + std::cerr << _title<<" took "<<1000*(get_monotonic_time()-_startTime)<<"ms.\n"; +} + +double get_monotonic_time() +{ +#if (defined(_POSIX_TIMERS) && (_POSIX_TIMERS+0 >= 0) && defined(_POSIX_MONOTONIC_CLOCK)) + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return ts.tv_sec + ts.tv_nsec*1e-9; +#else + return get_time(); +#endif +} + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/timeutil.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/timeutil.h new file mode 100644 index 0000000..bde8e31 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/stuff/timeutil.h @@ -0,0 +1,132 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_TIMEUTIL_H +#define G2O_TIMEUTIL_H + +#ifdef _WINDOWS +#include +#else +#include +#endif + +#include + + +/** @addtogroup utils **/ +// @{ + +/** \file timeutil.h + * \brief utility functions for handling time related stuff + */ + +/// Executes code, only if secs are gone since last exec. +/// extended version, in which the current time is given, e.g., timestamp of IPC message +#ifndef DO_EVERY_TS +#define DO_EVERY_TS(secs, currentTime, code) \ +if (1) {\ + static double s_lastDone_ = (currentTime); \ + double s_now_ = (currentTime); \ + if (s_lastDone_ > s_now_) \ + s_lastDone_ = s_now_; \ + if (s_now_ - s_lastDone_ > (secs)) { \ + code; \ + s_lastDone_ = s_now_; \ + }\ +} else \ + (void)0 +#endif + +/// Executes code, only if secs are gone since last exec. +#ifndef DO_EVERY +#define DO_EVERY(secs, code) DO_EVERY_TS(secs, g2o::get_time(), code) +#endif + +#ifndef MEASURE_TIME +#define MEASURE_TIME(text, code) \ + if(1) { \ + double _start_time_ = g2o::get_time(); \ + code; \ + fprintf(stderr, "%s took %f sec\n", text, g2o::get_time() - _start_time_); \ + } else \ + (void) 0 +#endif + +namespace g2o { + +#ifdef _WINDOWS +typedef struct timeval { + long tv_sec; + long tv_usec; +} timeval; + int gettimeofday(struct timeval *tv, struct timezone *tz); +#endif + +/** + * return the current time in seconds since 1. Jan 1970 + */ +inline double get_time() +{ + struct timeval ts; + gettimeofday(&ts,0); + return ts.tv_sec + ts.tv_usec*1e-6; +} + +/** + * return a monotonic increasing time which basically does not need to + * have a reference point. Consider this for measuring how long some + * code fragments required to execute. + * + * On Linux we call clock_gettime() on other systems we currently + * call get_time(). + */ + double get_monotonic_time(); + +/** + * \brief Class to measure the time spent in a scope + * + * To use this class, e.g. to measure the time spent in a function, + * just create and instance at the beginning of the function. + */ +class ScopeTime { + public: + ScopeTime(const char* title); + ~ScopeTime(); + private: + std::string _title; + double _startTime; +}; + +} // end namespace + +#ifndef MEASURE_FUNCTION_TIME +#define MEASURE_FUNCTION_TIME \ + g2o::ScopeTime scopeTime(__PRETTY_FUNCTION__) +#endif + + +// @} +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/se3_ops.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/se3_ops.h new file mode 100644 index 0000000..b5c59d3 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/se3_ops.h @@ -0,0 +1,47 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 H. Strasdat +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_MATH_STUFF +#define G2O_MATH_STUFF + +#include +#include + +namespace g2o { + using namespace Eigen; + + inline Matrix3d skew(const Vector3d&v); + inline Vector3d deltaR(const Matrix3d& R); + inline Vector2d project(const Vector3d&); + inline Vector3d project(const Vector4d&); + inline Vector3d unproject(const Vector2d&); + inline Vector4d unproject(const Vector3d&); + +#include "se3_ops.hpp" + +} + +#endif //MATH_STUFF diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/se3_ops.hpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/se3_ops.hpp new file mode 100644 index 0000000..c28860d --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/se3_ops.hpp @@ -0,0 +1,85 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 H. Strasdat +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + Matrix3d skew(const Vector3d&v) + { + Matrix3d m; + m.fill(0.); + m(0,1) = -v(2); + m(0,2) = v(1); + m(1,2) = -v(0); + m(1,0) = v(2); + m(2,0) = -v(1); + m(2,1) = v(0); + return m; + } + + Vector3d deltaR(const Matrix3d& R) + { + Vector3d v; + v(0)=R(2,1)-R(1,2); + v(1)=R(0,2)-R(2,0); + v(2)=R(1,0)-R(0,1); + return v; + } + + Vector2d project(const Vector3d& v) + { + Vector2d res; + res(0) = v(0)/v(2); + res(1) = v(1)/v(2); + return res; + } + + Vector3d project(const Vector4d& v) + { + Vector3d res; + res(0) = v(0)/v(3); + res(1) = v(1)/v(3); + res(2) = v(2)/v(3); + return res; + } + + Vector3d unproject(const Vector2d& v) + { + Vector3d res; + res(0) = v(0); + res(1) = v(1); + res(2) = 1; + return res; + } + + Vector4d unproject(const Vector3d& v) + { + Vector4d res; + res(0) = v(0); + res(1) = v(1); + res(2) = v(2); + res(3) = 1; + return res; + } + + diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/se3mat.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/se3mat.cpp new file mode 100644 index 0000000..d9d2c69 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/se3mat.cpp @@ -0,0 +1,39 @@ +#include "se3mat.h" + +namespace g2o { + + +void SE3mat::Retract(const Eigen::Vector3d dr, const Eigen::Vector3d &dt) +{ + t += R*dt; + R = R*ExpSO3(dr); +} + +Eigen::Matrix3d SE3mat::ExpSO3(const Eigen::Vector3d r) +{ + Eigen::Matrix3d W; + W << 0, -r[2], r[1], + r[2], 0, -r[0], + -r[1], r[0], 0; + + const double theta = r.norm(); + + if(theta<1e-6) + return Eigen::Matrix3d::Identity() + W + 0.5l*W*W; + else + return Eigen::Matrix3d::Identity() + W*sin(theta)/theta + W*W*(1-cos(theta))/(theta*theta); +} + +Eigen::Vector3d SE3mat::LogSO3(const Eigen::Matrix3d R) +{ + const double tr = R(0,0)+R(1,1)+R(2,2); + const double theta = acos((tr-1.0l)*0.5l); + Eigen::Vector3d w; + w << R(2,1), R(0,2), R(1,0); + if(theta<1e-6) + return w; + else + return theta*w/sin(theta); +} + +} //namespace g2o diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/se3mat.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/se3mat.h new file mode 100644 index 0000000..e78673c --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/se3mat.h @@ -0,0 +1,47 @@ +#ifndef SE3mat_H +#define SE3mat_H + +#include +#include + +namespace g2o { + +class SE3mat +{ +public: + SE3mat(){ + R = Eigen::Matrix3d::Identity(); + t.setZero(); + } + + SE3mat(const Eigen::Matrix3d &R_, const Eigen::Vector3d &t_):R(R_),t(t_){} + + void Retract(const Eigen::Vector3d dr, const Eigen::Vector3d &dt); + + inline Eigen::Vector3d operator* (const Eigen::Vector3d& v) const { + return R*v + t; + } + + inline SE3mat& operator*= (const SE3mat& T2){ + t+=R*T2.t; + R*=T2.R; + return *this; + } + + inline SE3mat inverse() const{ + Eigen::Matrix3d Rt = R.transpose(); + return SE3mat(Rt,-Rt*t); + } + +protected: + Eigen::Vector3d t; + Eigen::Matrix3d R; + +public: + static Eigen::Matrix3d ExpSO3(const Eigen::Vector3d r); + static Eigen::Vector3d LogSO3(const Eigen::Matrix3d R); +}; + +}//namespace g2o + +#endif // SE3mat_H diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/se3quat.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/se3quat.h new file mode 100644 index 0000000..ac5ec57 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/se3quat.h @@ -0,0 +1,306 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 H. Strasdat +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_SE3QUAT_H_ +#define G2O_SE3QUAT_H_ + +#include "se3_ops.h" + +#include +#include + +namespace g2o { + using namespace Eigen; + + typedef Matrix Vector6d; + typedef Matrix Vector7d; + + class SE3Quat { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + + + protected: + + Quaterniond _r; + Vector3d _t; + + + public: + SE3Quat(){ + _r.setIdentity(); + _t.setZero(); + } + + SE3Quat(const Matrix3d& R, const Vector3d& t):_r(Quaterniond(R)),_t(t){ + normalizeRotation(); + } + + SE3Quat(const Quaterniond& q, const Vector3d& t):_r(q),_t(t){ + normalizeRotation(); + } + + /** + * templaized constructor which allows v to be an arbitrary Eigen Vector type, e.g., Vector6d or Map + */ + template + explicit SE3Quat(const MatrixBase& v) + { + assert((v.size() == 6 || v.size() == 7) && "Vector dimension does not match"); + if (v.size() == 6) { + for (int i=0; i<3; i++){ + _t[i]=v[i]; + _r.coeffs()(i)=v[i+3]; + } + _r.w() = 0.; // recover the positive w + if (_r.norm()>1.){ + _r.normalize(); + } else { + double w2=1.-_r.squaredNorm(); + _r.w()= (w2<0.) ? 0. : sqrt(w2); + } + } + else if (v.size() == 7) { + int idx = 0; + for (int i=0; i<3; ++i, ++idx) + _t(i) = v(idx); + for (int i=0; i<4; ++i, ++idx) + _r.coeffs()(i) = v(idx); + normalizeRotation(); + } + } + + inline const Vector3d& translation() const {return _t;} + + inline void setTranslation(const Vector3d& t_) {_t = t_;} + + inline const Quaterniond& rotation() const {return _r;} + + void setRotation(const Quaterniond& r_) {_r=r_;} + + inline SE3Quat operator* (const SE3Quat& tr2) const{ + SE3Quat result(*this); + result._t += _r*tr2._t; + result._r*=tr2._r; + result.normalizeRotation(); + return result; + } + + inline SE3Quat& operator*= (const SE3Quat& tr2){ + _t+=_r*tr2._t; + _r*=tr2._r; + normalizeRotation(); + return *this; + } + + inline Vector3d operator* (const Vector3d& v) const { + return _t+_r*v; + } + + inline SE3Quat inverse() const{ + SE3Quat ret; + ret._r=_r.conjugate(); + ret._t=ret._r*(_t*-1.); + return ret; + } + + inline double operator [](int i) const { + assert(i<7); + if (i<3) + return _t[i]; + return _r.coeffs()[i-3]; + } + + + inline Vector7d toVector() const{ + Vector7d v; + v[0]=_t(0); + v[1]=_t(1); + v[2]=_t(2); + v[3]=_r.x(); + v[4]=_r.y(); + v[5]=_r.z(); + v[6]=_r.w(); + return v; + } + + inline void fromVector(const Vector7d& v){ + _r=Quaterniond(v[6], v[3], v[4], v[5]); + _t=Vector3d(v[0], v[1], v[2]); + } + + inline Vector6d toMinimalVector() const{ + Vector6d v; + v[0]=_t(0); + v[1]=_t(1); + v[2]=_t(2); + v[3]=_r.x(); + v[4]=_r.y(); + v[5]=_r.z(); + return v; + } + + inline void fromMinimalVector(const Vector6d& v){ + double w = 1.-v[3]*v[3]-v[4]*v[4]-v[5]*v[5]; + if (w>0){ + _r=Quaterniond(sqrt(w), v[3], v[4], v[5]); + } else { + _r=Quaterniond(0, -v[3], -v[4], -v[5]); + } + _t=Vector3d(v[0], v[1], v[2]); + } + + + + Vector6d log() const { + Vector6d res; + Matrix3d _R = _r.toRotationMatrix(); + double d = 0.5*(_R(0,0)+_R(1,1)+_R(2,2)-1); + Vector3d omega; + Vector3d upsilon; + + + Vector3d dR = deltaR(_R); + Matrix3d V_inv; + + if (d>0.99999) + { + + omega=0.5*dR; + Matrix3d Omega = skew(omega); + V_inv = Matrix3d::Identity()- 0.5*Omega + (1./12.)*(Omega*Omega); + } + else + { + double theta = acos(d); + omega = theta/(2*sqrt(1-d*d))*dR; + Matrix3d Omega = skew(omega); + V_inv = ( Matrix3d::Identity() - 0.5*Omega + + ( 1-theta/(2*tan(theta/2)))/(theta*theta)*(Omega*Omega) ); + } + + upsilon = V_inv*_t; + for (int i=0; i<3;i++){ + res[i]=omega[i]; + } + for (int i=0; i<3;i++){ + res[i+3]=upsilon[i]; + } + + return res; + + } + + Vector3d map(const Vector3d & xyz) const + { + return _r*xyz + _t; + } + + + static SE3Quat exp(const Vector6d & update) + { + Vector3d omega; + for (int i=0; i<3; i++) + omega[i]=update[i]; + Vector3d upsilon; + for (int i=0; i<3; i++) + upsilon[i]=update[i+3]; + + double theta = omega.norm(); + Matrix3d Omega = skew(omega); + + Matrix3d R; + Matrix3d V; + if (theta<0.00001) + { + //TODO: CHECK WHETHER THIS IS CORRECT!!! + R = (Matrix3d::Identity() + Omega + Omega*Omega); + + V = R; + } + else + { + Matrix3d Omega2 = Omega*Omega; + + R = (Matrix3d::Identity() + + sin(theta)/theta *Omega + + (1-cos(theta))/(theta*theta)*Omega2); + + V = (Matrix3d::Identity() + + (1-cos(theta))/(theta*theta)*Omega + + (theta-sin(theta))/(pow(theta,3))*Omega2); + } + return SE3Quat(Quaterniond(R),V*upsilon); + } + + Matrix adj() const + { + Matrix3d R = _r.toRotationMatrix(); + Matrix res; + res.block(0,0,3,3) = R; + res.block(3,3,3,3) = R; + res.block(3,0,3,3) = skew(_t)*R; + res.block(0,3,3,3) = Matrix3d::Zero(3,3); + return res; + } + + Matrix to_homogeneous_matrix() const + { + Matrix homogeneous_matrix; + homogeneous_matrix.setIdentity(); + homogeneous_matrix.block(0,0,3,3) = _r.toRotationMatrix(); + homogeneous_matrix.col(3).head(3) = translation(); + + return homogeneous_matrix; + } + + void normalizeRotation(){ + if (_r.w()<0){ + _r.coeffs() *= -1; + } + _r.normalize(); + } + + /** + * cast SE3Quat into an Eigen::Isometry3d + */ + operator Eigen::Isometry3d() const + { + Eigen::Isometry3d result = (Eigen::Isometry3d) rotation(); + result.translation() = translation(); + return result; + } + }; + + inline std::ostream& operator <<(std::ostream& out_str, const SE3Quat& se3) + { + out_str << se3.to_homogeneous_matrix() << std::endl; + return out_str; + } + +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/sim3.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/sim3.h new file mode 100644 index 0000000..cfd6d5e --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/sim3.h @@ -0,0 +1,307 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 H. Strasdat +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_SIM_3 +#define G2O_SIM_3 + +#include "se3_ops.h" +#include + +namespace g2o +{ + using namespace Eigen; + + typedef Matrix Vector7d; + typedef Matrix Matrix7d; + + + struct Sim3 + { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + protected: + Quaterniond r; + Vector3d t; + double s; + + +public: + Sim3() + { + r.setIdentity(); + t.fill(0.); + s=1.; + } + + Sim3(const Quaterniond & r, const Vector3d & t, double s) + : r(r),t(t),s(s) + { + } + + Sim3(const Matrix3d & R, const Vector3d & t, double s) + : r(Quaterniond(R)),t(t),s(s) + { + } + + + Sim3(const Vector7d & update) + { + + Vector3d omega; + for (int i=0; i<3; i++) + omega[i]=update[i]; + + Vector3d upsilon; + for (int i=0; i<3; i++) + upsilon[i]=update[i+3]; + + double sigma = update[6]; + double theta = omega.norm(); + Matrix3d Omega = skew(omega); + s = std::exp(sigma); + Matrix3d Omega2 = Omega*Omega; + Matrix3d I; + I.setIdentity(); + Matrix3d R; + + double eps = 0.00001; + double A,B,C; + if (fabs(sigma)1-eps) + { + omega=0.5*deltaR(R); + Omega = skew(omega); + A = 1./2.; + B = 1./6.; + } + else + { + double theta = acos(d); + double theta2 = theta*theta; + omega = theta/(2*sqrt(1-d*d))*deltaR(R); + Omega = skew(omega); + A = (1-cos(theta))/(theta2); + B = (theta-sin(theta))/(theta2*theta); + } + } + else + { + C=(s-1)/sigma; + if (d>1-eps) + { + + double sigma2 = sigma*sigma; + omega=0.5*deltaR(R); + Omega = skew(omega); + A = ((sigma-1)*s+1)/(sigma2); + B = ((0.5*sigma2-sigma+1)*s)/(sigma2*sigma); + } + else + { + double theta = acos(d); + omega = theta/(2*sqrt(1-d*d))*deltaR(R); + Omega = skew(omega); + double theta2 = theta*theta; + double a=s*sin(theta); + double b=s*cos(theta); + double c=theta2 + sigma*sigma; + A = (a*sigma+ (1-b)*theta)/(theta*c); + B = (C-((b-1)*sigma+a*theta)/(c))*1./(theta2); + } + } + + Matrix3d W = A*Omega + B*Omega*Omega + C*I; + + upsilon = W.lu().solve(t); + + + for (int i=0; i<3; i++) + res[i] = omega[i]; + + for (int i=0; i<3; i++) + res[i+3] = upsilon[i]; + + res[6] = sigma; + + return res; + + } + + + Sim3 inverse() const + { + return Sim3(r.conjugate(), r.conjugate()*((-1./s)*t), 1./s); + } + + + double operator[](int i) const + { + assert(i<8); + if (i<4){ + + return r.coeffs()[i]; + } + if (i<7){ + return t[i-4]; + } + return s; + } + + double& operator[](int i) + { + assert(i<8); + if (i<4){ + + return r.coeffs()[i]; + } + if (i<7) + { + return t[i-4]; + } + return s; + } + + Sim3 operator *(const Sim3& other) const { + Sim3 ret; + ret.r = r*other.r; + ret.t=s*(r*other.t)+t; + ret.s=s*other.s; + return ret; + } + + Sim3& operator *=(const Sim3& other){ + Sim3 ret=(*this)*other; + *this=ret; + return *this; + } + + inline const Vector3d& translation() const {return t;} + + inline Vector3d& translation() {return t;} + + inline const Quaterniond& rotation() const {return r;} + + inline Quaterniond& rotation() {return r;} + + inline const double& scale() const {return s;} + + inline double& scale() {return s;} + + }; + + inline std::ostream& operator <<(std::ostream& out_str, + const Sim3& sim3) + { + out_str << sim3.rotation().coeffs() << std::endl; + out_str << sim3.translation() << std::endl; + out_str << sim3.scale() << std::endl; + + return out_str; + } + +} // end namespace + + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/types_sba.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/types_sba.cpp new file mode 100644 index 0000000..d91b498 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/types_sba.cpp @@ -0,0 +1,56 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 Kurt Konolige +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "types_sba.h" +#include + +namespace g2o { + + using namespace std; + + + VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3d>() + { + } + + bool VertexSBAPointXYZ::read(std::istream& is) + { + Vector3d lv; + for (int i=0; i<3; i++) + is >> _estimate[i]; + return true; + } + + bool VertexSBAPointXYZ::write(std::ostream& os) const + { + Vector3d lv=estimate(); + for (int i=0; i<3; i++){ + os << lv[i] << " "; + } + return os.good(); + } + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/types_sba.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/types_sba.h new file mode 100644 index 0000000..bb419df --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/types_sba.h @@ -0,0 +1,61 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 Kurt Konolige +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_SBA_TYPES +#define G2O_SBA_TYPES + +#include "../core/base_vertex.h" + +#include +#include + +namespace g2o { + +/** + * \brief Point vertex, XYZ + */ + class VertexSBAPointXYZ : public BaseVertex<3, Vector3d> +{ + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + VertexSBAPointXYZ(); + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + + virtual void setToOriginImpl() { + _estimate.fill(0.); + } + + virtual void oplusImpl(const double* update) + { + Eigen::Map v(update); + _estimate += v; + } +}; + +} // end namespace + +#endif // SBA_TYPES diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.cpp new file mode 100644 index 0000000..abf1b1d --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.cpp @@ -0,0 +1,239 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 H. Strasdat +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "types_seven_dof_expmap.h" + +namespace g2o { + + VertexSim3Expmap::VertexSim3Expmap() : BaseVertex<7, Sim3>() + { + _marginalized=false; + _fix_scale = false; + } + + + EdgeSim3::EdgeSim3() : + BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap>() + { + } + + + bool VertexSim3Expmap::read(std::istream& is) + { + Vector7d cam2world; + for (int i=0; i<6; i++){ + is >> cam2world[i]; + } + is >> cam2world[6]; +// if (! is) { +// // if the scale is not specified we set it to 1; +// std::cerr << "!s"; +// cam2world[6]=0.; +// } + + for (int i=0; i<2; i++) + { + is >> _focal_length1[i]; + } + for (int i=0; i<2; i++) + { + is >> _principle_point1[i]; + } + + setEstimate(Sim3(cam2world).inverse()); + return true; + } + + bool VertexSim3Expmap::write(std::ostream& os) const + { + Sim3 cam2world(estimate().inverse()); + Vector7d lv=cam2world.log(); + for (int i=0; i<7; i++){ + os << lv[i] << " "; + } + for (int i=0; i<2; i++) + { + os << _focal_length1[i] << " "; + } + for (int i=0; i<2; i++) + { + os << _principle_point1[i] << " "; + } + return os.good(); + } + + bool EdgeSim3::read(std::istream& is) + { + Vector7d v7; + for (int i=0; i<7; i++){ + is >> v7[i]; + } + + Sim3 cam2world(v7); + setMeasurement(cam2world.inverse()); + + for (int i=0; i<7; i++) + for (int j=i; j<7; j++) + { + is >> information()(i,j); + if (i!=j) + information()(j,i)=information()(i,j); + } + return true; + } + + bool EdgeSim3::write(std::ostream& os) const + { + Sim3 cam2world(measurement().inverse()); + Vector7d v7 = cam2world.log(); + for (int i=0; i<7; i++) + { + os << v7[i] << " "; + } + for (int i=0; i<7; i++) + for (int j=i; j<7; j++){ + os << " " << information()(i,j); + } + return os.good(); + } + + /**Sim3ProjectXYZ*/ + + EdgeSim3ProjectXYZ::EdgeSim3ProjectXYZ() : + BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap>() + { + } + + bool EdgeSim3ProjectXYZ::read(std::istream& is) + { + for (int i=0; i<2; i++) + { + is >> _measurement[i]; + } + + for (int i=0; i<2; i++) + for (int j=i; j<2; j++) { + is >> information()(i,j); + if (i!=j) + information()(j,i)=information()(i,j); + } + return true; + } + + bool EdgeSim3ProjectXYZ::write(std::ostream& os) const + { + for (int i=0; i<2; i++){ + os << _measurement[i] << " "; + } + + for (int i=0; i<2; i++) + for (int j=i; j<2; j++){ + os << " " << information()(i,j); + } + return os.good(); + } + +/**InverseSim3ProjectXYZ*/ + + EdgeInverseSim3ProjectXYZ::EdgeInverseSim3ProjectXYZ() : + BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap>() + { + } + + bool EdgeInverseSim3ProjectXYZ::read(std::istream& is) + { + for (int i=0; i<2; i++) + { + is >> _measurement[i]; + } + + for (int i=0; i<2; i++) + for (int j=i; j<2; j++) { + is >> information()(i,j); + if (i!=j) + information()(j,i)=information()(i,j); + } + return true; + } + + bool EdgeInverseSim3ProjectXYZ::write(std::ostream& os) const + { + for (int i=0; i<2; i++){ + os << _measurement[i] << " "; + } + + for (int i=0; i<2; i++) + for (int j=i; j<2; j++){ + os << " " << information()(i,j); + } + return os.good(); + } + + +// void EdgeSim3ProjectXYZ::linearizeOplus() +// { +// VertexSim3Expmap * vj = static_cast(_vertices[1]); +// Sim3 T = vj->estimate(); + +// VertexPointXYZ* vi = static_cast(_vertices[0]); +// Vector3d xyz = vi->estimate(); +// Vector3d xyz_trans = T.map(xyz); + +// double x = xyz_trans[0]; +// double y = xyz_trans[1]; +// double z = xyz_trans[2]; +// double z_2 = z*z; + +// Matrix tmp; +// tmp(0,0) = _focal_length(0); +// tmp(0,1) = 0; +// tmp(0,2) = -x/z*_focal_length(0); + +// tmp(1,0) = 0; +// tmp(1,1) = _focal_length(1); +// tmp(1,2) = -y/z*_focal_length(1); + +// _jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix(); + +// _jacobianOplusXj(0,0) = x*y/z_2 * _focal_length(0); +// _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *_focal_length(0); +// _jacobianOplusXj(0,2) = y/z *_focal_length(0); +// _jacobianOplusXj(0,3) = -1./z *_focal_length(0); +// _jacobianOplusXj(0,4) = 0; +// _jacobianOplusXj(0,5) = x/z_2 *_focal_length(0); +// _jacobianOplusXj(0,6) = 0; // scale is ignored + + +// _jacobianOplusXj(1,0) = (1+y*y/z_2) *_focal_length(1); +// _jacobianOplusXj(1,1) = -x*y/z_2 *_focal_length(1); +// _jacobianOplusXj(1,2) = -x/z *_focal_length(1); +// _jacobianOplusXj(1,3) = 0; +// _jacobianOplusXj(1,4) = -1./z *_focal_length(1); +// _jacobianOplusXj(1,5) = y/z_2 *_focal_length(1); +// _jacobianOplusXj(1,6) = 0; // scale is ignored +// } + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h new file mode 100644 index 0000000..b63a585 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h @@ -0,0 +1,176 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 H. Strasdat +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +// Modified by Raúl Mur Artal (2014) +// - Added EdgeInverseSim3ProjectXYZ +// - Modified VertexSim3Expmap to represent relative transformation between two cameras. Includes calibration of both cameras. + +#ifndef G2O_SEVEN_DOF_EXPMAP_TYPES +#define G2O_SEVEN_DOF_EXPMAP_TYPES + +#include "../core/base_vertex.h" +#include "../core/base_binary_edge.h" +#include "types_six_dof_expmap.h" +#include "sim3.h" + +namespace g2o { + + using namespace Eigen; + + /** + * \brief Sim3 Vertex, (x,y,z,qw,qx,qy,qz) + * the parameterization for the increments constructed is a 7d vector + * (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion. + */ + class VertexSim3Expmap : public BaseVertex<7, Sim3> + { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + VertexSim3Expmap(); + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + + virtual void setToOriginImpl() { + _estimate = Sim3(); + } + + virtual void oplusImpl(const double* update_) + { + Eigen::Map update(const_cast(update_)); + + if (_fix_scale) + update[6] = 0; + + Sim3 s(update); + setEstimate(s*estimate()); + } + + Vector2d _principle_point1, _principle_point2; + Vector2d _focal_length1, _focal_length2; + + Vector2d cam_map1(const Vector2d & v) const + { + Vector2d res; + res[0] = v[0]*_focal_length1[0] + _principle_point1[0]; + res[1] = v[1]*_focal_length1[1] + _principle_point1[1]; + return res; + } + + Vector2d cam_map2(const Vector2d & v) const + { + Vector2d res; + res[0] = v[0]*_focal_length2[0] + _principle_point2[0]; + res[1] = v[1]*_focal_length2[1] + _principle_point2[1]; + return res; + } + + bool _fix_scale; + + + protected: + }; + + /** + * \brief 7D edge between two Vertex7 + */ + class EdgeSim3 : public BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap> + { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EdgeSim3(); + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + void computeError() + { + const VertexSim3Expmap* v1 = static_cast(_vertices[0]); + const VertexSim3Expmap* v2 = static_cast(_vertices[1]); + + Sim3 C(_measurement); + Sim3 error_=C*v1->estimate()*v2->estimate().inverse(); + _error = error_.log(); + } + + virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& , OptimizableGraph::Vertex* ) { return 1.;} + virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/) + { + VertexSim3Expmap* v1 = static_cast(_vertices[0]); + VertexSim3Expmap* v2 = static_cast(_vertices[1]); + if (from.count(v1) > 0) + v2->setEstimate(measurement()*v1->estimate()); + else + v1->setEstimate(measurement().inverse()*v2->estimate()); + } + }; + + +/**/ +class EdgeSim3ProjectXYZ : public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap> +{ + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EdgeSim3ProjectXYZ(); + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + + void computeError() + { + const VertexSim3Expmap* v1 = static_cast(_vertices[1]); + const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); + + Vector2d obs(_measurement); + _error = obs-v1->cam_map1(project(v1->estimate().map(v2->estimate()))); + } + + // virtual void linearizeOplus(); + +}; + +/**/ +class EdgeInverseSim3ProjectXYZ : public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap> +{ + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EdgeInverseSim3ProjectXYZ(); + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + + void computeError() + { + const VertexSim3Expmap* v1 = static_cast(_vertices[1]); + const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); + + Vector2d obs(_measurement); + _error = obs-v1->cam_map2(project(v1->estimate().inverse().map(v2->estimate()))); + } + + // virtual void linearizeOplus(); + +}; + +} // end namespace + +#endif + diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/types_six_dof_expmap.cpp b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/types_six_dof_expmap.cpp new file mode 100644 index 0000000..b74a1d1 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/types_six_dof_expmap.cpp @@ -0,0 +1,407 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 H. Strasdat +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "types_six_dof_expmap.h" + +#include "../core/factory.h" +#include "../stuff/macros.h" + +namespace g2o { + +using namespace std; + + +Vector2d project2d(const Vector3d& v) { + Vector2d res; + res(0) = v(0)/v(2); + res(1) = v(1)/v(2); + return res; +} + +Vector3d unproject2d(const Vector2d& v) { + Vector3d res; + res(0) = v(0); + res(1) = v(1); + res(2) = 1; + return res; +} + +VertexSE3Expmap::VertexSE3Expmap() : BaseVertex<6, SE3Quat>() { +} + +bool VertexSE3Expmap::read(std::istream& is) { + Vector7d est; + for (int i=0; i<7; i++) + is >> est[i]; + SE3Quat cam2world; + cam2world.fromVector(est); + setEstimate(cam2world.inverse()); + return true; +} + +bool VertexSE3Expmap::write(std::ostream& os) const { + SE3Quat cam2world(estimate().inverse()); + for (int i=0; i<7; i++) + os << cam2world[i] << " "; + return os.good(); +} + + EdgeSE3::EdgeSE3() : + BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>() + { + } + + bool EdgeSE3::read(std::istream& is) + { + Vector6d v6; + for (int i=0; i<6; i++){ + is >> v6[i]; + } + + SE3Quat cam2world(v6); + setMeasurement(cam2world.inverse()); + + for (int i=0; i<6; i++) + for (int j=i; j<6; j++) + { + is >> information()(i,j); + if (i!=j) + information()(j,i)=information()(i,j); + } + return true; + } + + bool EdgeSE3::write(std::ostream& os) const + { + SE3Quat cam2world(measurement().inverse()); + Vector6d v6 = cam2world.log(); + for (int i=0; i<6; i++) + { + os << v6[i] << " "; + } + for (int i=0; i<6; i++) + for (int j=i; j<6; j++){ + os << " " << information()(i,j); + } + return os.good(); + } + + +EdgeSE3ProjectXYZ::EdgeSE3ProjectXYZ() : BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>() { +} + +bool EdgeSE3ProjectXYZ::read(std::istream& is){ + for (int i=0; i<2; i++){ + is >> _measurement[i]; + } + for (int i=0; i<2; i++) + for (int j=i; j<2; j++) { + is >> information()(i,j); + if (i!=j) + information()(j,i)=information()(i,j); + } + return true; +} + +bool EdgeSE3ProjectXYZ::write(std::ostream& os) const { + + for (int i=0; i<2; i++){ + os << measurement()[i] << " "; + } + + for (int i=0; i<2; i++) + for (int j=i; j<2; j++){ + os << " " << information()(i,j); + } + return os.good(); +} + + +void EdgeSE3ProjectXYZ::linearizeOplus() { + VertexSE3Expmap * vj = static_cast(_vertices[1]); + SE3Quat T(vj->estimate()); + VertexSBAPointXYZ* vi = static_cast(_vertices[0]); + Vector3d xyz = vi->estimate(); + Vector3d xyz_trans = T.map(xyz); + + double x = xyz_trans[0]; + double y = xyz_trans[1]; + double z = xyz_trans[2]; + double z_2 = z*z; + + Matrix tmp; + tmp(0,0) = fx; + tmp(0,1) = 0; + tmp(0,2) = -x/z*fx; + + tmp(1,0) = 0; + tmp(1,1) = fy; + tmp(1,2) = -y/z*fy; + + _jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix(); + + _jacobianOplusXj(0,0) = x*y/z_2 *fx; + _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *fx; + _jacobianOplusXj(0,2) = y/z *fx; + _jacobianOplusXj(0,3) = -1./z *fx; + _jacobianOplusXj(0,4) = 0; + _jacobianOplusXj(0,5) = x/z_2 *fx; + + _jacobianOplusXj(1,0) = (1+y*y/z_2) *fy; + _jacobianOplusXj(1,1) = -x*y/z_2 *fy; + _jacobianOplusXj(1,2) = -x/z *fy; + _jacobianOplusXj(1,3) = 0; + _jacobianOplusXj(1,4) = -1./z *fy; + _jacobianOplusXj(1,5) = y/z_2 *fy; +} + +Vector2d EdgeSE3ProjectXYZ::cam_project(const Vector3d & trans_xyz) const{ + Vector2d proj = project2d(trans_xyz); + Vector2d res; + res[0] = proj[0]*fx + cx; + res[1] = proj[1]*fy + cy; + return res; +} + + +Vector3d EdgeStereoSE3ProjectXYZ::cam_project(const Vector3d & trans_xyz, const float &bf) const{ + const float invz = 1.0f/trans_xyz[2]; + Vector3d res; + res[0] = trans_xyz[0]*invz*fx + cx; + res[1] = trans_xyz[1]*invz*fy + cy; + res[2] = res[0] - bf*invz; + return res; +} + +EdgeStereoSE3ProjectXYZ::EdgeStereoSE3ProjectXYZ() : BaseBinaryEdge<3, Vector3d, VertexSBAPointXYZ, VertexSE3Expmap>() { +} + +bool EdgeStereoSE3ProjectXYZ::read(std::istream& is){ + for (int i=0; i<=3; i++){ + is >> _measurement[i]; + } + for (int i=0; i<=2; i++) + for (int j=i; j<=2; j++) { + is >> information()(i,j); + if (i!=j) + information()(j,i)=information()(i,j); + } + return true; +} + +bool EdgeStereoSE3ProjectXYZ::write(std::ostream& os) const { + + for (int i=0; i<=3; i++){ + os << measurement()[i] << " "; + } + + for (int i=0; i<=2; i++) + for (int j=i; j<=2; j++){ + os << " " << information()(i,j); + } + return os.good(); +} + +void EdgeStereoSE3ProjectXYZ::linearizeOplus() { + VertexSE3Expmap * vj = static_cast(_vertices[1]); + SE3Quat T(vj->estimate()); + VertexSBAPointXYZ* vi = static_cast(_vertices[0]); + Vector3d xyz = vi->estimate(); + Vector3d xyz_trans = T.map(xyz); + + const Matrix3d R = T.rotation().toRotationMatrix(); + + double x = xyz_trans[0]; + double y = xyz_trans[1]; + double z = xyz_trans[2]; + double z_2 = z*z; + + _jacobianOplusXi(0,0) = -fx*R(0,0)/z+fx*x*R(2,0)/z_2; + _jacobianOplusXi(0,1) = -fx*R(0,1)/z+fx*x*R(2,1)/z_2; + _jacobianOplusXi(0,2) = -fx*R(0,2)/z+fx*x*R(2,2)/z_2; + + _jacobianOplusXi(1,0) = -fy*R(1,0)/z+fy*y*R(2,0)/z_2; + _jacobianOplusXi(1,1) = -fy*R(1,1)/z+fy*y*R(2,1)/z_2; + _jacobianOplusXi(1,2) = -fy*R(1,2)/z+fy*y*R(2,2)/z_2; + + _jacobianOplusXi(2,0) = _jacobianOplusXi(0,0)-bf*R(2,0)/z_2; + _jacobianOplusXi(2,1) = _jacobianOplusXi(0,1)-bf*R(2,1)/z_2; + _jacobianOplusXi(2,2) = _jacobianOplusXi(0,2)-bf*R(2,2)/z_2; + + _jacobianOplusXj(0,0) = x*y/z_2 *fx; + _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *fx; + _jacobianOplusXj(0,2) = y/z *fx; + _jacobianOplusXj(0,3) = -1./z *fx; + _jacobianOplusXj(0,4) = 0; + _jacobianOplusXj(0,5) = x/z_2 *fx; + + _jacobianOplusXj(1,0) = (1+y*y/z_2) *fy; + _jacobianOplusXj(1,1) = -x*y/z_2 *fy; + _jacobianOplusXj(1,2) = -x/z *fy; + _jacobianOplusXj(1,3) = 0; + _jacobianOplusXj(1,4) = -1./z *fy; + _jacobianOplusXj(1,5) = y/z_2 *fy; + + _jacobianOplusXj(2,0) = _jacobianOplusXj(0,0)-bf*y/z_2; + _jacobianOplusXj(2,1) = _jacobianOplusXj(0,1)+bf*x/z_2; + _jacobianOplusXj(2,2) = _jacobianOplusXj(0,2); + _jacobianOplusXj(2,3) = _jacobianOplusXj(0,3); + _jacobianOplusXj(2,4) = 0; + _jacobianOplusXj(2,5) = _jacobianOplusXj(0,5)-bf/z_2; +} + + +//Only Pose + +bool EdgeSE3ProjectXYZOnlyPose::read(std::istream& is){ + for (int i=0; i<2; i++){ + is >> _measurement[i]; + } + for (int i=0; i<2; i++) + for (int j=i; j<2; j++) { + is >> information()(i,j); + if (i!=j) + information()(j,i)=information()(i,j); + } + return true; +} + +bool EdgeSE3ProjectXYZOnlyPose::write(std::ostream& os) const { + + for (int i=0; i<2; i++){ + os << measurement()[i] << " "; + } + + for (int i=0; i<2; i++) + for (int j=i; j<2; j++){ + os << " " << information()(i,j); + } + return os.good(); +} + + +void EdgeSE3ProjectXYZOnlyPose::linearizeOplus() { + VertexSE3Expmap * vi = static_cast(_vertices[0]); + Vector3d xyz_trans = vi->estimate().map(Xw); + + double x = xyz_trans[0]; + double y = xyz_trans[1]; + double invz = 1.0/xyz_trans[2]; + double invz_2 = invz*invz; + + _jacobianOplusXi(0,0) = x*y*invz_2 *fx; + _jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx; + _jacobianOplusXi(0,2) = y*invz *fx; + _jacobianOplusXi(0,3) = -invz *fx; + _jacobianOplusXi(0,4) = 0; + _jacobianOplusXi(0,5) = x*invz_2 *fx; + + _jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy; + _jacobianOplusXi(1,1) = -x*y*invz_2 *fy; + _jacobianOplusXi(1,2) = -x*invz *fy; + _jacobianOplusXi(1,3) = 0; + _jacobianOplusXi(1,4) = -invz *fy; + _jacobianOplusXi(1,5) = y*invz_2 *fy; +} + +Vector2d EdgeSE3ProjectXYZOnlyPose::cam_project(const Vector3d & trans_xyz) const{ + Vector2d proj = project2d(trans_xyz); + Vector2d res; + res[0] = proj[0]*fx + cx; + res[1] = proj[1]*fy + cy; + return res; +} + + +Vector3d EdgeStereoSE3ProjectXYZOnlyPose::cam_project(const Vector3d & trans_xyz) const{ + const float invz = 1.0f/trans_xyz[2]; + Vector3d res; + res[0] = trans_xyz[0]*invz*fx + cx; + res[1] = trans_xyz[1]*invz*fy + cy; + res[2] = res[0] - bf*invz; + return res; +} + + +bool EdgeStereoSE3ProjectXYZOnlyPose::read(std::istream& is){ + for (int i=0; i<=3; i++){ + is >> _measurement[i]; + } + for (int i=0; i<=2; i++) + for (int j=i; j<=2; j++) { + is >> information()(i,j); + if (i!=j) + information()(j,i)=information()(i,j); + } + return true; +} + +bool EdgeStereoSE3ProjectXYZOnlyPose::write(std::ostream& os) const { + + for (int i=0; i<=3; i++){ + os << measurement()[i] << " "; + } + + for (int i=0; i<=2; i++) + for (int j=i; j<=2; j++){ + os << " " << information()(i,j); + } + return os.good(); +} + +void EdgeStereoSE3ProjectXYZOnlyPose::linearizeOplus() { + VertexSE3Expmap * vi = static_cast(_vertices[0]); + Vector3d xyz_trans = vi->estimate().map(Xw); + + double x = xyz_trans[0]; + double y = xyz_trans[1]; + double invz = 1.0/xyz_trans[2]; + double invz_2 = invz*invz; + + _jacobianOplusXi(0,0) = x*y*invz_2 *fx; + _jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx; + _jacobianOplusXi(0,2) = y*invz *fx; + _jacobianOplusXi(0,3) = -invz *fx; + _jacobianOplusXi(0,4) = 0; + _jacobianOplusXi(0,5) = x*invz_2 *fx; + + _jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy; + _jacobianOplusXi(1,1) = -x*y*invz_2 *fy; + _jacobianOplusXi(1,2) = -x*invz *fy; + _jacobianOplusXi(1,3) = 0; + _jacobianOplusXi(1,4) = -invz *fy; + _jacobianOplusXi(1,5) = y*invz_2 *fy; + + _jacobianOplusXi(2,0) = _jacobianOplusXi(0,0)-bf*y*invz_2; + _jacobianOplusXi(2,1) = _jacobianOplusXi(0,1)+bf*x*invz_2; + _jacobianOplusXi(2,2) = _jacobianOplusXi(0,2); + _jacobianOplusXi(2,3) = _jacobianOplusXi(0,3); + _jacobianOplusXi(2,4) = 0; + _jacobianOplusXi(2,5) = _jacobianOplusXi(0,5)-bf*invz_2; +} + + +} // end namespace diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/types_six_dof_expmap.h b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/types_six_dof_expmap.h new file mode 100644 index 0000000..5d54e06 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/g2o/types/types_six_dof_expmap.h @@ -0,0 +1,242 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 H. Strasdat +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +// Modified by Raúl Mur Artal (2014) +// Added EdgeSE3ProjectXYZ (project using focal_length in x,y directions) +// Modified by Raúl Mur Artal (2016) +// Added EdgeStereoSE3ProjectXYZ (project using focal_length in x,y directions) +// Added EdgeSE3ProjectXYZOnlyPose (unary edge to optimize only the camera pose) +// Added EdgeStereoSE3ProjectXYZOnlyPose (unary edge to optimize only the camera pose) + +#ifndef G2O_SIX_DOF_TYPES_EXPMAP +#define G2O_SIX_DOF_TYPES_EXPMAP + +#include "../core/base_vertex.h" +#include "../core/base_binary_edge.h" +#include "../core/base_unary_edge.h" +#include "se3_ops.h" +#include "se3quat.h" +#include "types_sba.h" +#include + +namespace g2o { +namespace types_six_dof_expmap { +void init(); +} + +using namespace Eigen; + +typedef Matrix Matrix6d; + + +/** + * \brief SE3 Vertex parameterized internally with a transformation matrix + and externally with its exponential map + */ +class VertexSE3Expmap : public BaseVertex<6, SE3Quat>{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + VertexSE3Expmap(); + + bool read(std::istream& is); + + bool write(std::ostream& os) const; + + virtual void setToOriginImpl() { + _estimate = SE3Quat(); + } + + virtual void oplusImpl(const double* update_) { + Eigen::Map update(update_); + setEstimate(SE3Quat::exp(update)*estimate()); + } +}; + + +/** +* \brief 6D edge between two Vertex6 +*/ +class EdgeSE3 : public BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EdgeSE3(); + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + + void computeError() + { + const VertexSE3Expmap* v1 = static_cast(_vertices[0]); + const VertexSE3Expmap* v2 = static_cast(_vertices[1]); + + SE3Quat C(_measurement); + SE3Quat error_=C*v1->estimate()*v2->estimate().inverse(); + _error = error_.log(); + } + + virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& , OptimizableGraph::Vertex* ) { return 1.;} + + virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/) + { + VertexSE3Expmap* v1 = static_cast(_vertices[0]); + VertexSE3Expmap* v2 = static_cast(_vertices[1]); + if (from.count(v1) > 0) + v2->setEstimate(measurement()*v1->estimate()); + else + v1->setEstimate(measurement().inverse()*v2->estimate()); + } +}; + +class EdgeSE3ProjectXYZ: public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EdgeSE3ProjectXYZ(); + + bool read(std::istream& is); + + bool write(std::ostream& os) const; + + void computeError() { + const VertexSE3Expmap* v1 = static_cast(_vertices[1]); + const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); + Vector2d obs(_measurement); + _error = obs-cam_project(v1->estimate().map(v2->estimate())); + } + + bool isDepthPositive() { + const VertexSE3Expmap* v1 = static_cast(_vertices[1]); + const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); + return (v1->estimate().map(v2->estimate()))(2)>0.0; + } + + + virtual void linearizeOplus(); + + Vector2d cam_project(const Vector3d & trans_xyz) const; + + double fx, fy, cx, cy; +}; + + +class EdgeStereoSE3ProjectXYZ: public BaseBinaryEdge<3, Vector3d, VertexSBAPointXYZ, VertexSE3Expmap>{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EdgeStereoSE3ProjectXYZ(); + + bool read(std::istream& is); + + bool write(std::ostream& os) const; + + void computeError() { + const VertexSE3Expmap* v1 = static_cast(_vertices[1]); + const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); + Vector3d obs(_measurement); + _error = obs - cam_project(v1->estimate().map(v2->estimate()),bf); + } + + bool isDepthPositive() { + const VertexSE3Expmap* v1 = static_cast(_vertices[1]); + const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); + return (v1->estimate().map(v2->estimate()))(2)>0.0; + } + + + virtual void linearizeOplus(); + + Vector3d cam_project(const Vector3d & trans_xyz, const float &bf) const; + + double fx, fy, cx, cy, bf; +}; + +class EdgeSE3ProjectXYZOnlyPose: public BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EdgeSE3ProjectXYZOnlyPose(){} + + bool read(std::istream& is); + + bool write(std::ostream& os) const; + + void computeError() { + const VertexSE3Expmap* v1 = static_cast(_vertices[0]); + Vector2d obs(_measurement); + _error = obs-cam_project(v1->estimate().map(Xw)); + } + + bool isDepthPositive() { + const VertexSE3Expmap* v1 = static_cast(_vertices[0]); + return (v1->estimate().map(Xw))(2)>0.0; + } + + + virtual void linearizeOplus(); + + Vector2d cam_project(const Vector3d & trans_xyz) const; + + Vector3d Xw; + double fx, fy, cx, cy; +}; + + +class EdgeStereoSE3ProjectXYZOnlyPose: public BaseUnaryEdge<3, Vector3d, VertexSE3Expmap>{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EdgeStereoSE3ProjectXYZOnlyPose(){} + + bool read(std::istream& is); + + bool write(std::ostream& os) const; + + void computeError() { + const VertexSE3Expmap* v1 = static_cast(_vertices[0]); + Vector3d obs(_measurement); + _error = obs - cam_project(v1->estimate().map(Xw)); + } + + bool isDepthPositive() { + const VertexSE3Expmap* v1 = static_cast(_vertices[0]); + return (v1->estimate().map(Xw))(2)>0.0; + } + + + virtual void linearizeOplus(); + + Vector3d cam_project(const Vector3d & trans_xyz) const; + + Vector3d Xw; + double fx, fy, cx, cy, bf; +}; + + + +} // end namespace + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/license-bsd.txt b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/license-bsd.txt new file mode 100644 index 0000000..b9b8846 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Thirdparty/g2o/license-bsd.txt @@ -0,0 +1,27 @@ +g2o - General Graph Optimization +Copyright (C) 2011 Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat, +Kurt Konolige, and Wolfram Burgard +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are +met: + +* Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. +* Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + diff --git a/ORB-SLAM3-UESTC/Workspace/Vocabulary/ORBvoc.txt.tar.gz b/ORB-SLAM3-UESTC/Workspace/Vocabulary/ORBvoc.txt.tar.gz new file mode 100644 index 0000000..96f5a81 Binary files /dev/null and b/ORB-SLAM3-UESTC/Workspace/Vocabulary/ORBvoc.txt.tar.gz differ diff --git a/ORB-SLAM3-UESTC/Workspace/include/Atlas.h b/ORB-SLAM3-UESTC/Workspace/include/Atlas.h new file mode 100644 index 0000000..4111c71 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/Atlas.h @@ -0,0 +1,174 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#ifndef ATLAS_H +#define ATLAS_H + +#include "Map.h" +#include "MapPoint.h" +#include "KeyFrame.h" +#include "GeometricCamera.h" +#include "Pinhole.h" +#include "KannalaBrandt8.h" + +#include +#include +#include +#include + + +namespace ORB_SLAM3 +{ +class Viewer; +class Map; +class MapPoint; +class KeyFrame; +class KeyFrameDatabase; +class Frame; +class KannalaBrandt8; +class Pinhole; + +//BOOST_CLASS_EXPORT_GUID(Pinhole, "Pinhole") +//BOOST_CLASS_EXPORT_GUID(KannalaBrandt8, "KannalaBrandt8") + +class Atlas +{ + // 1. 对boost声明友元，boost就能调用Atlas的serialize了 + friend class boost::serialization::access; + + // 保存读取都用这个 + template + void serialize(Archive &ar, const unsigned int version) + { + // 由于保存相机是基类，但是实际使用是派生类，所以声明一下 + ar.template register_type(); + ar.template register_type(); + + // Save/load a set structure, the set structure is broken in libboost 1.58 for ubuntu 16.04, a vector is serializated + //ar & mspMaps; + // 基础类型不用管，但是自定义的类里面要进一步写serialize函数，确定保存内容 + ar & mvpBackupMaps; + ar & mvpCameras; + // Need to save/load the static Id from Frame, KeyFrame, MapPoint and Map + ar & Map::nNextId; + ar & Frame::nNextId; + ar & KeyFrame::nNextId; + ar & MapPoint::nNextId; + ar & GeometricCamera::nNextId; + ar & mnLastInitKFidMap; + } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Atlas(); + Atlas(int initKFid); // When its initialization the first map is created + ~Atlas(); + + void CreateNewMap(); + void ChangeMap(Map* pMap); + + unsigned long int GetLastInitKFid(); + + void SetViewer(Viewer* pViewer); + + // Method for change components in the current map + void AddKeyFrame(KeyFrame* pKF); + void AddMapPoint(MapPoint* pMP); + //void EraseMapPoint(MapPoint* pMP); + //void EraseKeyFrame(KeyFrame* pKF); + + GeometricCamera* AddCamera(GeometricCamera* pCam); + std::vector GetAllCameras(); + + /* All methods without Map pointer work on current map */ + void SetReferenceMapPoints(const std::vector &vpMPs); + void InformNewBigChange(); + int GetLastBigChangeIdx(); + + long unsigned int MapPointsInMap(); + long unsigned KeyFramesInMap(); + + // Method for get data in current map + std::vector GetAllKeyFrames(); + std::vector GetAllMapPoints(); + std::vector GetReferenceMapPoints(); + + vector GetAllMaps(); + + int CountMaps(); + + void clearMap(); + + void clearAtlas(); + + Map* GetCurrentMap(); + + void SetMapBad(Map* pMap); + void RemoveBadMaps(); + + bool isInertial(); + void SetInertialSensor(); + void SetImuInitialized(); + bool isImuInitialized(); + + // Function for garantee the correction of serialization of this object + void PreSave(); + void PostLoad(); + + map GetAtlasKeyframes(); + + void SetKeyFrameDababase(KeyFrameDatabase* pKFDB); + KeyFrameDatabase* GetKeyFrameDatabase(); + + void SetORBVocabulary(ORBVocabulary* pORBVoc); + ORBVocabulary* GetORBVocabulary(); + + long unsigned int GetNumLivedKF(); + + long unsigned int GetNumLivedMP(); + +protected: + + std::set mspMaps; + std::set mspBadMaps; + // Its necessary change the container from set to vector because libboost 1.58 and Ubuntu 16.04 have an error with this cointainer + std::vector mvpBackupMaps; + + Map* mpCurrentMap; + + std::vector mvpCameras; + + unsigned long int mnLastInitKFidMap; + + Viewer* mpViewer; + bool mHasViewer; + + // Class references for the map reconstruction from the save file + KeyFrameDatabase* mpKeyFrameDB; + ORBVocabulary* mpORBVocabulary; + + // Mutex + std::mutex mMutexAtlas; + + +}; // class Atlas + +} // namespace ORB_SLAM3 + +#endif // ATLAS_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/CameraModels/GeometricCamera.h b/ORB-SLAM3-UESTC/Workspace/include/CameraModels/GeometricCamera.h new file mode 100644 index 0000000..6a4f5d6 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/CameraModels/GeometricCamera.h @@ -0,0 +1,110 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#ifndef CAMERAMODELS_GEOMETRICCAMERA_H +#define CAMERAMODELS_GEOMETRICCAMERA_H + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include "Converter.h" +#include "GeometricTools.h" + +namespace ORB_SLAM3 { + class GeometricCamera { + + friend class boost::serialization::access; + + template + void serialize(Archive& ar, const unsigned int version) + { + ar & mnId; + ar & mnType; + ar & mvParameters; + } + + + public: + GeometricCamera() {} + GeometricCamera(const std::vector &_vParameters) : mvParameters(_vParameters) {} + ~GeometricCamera() {} + + virtual cv::Point2f project(const cv::Point3f &p3D) = 0; + virtual Eigen::Vector2d project(const Eigen::Vector3d & v3D) = 0; + virtual Eigen::Vector2f project(const Eigen::Vector3f & v3D) = 0; + virtual Eigen::Vector2f projectMat(const cv::Point3f& p3D) = 0; + + virtual float uncertainty2(const Eigen::Matrix &p2D) = 0; + + virtual Eigen::Vector3f unprojectEig(const cv::Point2f &p2D) = 0; + virtual cv::Point3f unproject(const cv::Point2f &p2D) = 0; + + virtual Eigen::Matrix projectJac(const Eigen::Vector3d& v3D) = 0; + + virtual bool ReconstructWithTwoViews(const std::vector& vKeys1, const std::vector& vKeys2, const std::vector &vMatches12, + Sophus::SE3f &T21, std::vector &vP3D, std::vector &vbTriangulated) = 0; + + virtual cv::Mat toK() = 0; + virtual Eigen::Matrix3f toK_() = 0; + + virtual bool epipolarConstrain(GeometricCamera* otherCamera, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12, const Eigen::Vector3f& t12, const float sigmaLevel, const float unc) = 0; + + float getParameter(const int i){return mvParameters[i];} + void setParameter(const float p, const size_t i){mvParameters[i] = p;} + + size_t size(){return mvParameters.size();} + + virtual bool matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, GeometricCamera* pOther, + Sophus::SE3f& Tcw1, Sophus::SE3f& Tcw2, + const float sigmaLevel1, const float sigmaLevel2, + Eigen::Vector3f& x3Dtriangulated) = 0; + + unsigned int GetId() { return mnId; } + + unsigned int GetType() { return mnType; } + + const static unsigned int CAM_PINHOLE = 0; + const static unsigned int CAM_FISHEYE = 1; + + static long unsigned int nNextId; + + protected: + std::vector mvParameters; + + unsigned int mnId; + + unsigned int mnType; + }; +} + + +#endif //CAMERAMODELS_GEOMETRICCAMERA_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/CameraModels/KannalaBrandt8.h b/ORB-SLAM3-UESTC/Workspace/include/CameraModels/KannalaBrandt8.h new file mode 100644 index 0000000..b8fd2ed --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/CameraModels/KannalaBrandt8.h @@ -0,0 +1,115 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#ifndef CAMERAMODELS_KANNALABRANDT8_H +#define CAMERAMODELS_KANNALABRANDT8_H + + +#include + +#include "GeometricCamera.h" + +#include "TwoViewReconstruction.h" + +namespace ORB_SLAM3 { + class KannalaBrandt8 : public GeometricCamera { + + friend class boost::serialization::access; + + template + void serialize(Archive& ar, const unsigned int version) + { + ar & boost::serialization::base_object(*this); + ar & const_cast(precision); + } + + public: + KannalaBrandt8() : precision(1e-6) { + mvParameters.resize(8); + mnId=nNextId++; + mnType = CAM_FISHEYE; + } + KannalaBrandt8(const std::vector _vParameters) : GeometricCamera(_vParameters), precision(1e-6), mvLappingArea(2,0) ,tvr(nullptr) { + assert(mvParameters.size() == 8); + mnId=nNextId++; + mnType = CAM_FISHEYE; + } + + KannalaBrandt8(const std::vector _vParameters, const float _precision) : GeometricCamera(_vParameters), + precision(_precision), mvLappingArea(2,0) { + assert(mvParameters.size() == 8); + mnId=nNextId++; + mnType = CAM_FISHEYE; + } + KannalaBrandt8(KannalaBrandt8* pKannala) : GeometricCamera(pKannala->mvParameters), precision(pKannala->precision), mvLappingArea(2,0) ,tvr(nullptr) { + assert(mvParameters.size() == 8); + mnId=nNextId++; + mnType = CAM_FISHEYE; + } + + cv::Point2f project(const cv::Point3f &p3D); + Eigen::Vector2d project(const Eigen::Vector3d & v3D); + Eigen::Vector2f project(const Eigen::Vector3f & v3D); + Eigen::Vector2f projectMat(const cv::Point3f& p3D); + + float uncertainty2(const Eigen::Matrix &p2D); + + Eigen::Vector3f unprojectEig(const cv::Point2f &p2D); + cv::Point3f unproject(const cv::Point2f &p2D); + + Eigen::Matrix projectJac(const Eigen::Vector3d& v3D); + + + bool ReconstructWithTwoViews(const std::vector& vKeys1, const std::vector& vKeys2, const std::vector &vMatches12, + Sophus::SE3f &T21, std::vector &vP3D, std::vector &vbTriangulated); + + cv::Mat toK(); + Eigen::Matrix3f toK_(); + + bool epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12, const Eigen::Vector3f& t12, const float sigmaLevel, const float unc); + + float TriangulateMatches(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12, const Eigen::Vector3f& t12, const float sigmaLevel, const float unc, Eigen::Vector3f& p3D); + + std::vector mvLappingArea; + + bool matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, GeometricCamera* pOther, + Sophus::SE3f& Tcw1, Sophus::SE3f& Tcw2, + const float sigmaLevel1, const float sigmaLevel2, + Eigen::Vector3f& x3Dtriangulated); + + friend std::ostream& operator<<(std::ostream& os, const KannalaBrandt8& kb); + friend std::istream& operator>>(std::istream& is, KannalaBrandt8& kb); + + float GetPrecision(){ return precision;} + + bool IsEqual(GeometricCamera* pCam); + private: + const float precision; + + //Parameters vector corresponds to + //[fx, fy, cx, cy, k0, k1, k2, k3] + + TwoViewReconstruction* tvr; + + void Triangulate(const cv::Point2f &p1, const cv::Point2f &p2, const Eigen::Matrix &Tcw1, + const Eigen::Matrix &Tcw2, Eigen::Vector3f &x3D); + }; +} + + +#endif //CAMERAMODELS_KANNALABRANDT8_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/CameraModels/Pinhole.h b/ORB-SLAM3-UESTC/Workspace/include/CameraModels/Pinhole.h new file mode 100644 index 0000000..d9b3744 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/CameraModels/Pinhole.h @@ -0,0 +1,101 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#ifndef CAMERAMODELS_PINHOLE_H +#define CAMERAMODELS_PINHOLE_H + +#include + +#include "GeometricCamera.h" + +#include "TwoViewReconstruction.h" + +namespace ORB_SLAM3 { + class Pinhole : public GeometricCamera { + + friend class boost::serialization::access; + + template + void serialize(Archive& ar, const unsigned int version) + { + ar & boost::serialization::base_object(*this); + } + + public: + Pinhole() { + mvParameters.resize(4); + mnId=nNextId++; + mnType = CAM_PINHOLE; + } + Pinhole(const std::vector _vParameters) : GeometricCamera(_vParameters), tvr(nullptr) { + assert(mvParameters.size() == 4); + mnId=nNextId++; + mnType = CAM_PINHOLE; + } + + Pinhole(Pinhole* pPinhole) : GeometricCamera(pPinhole->mvParameters), tvr(nullptr) { + assert(mvParameters.size() == 4); + mnId=nNextId++; + mnType = CAM_PINHOLE; + } + + + ~Pinhole(){ + if(tvr) delete tvr; + } + + cv::Point2f project(const cv::Point3f &p3D); + Eigen::Vector2d project(const Eigen::Vector3d & v3D); + Eigen::Vector2f project(const Eigen::Vector3f & v3D); + Eigen::Vector2f projectMat(const cv::Point3f& p3D); + + float uncertainty2(const Eigen::Matrix &p2D); + + Eigen::Vector3f unprojectEig(const cv::Point2f &p2D); + cv::Point3f unproject(const cv::Point2f &p2D); + + Eigen::Matrix projectJac(const Eigen::Vector3d& v3D); + + + bool ReconstructWithTwoViews(const std::vector& vKeys1, const std::vector& vKeys2, const std::vector &vMatches12, + Sophus::SE3f &T21, std::vector &vP3D, std::vector &vbTriangulated); + + cv::Mat toK(); + Eigen::Matrix3f toK_(); + + bool epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12, const Eigen::Vector3f& t12, const float sigmaLevel, const float unc); + + bool matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, GeometricCamera* pOther, + Sophus::SE3f& Tcw1, Sophus::SE3f& Tcw2, + const float sigmaLevel1, const float sigmaLevel2, + Eigen::Vector3f& x3Dtriangulated) { return false;} + + friend std::ostream& operator<<(std::ostream& os, const Pinhole& ph); + friend std::istream& operator>>(std::istream& os, Pinhole& ph); + + bool IsEqual(GeometricCamera* pCam); + private: + //Parameters vector corresponds to + // [fx, fy, cx, cy] + TwoViewReconstruction* tvr; + }; +} + +//BOOST_CLASS_EXPORT_KEY(ORBSLAM2::Pinhole) + +#endif //CAMERAMODELS_PINHOLE_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/Config.h b/ORB-SLAM3-UESTC/Workspace/include/Config.h new file mode 100644 index 0000000..20f3a0b --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/Config.h @@ -0,0 +1,67 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#ifndef CONFIG_H +#define CONFIG_H + +#include +#include +#include +#include + + +namespace ORB_SLAM3 +{ + +class ViewerConfig +{ + +}; + +class CameraConfig +{ + +}; + +class ORBExtractorConfig +{ + +}; + +class IMUConfig +{ + +}; + +class ConfigParser +{ +public: + bool ParseConfigFile(std::string &strConfigFile); + +private: + + ViewerConfig mViewerConfig; + CameraConfig mCameraConfig; + ORBExtractorConfig mORBConfig; + IMUConfig mIMUConfig; + +}; + +} + +#endif // CONFIG_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/Converter.h b/ORB-SLAM3-UESTC/Workspace/include/Converter.h new file mode 100644 index 0000000..30b051e --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/Converter.h @@ -0,0 +1,81 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef CONVERTER_H +#define CONVERTER_H + +#include + +#include +#include"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" +#include"Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" + +#include "Thirdparty/Sophus/sophus/geometry.hpp" +#include "Thirdparty/Sophus/sophus/sim3.hpp" + +namespace ORB_SLAM3 +{ + +class Converter +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + static std::vector toDescriptorVector(const cv::Mat &Descriptors); + + static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT); + static g2o::SE3Quat toSE3Quat(const Sophus::SE3f &T); + static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3); + + // TODO templetize these functions + static cv::Mat toCvMat(const g2o::SE3Quat &SE3); + static cv::Mat toCvMat(const g2o::Sim3 &Sim3); + static cv::Mat toCvMat(const Eigen::Matrix &m); + static cv::Mat toCvMat(const Eigen::Matrix &m); + static cv::Mat toCvMat(const Eigen::Matrix &m); + static cv::Mat toCvMat(const Eigen::Matrix3d &m); + static cv::Mat toCvMat(const Eigen::Matrix &m); + static cv::Mat toCvMat(const Eigen::Matrix &m); + static cv::Mat toCvMat(const Eigen::Matrix &m); + + static cv::Mat toCvMat(const Eigen::MatrixXf &m); + static cv::Mat toCvMat(const Eigen::MatrixXd &m); + + static cv::Mat toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t); + static cv::Mat tocvSkewMatrix(const cv::Mat &v); + + static Eigen::Matrix toVector3d(const cv::Mat &cvVector); + static Eigen::Matrix toVector3f(const cv::Mat &cvVector); + static Eigen::Matrix toVector3d(const cv::Point3f &cvPoint); + static Eigen::Matrix toMatrix3d(const cv::Mat &cvMat3); + static Eigen::Matrix toMatrix4d(const cv::Mat &cvMat4); + static Eigen::Matrix toMatrix3f(const cv::Mat &cvMat3); + static Eigen::Matrix toMatrix4f(const cv::Mat &cvMat4); + static std::vector toQuaternion(const cv::Mat &M); + + static bool isRotationMatrix(const cv::Mat &R); + static std::vector toEuler(const cv::Mat &R); + + //TODO: Sophus migration, to be deleted in the future + static Sophus::SE3 toSophus(const cv::Mat& T); + static Sophus::Sim3f toSophus(const g2o::Sim3& S); +}; + +}// namespace ORB_SLAM + +#endif // CONVERTER_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/Frame.h b/ORB-SLAM3-UESTC/Workspace/include/Frame.h new file mode 100644 index 0000000..572073f --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/Frame.h @@ -0,0 +1,374 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef FRAME_H +#define FRAME_H + +#include + +#include "Thirdparty/DBoW2/DBoW2/BowVector.h" +#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" + +#include "Thirdparty/Sophus/sophus/geometry.hpp" + +#include "ImuTypes.h" +#include "ORBVocabulary.h" + +#include "Converter.h" +#include "Settings.h" + +#include +#include + +#include "Eigen/Core" +#include "sophus/se3.hpp" + +namespace ORB_SLAM3 +{ +#define FRAME_GRID_ROWS 48 +#define FRAME_GRID_COLS 64 + +class MapPoint; +class KeyFrame; +class ConstraintPoseImu; +class GeometricCamera; +class ORBextractor; + +class Frame +{ +public: + Frame(); + + // Copy constructor. + Frame(const Frame &frame); + + // Constructor for stereo cameras. + Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera,Frame* pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); + + // Constructor for RGB-D cameras. + Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera,Frame* pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); + + // Constructor for Monocular cameras. + Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, GeometricCamera* pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, Frame* pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); + + // Destructor + // ~Frame(); + + // Extract ORB on the image. 0 for left image and 1 for right image. + void ExtractORB(int flag, const cv::Mat &im, const int x0, const int x1); + + // Compute Bag of Words representation. + void ComputeBoW(); + + // Set the camera pose. (Imu pose is not modified!) + void SetPose(const Sophus::SE3 &Tcw); + + // Set IMU velocity + void SetVelocity(Eigen::Vector3f Vw); + + Eigen::Vector3f GetVelocity() const; + + // Set IMU pose and velocity (implicitly changes camera pose) + void SetImuPoseVelocity(const Eigen::Matrix3f &Rwb, const Eigen::Vector3f &twb, const Eigen::Vector3f &Vwb); + + Eigen::Matrix GetImuPosition() const; + Eigen::Matrix GetImuRotation(); + Sophus::SE3 GetImuPose(); + + Sophus::SE3f GetRelativePoseTrl(); + Sophus::SE3f GetRelativePoseTlr(); + Eigen::Matrix3f GetRelativePoseTlr_rotation(); + Eigen::Vector3f GetRelativePoseTlr_translation(); + + void SetNewBias(const IMU::Bias &b); + + // Check if a MapPoint is in the frustum of the camera + // and fill variables of the MapPoint to be used by the tracking + bool isInFrustum(MapPoint* pMP, float viewingCosLimit); + + bool ProjectPointDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float &v); + + Eigen::Vector3f inRefCoordinates(Eigen::Vector3f pCw); + + // Compute the cell of a keypoint (return false if outside the grid) + bool PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY); + + vector GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel=-1, const int maxLevel=-1, const bool bRight = false) const; + + // Search a match for each keypoint in the left image to a keypoint in the right image. + // If there is a match, depth is computed and the right coordinate associated to the left keypoint is stored. + void ComputeStereoMatches(); + + // Associate a "right" coordinate to a keypoint if there is valid depth in the depthmap. + void ComputeStereoFromRGBD(const cv::Mat &imDepth); + + // Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates. + bool UnprojectStereo(const int &i, Eigen::Vector3f &x3D); + + ConstraintPoseImu* mpcpi; + + bool imuIsPreintegrated(); + void setIntegrated(); + + bool isSet() const; + + // Computes rotation, translation and camera center matrices from the camera pose. + void UpdatePoseMatrices(); + + // Returns the camera center. + inline Eigen::Vector3f GetCameraCenter(){ + return mOw; + } + + // Returns inverse of rotation + inline Eigen::Matrix3f GetRotationInverse(){ + return mRwc; + } + + inline Sophus::SE3 GetPose() const { + //TODO: can the Frame pose be accsessed from several threads? should this be protected somehow? + return mTcw; + } + + inline Eigen::Matrix3f GetRwc() const { + return mRwc; + } + + inline Eigen::Vector3f GetOw() const { + return mOw; + } + + inline bool HasPose() const { + return mbHasPose; + } + + inline bool HasVelocity() const { + return mbHasVelocity; + } + + + +private: + //Sophus/Eigen migration + Sophus::SE3 mTcw; + Eigen::Matrix mRwc; + Eigen::Matrix mOw; + Eigen::Matrix mRcw; + Eigen::Matrix mtcw; + bool mbHasPose; + + //Rcw_ not necessary as Sophus has a method for extracting the rotation matrix: Tcw_.rotationMatrix() + //tcw_ not necessary as Sophus has a method for extracting the translation vector: Tcw_.translation() + //Twc_ not necessary as Sophus has a method for easily computing the inverse pose: Tcw_.inverse() + + Sophus::SE3 mTlr, mTrl; + Eigen::Matrix mRlr; + Eigen::Vector3f mtlr; + + + // IMU linear velocity + Eigen::Vector3f mVw; + bool mbHasVelocity; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + // Vocabulary used for relocalization. + ORBVocabulary* mpORBvocabulary; + + // Feature extractor. The right is used only in the stereo case. + ORBextractor* mpORBextractorLeft, *mpORBextractorRight; + + // Frame timestamp. + double mTimeStamp; + + // Calibration matrix and OpenCV distortion parameters. + cv::Mat mK; + Eigen::Matrix3f mK_; + static float fx; + static float fy; + static float cx; + static float cy; + static float invfx; + static float invfy; + cv::Mat mDistCoef; + + // Stereo baseline multiplied by fx. + float mbf; + + // Stereo baseline in meters. + float mb; + + // Threshold close/far points. Close points are inserted from 1 view. + // Far points are inserted as in the monocular case from 2 views. + float mThDepth; + + // Number of KeyPoints. + int N; + + // Vector of keypoints (original for visualization) and undistorted (actually used by the system). + // In the stereo case, mvKeysUn is redundant as images must be rectified. + // In the RGB-D case, RGB images can be distorted. + std::vector mvKeys, mvKeysRight; + std::vector mvKeysUn; + + // Corresponding stereo coordinate and depth for each keypoint. + std::vector mvpMapPoints; + // "Monocular" keypoints have a negative value. + std::vector mvuRight; + std::vector mvDepth; + + // Bag of Words Vector structures. + DBoW2::BowVector mBowVec; + DBoW2::FeatureVector mFeatVec; + + // ORB descriptor, each row associated to a keypoint. + cv::Mat mDescriptors, mDescriptorsRight; + + // MapPoints associated to keypoints, NULL pointer if no association. + // Flag to identify outlier associations. + std::vector mvbOutlier; + int mnCloseMPs; + + // Keypoints are assigned to cells in a grid to reduce matching complexity when projecting MapPoints. + static float mfGridElementWidthInv; + static float mfGridElementHeightInv; + std::vector mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]; + + IMU::Bias mPredBias; + + // IMU bias + IMU::Bias mImuBias; + + // Imu calibration + IMU::Calib mImuCalib; + + // Imu preintegration from last keyframe + IMU::Preintegrated* mpImuPreintegrated; + KeyFrame* mpLastKeyFrame; + + // Pointer to previous frame + Frame* mpPrevFrame; + IMU::Preintegrated* mpImuPreintegratedFrame; + + // Current and Next Frame id. + static long unsigned int nNextId; + long unsigned int mnId; + + // Reference Keyframe. + KeyFrame* mpReferenceKF; + + // Scale pyramid info. + int mnScaleLevels; + float mfScaleFactor; + float mfLogScaleFactor; + vector mvScaleFactors; + vector mvInvScaleFactors; + vector mvLevelSigma2; + vector mvInvLevelSigma2; + + // Undistorted Image Bounds (computed once). + static float mnMinX; + static float mnMaxX; + static float mnMinY; + static float mnMaxY; + + static bool mbInitialComputations; + + map mmProjectPoints; + map mmMatchedInImage; + + string mNameFile; + + int mnDataset; + +#ifdef REGISTER_TIMES + double mTimeORB_Ext; + double mTimeStereoMatch; +#endif + +private: + + // Undistort keypoints given OpenCV distortion parameters. + // Only for the RGB-D case. Stereo must be already rectified! + // (called in the constructor). + void UndistortKeyPoints(); + + // Computes image bounds for the undistorted image (called in the constructor). + void ComputeImageBounds(const cv::Mat &imLeft); + + // Assign keypoints to the grid for speed up feature matching (called in the constructor). + void AssignFeaturesToGrid(); + + bool mbIsSet; + + bool mbImuPreintegrated; + + std::mutex *mpMutexImu; + +public: + GeometricCamera* mpCamera, *mpCamera2; + + //Number of KeyPoints extracted in the left and right images + int Nleft, Nright; + //Number of Non Lapping Keypoints + int monoLeft, monoRight; + + //For stereo matching + std::vector mvLeftToRightMatch, mvRightToLeftMatch; + + //For stereo fisheye matching + static cv::BFMatcher BFmatcher; + + //Triangulated stereo observations using as reference the left camera. These are + //computed during ComputeStereoFishEyeMatches + std::vector mvStereo3Dpoints; + + //Grid for the right image + std::vector mGridRight[FRAME_GRID_COLS][FRAME_GRID_ROWS]; + + Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, GeometricCamera* pCamera2, Sophus::SE3f& Tlr,Frame* pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); + + //Stereo fisheye + void ComputeStereoFishEyeMatches(); + + bool isInFrustumChecks(MapPoint* pMP, float viewingCosLimit, bool bRight = false); + + Eigen::Vector3f UnprojectStereoFishEye(const int &i); + + cv::Mat imgLeft, imgRight; + + void PrintPointDistribution(){ + int left = 0, right = 0; + int Nlim = (Nleft != -1) ? Nleft : N; + for(int i = 0; i < N; i++){ + if(mvpMapPoints[i] && !mvbOutlier[i]){ + if(i < Nlim) left++; + else right++; + } + } + cout << "Point distribution in Frame: left-> " << left << " --- right-> " << right << endl; + } + + Sophus::SE3 T_test; +}; + +}// namespace ORB_SLAM + +#endif // FRAME_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/FrameDrawer.h b/ORB-SLAM3-UESTC/Workspace/include/FrameDrawer.h new file mode 100644 index 0000000..b499650 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/FrameDrawer.h @@ -0,0 +1,91 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef FRAMEDRAWER_H +#define FRAMEDRAWER_H + +#include "Tracking.h" +#include "MapPoint.h" +#include "Atlas.h" + +#include +#include + +#include +#include + + +namespace ORB_SLAM3 +{ + +class Tracking; +class Viewer; + +class FrameDrawer +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + FrameDrawer(Atlas* pAtlas); + + // Update info from the last processed frame. + void Update(Tracking *pTracker); + + // Draw last processed frame. + cv::Mat DrawFrame(float imageScale=1.f); + cv::Mat DrawRightFrame(float imageScale=1.f); + + bool both; + +protected: + + void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText); + + // Info of the frame to be drawn + cv::Mat mIm, mImRight; + int N; + vector mvCurrentKeys,mvCurrentKeysRight; + vector mvbMap, mvbVO; + bool mbOnlyTracking; + int mnTracked, mnTrackedVO; + vector mvIniKeys; + vector mvIniMatches; + int mState; + std::vector mvCurrentDepth; + float mThDepth; + + Atlas* mpAtlas; + + std::mutex mMutex; + vector > mvTracks; + + Frame mCurrentFrame; + vector mvpLocalMap; + vector mvMatchedKeys; + vector mvpMatchedMPs; + vector mvOutlierKeys; + vector mvpOutlierMPs; + + map mmProjectPoints; + map mmMatchedInImage; + +}; + +} //namespace ORB_SLAM + +#endif // FRAMEDRAWER_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/G2oTypes.h b/ORB-SLAM3-UESTC/Workspace/include/G2oTypes.h new file mode 100644 index 0000000..639bd38 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/G2oTypes.h @@ -0,0 +1,982 @@ +/** + * This file is part of ORB-SLAM3 + * + * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * + * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public + * License as published by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even + * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with ORB-SLAM3. + * If not, see . + */ + +#ifndef G2OTYPES_H +#define G2OTYPES_H + +#include "Thirdparty/g2o/g2o/core/base_vertex.h" +#include "Thirdparty/g2o/g2o/core/base_binary_edge.h" +#include "Thirdparty/g2o/g2o/types/types_sba.h" +#include "Thirdparty/g2o/g2o/core/base_multi_edge.h" +#include "Thirdparty/g2o/g2o/core/base_unary_edge.h" + +#include + +#include +#include +#include + +#include +#include + +#include "Converter.h" +#include + +namespace ORB_SLAM3 +{ + +class KeyFrame; +class Frame; +class GeometricCamera; + +typedef Eigen::Matrix Vector6d; +typedef Eigen::Matrix Vector9d; +typedef Eigen::Matrix Vector12d; +typedef Eigen::Matrix Vector15d; +typedef Eigen::Matrix Matrix12d; +typedef Eigen::Matrix Matrix15d; +typedef Eigen::Matrix Matrix9d; + +Eigen::Matrix3d ExpSO3(const double x, const double y, const double z); +Eigen::Matrix3d ExpSO3(const Eigen::Vector3d &w); + +Eigen::Vector3d LogSO3(const Eigen::Matrix3d &R); + +Eigen::Matrix3d InverseRightJacobianSO3(const Eigen::Vector3d &v); +Eigen::Matrix3d RightJacobianSO3(const Eigen::Vector3d &v); +Eigen::Matrix3d RightJacobianSO3(const double x, const double y, const double z); + +Eigen::Matrix3d Skew(const Eigen::Vector3d &w); +Eigen::Matrix3d InverseRightJacobianSO3(const double x, const double y, const double z); + +template +Eigen::Matrix NormalizeRotation(const Eigen::Matrix &R) +{ + Eigen::JacobiSVD> svd(R, Eigen::ComputeFullU | Eigen::ComputeFullV); + return svd.matrixU() * svd.matrixV().transpose(); +} + +// 相关节点中使用，存放的是imu与cam的内外参 +class ImuCamPose +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + ImuCamPose() {} + ImuCamPose(KeyFrame *pKF); + ImuCamPose(Frame *pF); + ImuCamPose(Eigen::Matrix3d &_Rwc, Eigen::Vector3d &_twc, KeyFrame *pKF); + + void SetParam(const std::vector &_Rcw, const std::vector &_tcw, const std::vector &_Rbc, + const std::vector &_tbc, const double &_bf); + + void Update(const double *pu); // update in the imu reference + void UpdateW(const double *pu); // update in the world reference + Eigen::Vector2d Project(const Eigen::Vector3d &Xw, int cam_idx = 0) const; // Mono + Eigen::Vector3d ProjectStereo(const Eigen::Vector3d &Xw, int cam_idx = 0) const; // Stereo + bool isDepthPositive(const Eigen::Vector3d &Xw, int cam_idx = 0) const; + +public: + // For IMU + Eigen::Matrix3d Rwb; + Eigen::Vector3d twb; + + // For set of cameras + std::vector Rcw; + std::vector tcw; + std::vector Rcb, Rbc; + std::vector tcb, tbc; + double bf; + std::vector pCamera; + + // For posegraph 4DoF + Eigen::Matrix3d Rwb0; + Eigen::Matrix3d DR; + + int its; // 记录更新次数 +}; + +// 逆深度点，后面节点虽然有用到，但是声明的节点并没有使用到，暂时不看 +class InvDepthPoint +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + InvDepthPoint() {} + InvDepthPoint(double _rho, double _u, double _v, KeyFrame *pHostKF); + + void Update(const double *pu); + + double rho; + double u, v; // they are not variables, observation in the host frame + + double fx, fy, cx, cy, bf; // from host frame + + int its; +}; + +// Optimizable parameters are IMU pose +// 优化中关于位姿的节点，6自由度 +class VertexPose : public g2o::BaseVertex<6, ImuCamPose> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + VertexPose() {} + VertexPose(KeyFrame *pKF) + { + setEstimate(ImuCamPose(pKF)); + } + VertexPose(Frame *pF) + { + setEstimate(ImuCamPose(pF)); + } + + virtual bool read(std::istream &is); + virtual bool write(std::ostream &os) const; + + // 重置函数,设定被优化变量的原始值 + virtual void setToOriginImpl() + { + } + + virtual void oplusImpl(const double *update_) + { + // https://github.com/RainerKuemmerle/g2o/blob/master/doc/README_IF_IT_WAS_WORKING_AND_IT_DOES_NOT.txt + // 官方讲解cache + // 需要在oplusImpl与setEstimate函数中添加 + _estimate.Update(update_); + updateCache(); + } +}; + +// 优化中关于位姿的节点，4自由度 3个平移加一个航偏 +class VertexPose4DoF : public g2o::BaseVertex<4, ImuCamPose> +{ + // Translation and yaw are the only optimizable variables +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + VertexPose4DoF() {} + VertexPose4DoF(KeyFrame *pKF) + { + setEstimate(ImuCamPose(pKF)); + } + VertexPose4DoF(Frame *pF) + { + setEstimate(ImuCamPose(pF)); + } + VertexPose4DoF(Eigen::Matrix3d &_Rwc, Eigen::Vector3d &_twc, KeyFrame *pKF) + { + + setEstimate(ImuCamPose(_Rwc, _twc, pKF)); + } + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + virtual void setToOriginImpl() + { + } + + // 强制让旋转的前两维的更新量为0 + virtual void oplusImpl(const double *update_) + { + double update6DoF[6]; + update6DoF[0] = 0; + update6DoF[1] = 0; + update6DoF[2] = update_[0]; + update6DoF[3] = update_[1]; + update6DoF[4] = update_[2]; + update6DoF[5] = update_[3]; + _estimate.UpdateW(update6DoF); + updateCache(); + } +}; + +/** + * @brief 速度节点 + */ +class VertexVelocity : public g2o::BaseVertex<3, Eigen::Vector3d> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + VertexVelocity() {} + VertexVelocity(KeyFrame *pKF); + VertexVelocity(Frame *pF); + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + virtual void setToOriginImpl() + { + } + + virtual void oplusImpl(const double *update_) + { + Eigen::Vector3d uv; + uv << update_[0], update_[1], update_[2]; + setEstimate(estimate() + uv); + } +}; + +/** + * @brief 陀螺仪偏置节点 + */ +class VertexGyroBias : public g2o::BaseVertex<3, Eigen::Vector3d> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + VertexGyroBias() {} + VertexGyroBias(KeyFrame *pKF); + VertexGyroBias(Frame *pF); + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + virtual void setToOriginImpl() + { + } + + virtual void oplusImpl(const double *update_) + { + Eigen::Vector3d ubg; + ubg << update_[0], update_[1], update_[2]; + setEstimate(estimate() + ubg); + } +}; + +/** + * @brief 加速度计偏置节点 + */ +class VertexAccBias : public g2o::BaseVertex<3, Eigen::Vector3d> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + VertexAccBias() {} + VertexAccBias(KeyFrame *pKF); + VertexAccBias(Frame *pF); + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + virtual void setToOriginImpl() + { + } + + virtual void oplusImpl(const double *update_) + { + Eigen::Vector3d uba; + uba << update_[0], update_[1], update_[2]; + setEstimate(estimate() + uba); + } +}; + +// Gravity direction vertex +// 重力方向 +class GDirection +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GDirection() {} + GDirection(Eigen::Matrix3d pRwg) : Rwg(pRwg) {} + + void Update(const double *pu) + { + // 强行优化不可观的数据，会导致不收敛 + Rwg = Rwg * ExpSO3(pu[0], pu[1], 0.0); + } + + Eigen::Matrix3d Rwg, Rgw; + + int its; +}; + +/** + * @brief 重力方向节点 + */ +class VertexGDir : public g2o::BaseVertex<2, GDirection> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + VertexGDir() {} + VertexGDir(Eigen::Matrix3d pRwg) + { + setEstimate(GDirection(pRwg)); + } + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + virtual void setToOriginImpl() + { + } + + virtual void oplusImpl(const double *update_) + { + _estimate.Update(update_); + updateCache(); + } +}; + +// scale vertex +/** + * @brief 尺度节点 + */ +class VertexScale : public g2o::BaseVertex<1, double> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + VertexScale() + { + setEstimate(1.0); + } + VertexScale(double ps) + { + setEstimate(ps); + } + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + virtual void setToOriginImpl() + { + setEstimate(1.0); + } + + virtual void oplusImpl(const double *update_) + { + setEstimate(estimate() * exp(*update_)); + } +}; + +// Inverse depth point (just one parameter, inverse depth at the host frame) +/** + * @brief 没用 + */ +class VertexInvDepth : public g2o::BaseVertex<1, InvDepthPoint> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + VertexInvDepth() {} + VertexInvDepth(double invDepth, double u, double v, KeyFrame *pHostKF) + { + setEstimate(InvDepthPoint(invDepth, u, v, pHostKF)); + } + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + virtual void setToOriginImpl() + { + } + + virtual void oplusImpl(const double *update_) + { + _estimate.Update(update_); + updateCache(); + } +}; + +/** + * @brief 单目视觉重投影的边 + * 这里或许会有疑问，OptimizableTypes.h 里面不是定义了视觉重投影的边么？ + * 原因是这样，那里面定义的边也用，不过只是在纯视觉时，没有imu情况下，因为用已经定义好的节点就好 + * 但是加入imu时，优化要有残差的边与重投影的边同时存在，且两个边可能连接同一个位姿节点，所以需要重新弄一个包含imu位姿的节点 + * 因此，边也需要重新写，并且在imu优化时使用这个边 + */ +// 误差为2维， 类型为Eigen::Vector2d， 节点1类型为g2o::VertexSBAPointXYZ，节点二类型为VertexPose +class EdgeMono : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, VertexPose> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EdgeMono(int cam_idx_ = 0) : cam_idx(cam_idx_) + { + } + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + // 计算重投影误差 + void computeError() + { + const g2o::VertexSBAPointXYZ *VPoint = static_cast(_vertices[0]); + const VertexPose *VPose = static_cast(_vertices[1]); + const Eigen::Vector2d obs(_measurement); + _error = obs - VPose->estimate().Project(VPoint->estimate(), cam_idx); + } + + virtual void linearizeOplus(); + + bool isDepthPositive() + { + const g2o::VertexSBAPointXYZ *VPoint = static_cast(_vertices[0]); + const VertexPose *VPose = static_cast(_vertices[1]); + return VPose->estimate().isDepthPositive(VPoint->estimate(), cam_idx); + } + + Eigen::Matrix GetJacobian() + { + linearizeOplus(); + Eigen::Matrix J; + J.block<2, 3>(0, 0) = _jacobianOplusXi; + J.block<2, 6>(0, 3) = _jacobianOplusXj; + return J; + } + + // 由2*2的像素点信息矩阵变成了9*9的关于旋转平移与三维点坐标的信息矩阵 + // 可以理解为像素的不确定性给旋转平移跟三维点带来了多大的不确定性 + Eigen::Matrix GetHessian() + { + linearizeOplus(); + Eigen::Matrix J; + J.block<2, 3>(0, 0) = _jacobianOplusXi; + J.block<2, 6>(0, 3) = _jacobianOplusXj; + return J.transpose() * information() * J; + } + +public: + const int cam_idx; +}; + +/** + * @brief 单目纯位姿一元边 + */ +class EdgeMonoOnlyPose : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EdgeMonoOnlyPose(const Eigen::Vector3f &Xw_, int cam_idx_ = 0) : Xw(Xw_.cast()), + cam_idx(cam_idx_) {} + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + void computeError() + { + const VertexPose *VPose = static_cast(_vertices[0]); + const Eigen::Vector2d obs(_measurement); + _error = obs - VPose->estimate().Project(Xw, cam_idx); + } + + virtual void linearizeOplus(); + + bool isDepthPositive() + { + const VertexPose *VPose = static_cast(_vertices[0]); + return VPose->estimate().isDepthPositive(Xw, cam_idx); + } + + Eigen::Matrix GetHessian() + { + linearizeOplus(); + return _jacobianOplusXi.transpose() * information() * _jacobianOplusXi; + } + +public: + const Eigen::Vector3d Xw; + const int cam_idx; +}; + +/** + * @brief 双目位姿三维点二元边 + */ +class EdgeStereo : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, g2o::VertexSBAPointXYZ, VertexPose> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + // 代码中都是0 + EdgeStereo(int cam_idx_ = 0) : cam_idx(cam_idx_) {} + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + void computeError() + { + const g2o::VertexSBAPointXYZ *VPoint = static_cast(_vertices[0]); + const VertexPose *VPose = static_cast(_vertices[1]); + const Eigen::Vector3d obs(_measurement); + _error = obs - VPose->estimate().ProjectStereo(VPoint->estimate(), cam_idx); + } + + virtual void linearizeOplus(); + + Eigen::Matrix GetJacobian() + { + linearizeOplus(); + Eigen::Matrix J; + J.block<3, 3>(0, 0) = _jacobianOplusXi; + J.block<3, 6>(0, 3) = _jacobianOplusXj; + return J; + } + + Eigen::Matrix GetHessian() + { + linearizeOplus(); + Eigen::Matrix J; + J.block<3, 3>(0, 0) = _jacobianOplusXi; + J.block<3, 6>(0, 3) = _jacobianOplusXj; + return J.transpose() * information() * J; + } + +public: + const int cam_idx; +}; + +/** + * @brief 双目纯位姿一元边 + */ +class EdgeStereoOnlyPose : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPose> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EdgeStereoOnlyPose(const Eigen::Vector3f &Xw_, int cam_idx_ = 0) : Xw(Xw_.cast()), cam_idx(cam_idx_) {} + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + void computeError() + { + const VertexPose *VPose = static_cast(_vertices[0]); + const Eigen::Vector3d obs(_measurement); + _error = obs - VPose->estimate().ProjectStereo(Xw, cam_idx); + } + + virtual void linearizeOplus(); + + Eigen::Matrix GetHessian() + { + linearizeOplus(); + return _jacobianOplusXi.transpose() * information() * _jacobianOplusXi; + } + +public: + const Eigen::Vector3d Xw; // 3D point coordinates + const int cam_idx; +}; + +/** + * @brief 惯性边（误差为残差） + */ +class EdgeInertial : public g2o::BaseMultiEdge<9, Vector9d> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EdgeInertial(IMU::Preintegrated *pInt); + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + void computeError(); + virtual void linearizeOplus(); + + // 关于pose1与2 的旋转平移速度，以及之间的偏置的信息矩阵 + Eigen::Matrix GetHessian() + { + linearizeOplus(); + Eigen::Matrix J; + J.block<9, 6>(0, 0) = _jacobianOplus[0]; + J.block<9, 3>(0, 6) = _jacobianOplus[1]; + J.block<9, 3>(0, 9) = _jacobianOplus[2]; + J.block<9, 3>(0, 12) = _jacobianOplus[3]; + J.block<9, 6>(0, 15) = _jacobianOplus[4]; + J.block<9, 3>(0, 21) = _jacobianOplus[5]; + return J.transpose() * information() * J; + } + + // 没用 + Eigen::Matrix GetHessianNoPose1() + { + linearizeOplus(); + Eigen::Matrix J; + J.block<9, 3>(0, 0) = _jacobianOplus[1]; + J.block<9, 3>(0, 3) = _jacobianOplus[2]; + J.block<9, 3>(0, 6) = _jacobianOplus[3]; + J.block<9, 6>(0, 9) = _jacobianOplus[4]; + J.block<9, 3>(0, 15) = _jacobianOplus[5]; + return J.transpose() * information() * J; + } + + // 关于pose2 的旋转平移信息矩阵 + Eigen::Matrix GetHessian2() + { + linearizeOplus(); + Eigen::Matrix J; + J.block<9, 6>(0, 0) = _jacobianOplus[4]; + J.block<9, 3>(0, 6) = _jacobianOplus[5]; + return J.transpose() * information() * J; + } + + // 预积分中对应的状态对偏置的雅可比 + const Eigen::Matrix3d JRg, JVg, JPg; + const Eigen::Matrix3d JVa, JPa; + + IMU::Preintegrated *mpInt; // 预积分 + const double dt; // 预积分时间 + Eigen::Vector3d g; // 0, 0, -IMU::GRAVITY_VALUE +}; + +// Edge inertial whre gravity is included as optimizable variable and it is not supposed to be pointing in -z axis, as well as scale +/** + * @brief 初始化惯性边（误差为残差） + */ +class EdgeInertialGS : public g2o::BaseMultiEdge<9, Vector9d> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + // EdgeInertialGS(IMU::Preintegrated* pInt); + EdgeInertialGS(IMU::Preintegrated *pInt); + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + void computeError(); + virtual void linearizeOplus(); + + const Eigen::Matrix3d JRg, JVg, JPg; + const Eigen::Matrix3d JVa, JPa; + IMU::Preintegrated *mpInt; + const double dt; + Eigen::Vector3d g, gI; + + // 关于pose1与2 的旋转平移速度，以及之间的偏置，还有重力方向与尺度的信息矩阵 + Eigen::Matrix GetHessian() + { + linearizeOplus(); + Eigen::Matrix J; + J.block<9, 6>(0, 0) = _jacobianOplus[0]; + J.block<9, 3>(0, 6) = _jacobianOplus[1]; + J.block<9, 3>(0, 9) = _jacobianOplus[2]; + J.block<9, 3>(0, 12) = _jacobianOplus[3]; + J.block<9, 6>(0, 15) = _jacobianOplus[4]; + J.block<9, 3>(0, 21) = _jacobianOplus[5]; + J.block<9, 2>(0, 24) = _jacobianOplus[6]; + J.block<9, 1>(0, 26) = _jacobianOplus[7]; + return J.transpose() * information() * J; + } + + // 与上面摆放不同 + Eigen::Matrix GetHessian2() + { + linearizeOplus(); + Eigen::Matrix J; + J.block<9, 3>(0, 0) = _jacobianOplus[2]; + J.block<9, 3>(0, 3) = _jacobianOplus[3]; + J.block<9, 2>(0, 6) = _jacobianOplus[6]; + J.block<9, 1>(0, 8) = _jacobianOplus[7]; + J.block<9, 3>(0, 9) = _jacobianOplus[1]; + J.block<9, 3>(0, 12) = _jacobianOplus[5]; + J.block<9, 6>(0, 15) = _jacobianOplus[0]; + J.block<9, 6>(0, 21) = _jacobianOplus[4]; + return J.transpose() * information() * J; + } + + // 关于偏置，重力方向与尺度的信息矩阵 + Eigen::Matrix GetHessian3() + { + linearizeOplus(); + Eigen::Matrix J; + J.block<9, 3>(0, 0) = _jacobianOplus[2]; + J.block<9, 3>(0, 3) = _jacobianOplus[3]; + J.block<9, 2>(0, 6) = _jacobianOplus[6]; + J.block<9, 1>(0, 8) = _jacobianOplus[7]; + return J.transpose() * information() * J; + } + + // 下面的没有用到，其实也是获取状态的信息矩阵 + Eigen::Matrix GetHessianScale() + { + linearizeOplus(); + Eigen::Matrix J = _jacobianOplus[7]; + return J.transpose() * information() * J; + } + + Eigen::Matrix GetHessianBiasGyro() + { + linearizeOplus(); + Eigen::Matrix J = _jacobianOplus[2]; + return J.transpose() * information() * J; + } + + Eigen::Matrix GetHessianBiasAcc() + { + linearizeOplus(); + Eigen::Matrix J = _jacobianOplus[3]; + return J.transpose() * information() * J; + } + + Eigen::Matrix GetHessianGDir() + { + linearizeOplus(); + Eigen::Matrix J = _jacobianOplus[6]; + return J.transpose() * information() * J; + } +}; + +/** + * @brief 陀螺仪偏置的二元边，除了残差及重投影误差外的第三个边，控制偏置变化 + */ +class EdgeGyroRW : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, VertexGyroBias, VertexGyroBias> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EdgeGyroRW() {} + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + void computeError() + { + const VertexGyroBias *VG1 = static_cast(_vertices[0]); + const VertexGyroBias *VG2 = static_cast(_vertices[1]); + _error = VG2->estimate() - VG1->estimate(); + } + + virtual void linearizeOplus() + { + _jacobianOplusXi = -Eigen::Matrix3d::Identity(); + _jacobianOplusXj.setIdentity(); + } + + Eigen::Matrix GetHessian() + { + linearizeOplus(); + Eigen::Matrix J; + J.block<3, 3>(0, 0) = _jacobianOplusXi; + J.block<3, 3>(0, 3) = _jacobianOplusXj; + return J.transpose() * information() * J; + } + + Eigen::Matrix3d GetHessian2() + { + linearizeOplus(); + return _jacobianOplusXj.transpose() * information() * _jacobianOplusXj; + } +}; + +/** + * @brief 加速度计偏置的二元边，除了残差及重投影误差外的第三个边，控制偏置变化 + */ +class EdgeAccRW : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, VertexAccBias, VertexAccBias> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EdgeAccRW() {} + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + void computeError() + { + const VertexAccBias *VA1 = static_cast(_vertices[0]); + const VertexAccBias *VA2 = static_cast(_vertices[1]); + _error = VA2->estimate() - VA1->estimate(); + } + + virtual void linearizeOplus() + { + _jacobianOplusXi = -Eigen::Matrix3d::Identity(); + _jacobianOplusXj.setIdentity(); + } + + Eigen::Matrix GetHessian() + { + linearizeOplus(); + Eigen::Matrix J; + J.block<3, 3>(0, 0) = _jacobianOplusXi; + J.block<3, 3>(0, 3) = _jacobianOplusXj; + return J.transpose() * information() * J; + } + + Eigen::Matrix3d GetHessian2() + { + linearizeOplus(); + return _jacobianOplusXj.transpose() * information() * _jacobianOplusXj; + } +}; + +/** + * @brief 先验类 + */ +class ConstraintPoseImu +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ConstraintPoseImu( + const Eigen::Matrix3d &Rwb_, const Eigen::Vector3d &twb_, const Eigen::Vector3d &vwb_, + const Eigen::Vector3d &bg_, const Eigen::Vector3d &ba_, const Matrix15d &H_) + : Rwb(Rwb_), twb(twb_), vwb(vwb_), bg(bg_), ba(ba_), H(H_) + { + H = (H + H) / 2; // 对称化 + // 令特征值小的位置变为0 + Eigen::SelfAdjointEigenSolver> es(H); + Eigen::Matrix eigs = es.eigenvalues(); + for (int i = 0; i < 15; i++) + if (eigs[i] < 1e-12) + eigs[i] = 0; + H = es.eigenvectors() * eigs.asDiagonal() * es.eigenvectors().transpose(); + } + + Eigen::Matrix3d Rwb; + Eigen::Vector3d twb; + Eigen::Vector3d vwb; + Eigen::Vector3d bg; + Eigen::Vector3d ba; + Matrix15d H; +}; + +/** + * @brief 先验边，前端优化单帧用到 + */ +class EdgePriorPoseImu : public g2o::BaseMultiEdge<15, Vector15d> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EdgePriorPoseImu(ConstraintPoseImu *c); + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + void computeError(); + virtual void linearizeOplus(); + + Eigen::Matrix GetHessian() + { + linearizeOplus(); + Eigen::Matrix J; + J.block<15, 6>(0, 0) = _jacobianOplus[0]; + J.block<15, 3>(0, 6) = _jacobianOplus[1]; + J.block<15, 3>(0, 9) = _jacobianOplus[2]; + J.block<15, 3>(0, 12) = _jacobianOplus[3]; + return J.transpose() * information() * J; + } + + Eigen::Matrix GetHessianNoPose() + { + linearizeOplus(); + Eigen::Matrix J; + J.block<15, 3>(0, 0) = _jacobianOplus[1]; + J.block<15, 3>(0, 3) = _jacobianOplus[2]; + J.block<15, 3>(0, 6) = _jacobianOplus[3]; + return J.transpose() * information() * J; + } + Eigen::Matrix3d Rwb; + Eigen::Vector3d twb, vwb; + Eigen::Vector3d bg, ba; +}; + +// Priors for biases +/** + * @brief 根据给定值的加速度计先验边，目的是把优化量维持在先验附近 + */ +class EdgePriorAcc : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexAccBias> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EdgePriorAcc(const Eigen::Vector3f &bprior_) : bprior(bprior_.cast()) {} + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + void computeError() + { + const VertexAccBias *VA = static_cast(_vertices[0]); + _error = bprior - VA->estimate(); + } + virtual void linearizeOplus(); + + Eigen::Matrix GetHessian() + { + linearizeOplus(); + return _jacobianOplusXi.transpose() * information() * _jacobianOplusXi; + } + + const Eigen::Vector3d bprior; +}; + +/** + * @brief 根据给定值的陀螺仪先验边，目的是把优化量维持在先验附近 + */ +class EdgePriorGyro : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexGyroBias> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EdgePriorGyro(const Eigen::Vector3f &bprior_) : bprior(bprior_.cast()) {} + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + void computeError() + { + const VertexGyroBias *VG = static_cast(_vertices[0]); + _error = bprior - VG->estimate(); + } + virtual void linearizeOplus(); + + Eigen::Matrix GetHessian() + { + linearizeOplus(); + return _jacobianOplusXi.transpose() * information() * _jacobianOplusXi; + } + + const Eigen::Vector3d bprior; +}; + +/** + * @brief 4DOF的二元边，误差为给定的旋转平移改变量 与 两个输入节点之间的旋转平移改变量的偏差 + */ +class Edge4DoF : public g2o::BaseBinaryEdge<6, Vector6d, VertexPose4DoF, VertexPose4DoF> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Edge4DoF(const Eigen::Matrix4d &deltaT) + { + dTij = deltaT; + dRij = deltaT.block<3, 3>(0, 0); + dtij = deltaT.block<3, 1>(0, 3); + } + + virtual bool read(std::istream &is) { return false; } + virtual bool write(std::ostream &os) const { return false; } + + void computeError() + { + const VertexPose4DoF *VPi = static_cast(_vertices[0]); + const VertexPose4DoF *VPj = static_cast(_vertices[1]); + _error << LogSO3(VPi->estimate().Rcw[0] * VPj->estimate().Rcw[0].transpose() * dRij.transpose()), + VPi->estimate().Rcw[0] * (-VPj->estimate().Rcw[0].transpose() * VPj->estimate().tcw[0]) + VPi->estimate().tcw[0] - dtij; + } + + // virtual void linearizeOplus(); // numerical implementation + + Eigen::Matrix4d dTij; + Eigen::Matrix3d dRij; + Eigen::Vector3d dtij; +}; + +} // namespace ORB_SLAM2 + +#endif // G2OTYPES_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/GeometricTools.h b/ORB-SLAM3-UESTC/Workspace/include/GeometricTools.h new file mode 100644 index 0000000..6fa8cd4 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/GeometricTools.h @@ -0,0 +1,79 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef GEOMETRIC_TOOLS_H +#define GEOMETRIC_TOOLS_H + +#include +#include +#include + +namespace ORB_SLAM3 +{ + +class KeyFrame; + +class GeometricTools +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + // Compute the Fundamental matrix between KF1 and KF2 + static Eigen::Matrix3f ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2); + + //Triangulate point with KF1 and KF2 + static bool Triangulate(Eigen::Vector3f &x_c1, Eigen::Vector3f &x_c2,Eigen::Matrix &Tc1w ,Eigen::Matrix &Tc2w , Eigen::Vector3f &x3D); + + template + static bool CheckMatrices(const cv::Mat &cvMat, const Eigen::Matrix &eigMat) { + const float epsilon = 1e-3; + // std::cout << cvMat.cols - cols << cvMat.rows - rows << std::endl; + if(rows != cvMat.rows || cols != cvMat.cols) { + std::cout << "wrong cvmat size\n"; + return false; + } + for(int i = 0; i < rows; i++) + for(int j = 0; j < cols; j++) + if ((cvMat.at(i,j) > (eigMat(i,j) + epsilon)) || + (cvMat.at(i,j) < (eigMat(i,j) - epsilon))){ + std::cout << "cv mat:\n" << cvMat << std::endl; + std::cout << "eig mat:\n" << eigMat << std::endl; + return false; + } + return true; + } + + template + static bool CheckMatrices( const Eigen::Matrix &eigMat1, const Eigen::Matrix &eigMat2) { + const float epsilon = 1e-3; + for(int i = 0; i < rows; i++) + for(int j = 0; j < cols; j++) + if ((eigMat1(i,j) > (eigMat2(i,j) + epsilon)) || + (eigMat1(i,j) < (eigMat2(i,j) - epsilon))){ + std::cout << "eig mat 1:\n" << eigMat1 << std::endl; + std::cout << "eig mat 2:\n" << eigMat2 << std::endl; + return false; + } + return true; + } + +}; + +}// namespace ORB_SLAM + +#endif // GEOMETRIC_TOOLS_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/ImuTypes.h b/ORB-SLAM3-UESTC/Workspace/include/ImuTypes.h new file mode 100644 index 0000000..79d9e4a --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/ImuTypes.h @@ -0,0 +1,266 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef IMUTYPES_H +#define IMUTYPES_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "SerializationUtils.h" + +#include +#include + +namespace ORB_SLAM3 +{ + +namespace IMU +{ + +const float GRAVITY_VALUE=9.81; + +//IMU measurement (gyro, accelerometer and timestamp) +class Point +{ +public: + Point(const float &acc_x, const float &acc_y, const float &acc_z, + const float &ang_vel_x, const float &ang_vel_y, const float &ang_vel_z, + const double ×tamp): a(acc_x,acc_y,acc_z), w(ang_vel_x,ang_vel_y,ang_vel_z), t(timestamp){} + Point(const cv::Point3f Acc, const cv::Point3f Gyro, const double ×tamp): + a(Acc.x,Acc.y,Acc.z), w(Gyro.x,Gyro.y,Gyro.z), t(timestamp){} +public: + Eigen::Vector3f a; + Eigen::Vector3f w; + double t; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +//IMU biases (gyro and accelerometer) +class Bias +{ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & bax; + ar & bay; + ar & baz; + + ar & bwx; + ar & bwy; + ar & bwz; + } + +public: + Bias():bax(0),bay(0),baz(0),bwx(0),bwy(0),bwz(0){} + Bias(const float &b_acc_x, const float &b_acc_y, const float &b_acc_z, + const float &b_ang_vel_x, const float &b_ang_vel_y, const float &b_ang_vel_z): + bax(b_acc_x), bay(b_acc_y), baz(b_acc_z), bwx(b_ang_vel_x), bwy(b_ang_vel_y), bwz(b_ang_vel_z){} + void CopyFrom(Bias &b); + friend std::ostream& operator<< (std::ostream &out, const Bias &b); + +public: + float bax, bay, baz; + float bwx, bwy, bwz; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +//IMU calibration (Tbc, Tcb, noise) +class Calib +{ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + serializeSophusSE3(ar,mTcb,version); + serializeSophusSE3(ar,mTbc,version); + + ar & boost::serialization::make_array(Cov.diagonal().data(), Cov.diagonal().size()); + ar & boost::serialization::make_array(CovWalk.diagonal().data(), CovWalk.diagonal().size()); + + ar & mbIsSet; + } + +public: + + Calib(const Sophus::SE3 &Tbc, const float &ng, const float &na, const float &ngw, const float &naw) + { + Set(Tbc,ng,na,ngw,naw); + } + + Calib(const Calib &calib); + Calib(){mbIsSet = false;} + + //void Set(const cv::Mat &cvTbc, const float &ng, const float &na, const float &ngw, const float &naw); + void Set(const Sophus::SE3 &sophTbc, const float &ng, const float &na, const float &ngw, const float &naw); + +public: + // Sophus/Eigen implementation + Sophus::SE3 mTcb; + Sophus::SE3 mTbc; + Eigen::DiagonalMatrix Cov, CovWalk; + bool mbIsSet; +}; + +//Integration of 1 gyro measurement +class IntegratedRotation +{ +public: + IntegratedRotation(){} + IntegratedRotation(const Eigen::Vector3f &angVel, const Bias &imuBias, const float &time); + +public: + float deltaT; //integration time + Eigen::Matrix3f deltaR; + Eigen::Matrix3f rightJ; // right jacobian + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +//Preintegration of Imu Measurements +class Preintegrated +{ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & dT; + ar & boost::serialization::make_array(C.data(), C.size()); + ar & boost::serialization::make_array(Info.data(), Info.size()); + ar & boost::serialization::make_array(Nga.diagonal().data(), Nga.diagonal().size()); + ar & boost::serialization::make_array(NgaWalk.diagonal().data(), NgaWalk.diagonal().size()); + ar & b; + ar & boost::serialization::make_array(dR.data(), dR.size()); + ar & boost::serialization::make_array(dV.data(), dV.size()); + ar & boost::serialization::make_array(dP.data(), dP.size()); + ar & boost::serialization::make_array(JRg.data(), JRg.size()); + ar & boost::serialization::make_array(JVg.data(), JVg.size()); + ar & boost::serialization::make_array(JVa.data(), JVa.size()); + ar & boost::serialization::make_array(JPg.data(), JPg.size()); + ar & boost::serialization::make_array(JPa.data(), JPa.size()); + ar & boost::serialization::make_array(avgA.data(), avgA.size()); + ar & boost::serialization::make_array(avgW.data(), avgW.size()); + + ar & bu; + ar & boost::serialization::make_array(db.data(), db.size()); + ar & mvMeasurements; + } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Preintegrated(const Bias &b_, const Calib &calib); + Preintegrated(Preintegrated* pImuPre); + Preintegrated() {} + ~Preintegrated() {} + void CopyFrom(Preintegrated* pImuPre); + void Initialize(const Bias &b_); + void IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt); + void Reintegrate(); + void MergePrevious(Preintegrated* pPrev); + void SetNewBias(const Bias &bu_); + IMU::Bias GetDeltaBias(const Bias &b_); + + Eigen::Matrix3f GetDeltaRotation(const Bias &b_); + Eigen::Vector3f GetDeltaVelocity(const Bias &b_); + Eigen::Vector3f GetDeltaPosition(const Bias &b_); + + Eigen::Matrix3f GetUpdatedDeltaRotation(); + Eigen::Vector3f GetUpdatedDeltaVelocity(); + Eigen::Vector3f GetUpdatedDeltaPosition(); + + Eigen::Matrix3f GetOriginalDeltaRotation(); + Eigen::Vector3f GetOriginalDeltaVelocity(); + Eigen::Vector3f GetOriginalDeltaPosition(); + + Eigen::Matrix GetDeltaBias(); + + Bias GetOriginalBias(); + Bias GetUpdatedBias(); + + void printMeasurements() const { + std::cout << "pint meas:\n"; + for(int i=0; i C; + Eigen::Matrix Info; + Eigen::DiagonalMatrix Nga, NgaWalk; + + // Values for the original bias (when integration was computed) + Bias b; + Eigen::Matrix3f dR; + Eigen::Vector3f dV, dP; + Eigen::Matrix3f JRg, JVg, JVa, JPg, JPa; + Eigen::Vector3f avgA, avgW; + + +private: + // Updated bias + Bias bu; //更新后的零偏 + // Dif between original and updated bias + // This is used to compute the updated values of the preintegration + Eigen::Matrix db; + + struct integrable + { + template + void serialize(Archive & ar, const unsigned int version) + { + ar & boost::serialization::make_array(a.data(), a.size()); + ar & boost::serialization::make_array(w.data(), w.size()); + ar & t; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + integrable(){} + integrable(const Eigen::Vector3f &a_, const Eigen::Vector3f &w_ , const float &t_):a(a_),w(w_),t(t_){} + Eigen::Vector3f a, w; + float t; + }; + + std::vector mvMeasurements; + + std::mutex mMutex; +}; + +// Lie Algebra Functions +Eigen::Matrix3f RightJacobianSO3(const float &x, const float &y, const float &z); +Eigen::Matrix3f RightJacobianSO3(const Eigen::Vector3f &v); + +Eigen::Matrix3f InverseRightJacobianSO3(const float &x, const float &y, const float &z); +Eigen::Matrix3f InverseRightJacobianSO3(const Eigen::Vector3f &v); + +Eigen::Matrix3f NormalizeRotation(const Eigen::Matrix3f &R); + +} + +} //namespace ORB_SLAM2 + +#endif // IMUTYPES_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/Initializer.h b/ORB-SLAM3-UESTC/Workspace/include/Initializer.h new file mode 100644 index 0000000..0f866c4 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/Initializer.h @@ -0,0 +1,106 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef INITIALIZER_H +#define INITIALIZER_H + +#include +#include "Frame.h" + +#include + +namespace ORB_SLAM3 +{ + +class Map; + +// THIS IS THE INITIALIZER FOR MONOCULAR SLAM. NOT USED IN THE STEREO OR RGBD CASE. +class Initializer +{ + typedef pair Match; + +public: + + // Fix the reference frame + Initializer(const Frame &ReferenceFrame, float sigma = 1.0, int iterations = 200); + + // Computes in parallel a fundamental matrix and a homography + // Selects a model and tries to recover the motion and the structure from motion + bool Initialize(const Frame &CurrentFrame, const vector &vMatches12, cv::Mat &R21, + cv::Mat &t21, vector &vP3D, vector &vbTriangulated); + +private: + + void FindHomography(vector &vbMatchesInliers, float &score, cv::Mat &H21); + void FindFundamental(vector &vbInliers, float &score, cv::Mat &F21); + + cv::Mat ComputeH21(const vector &vP1, const vector &vP2); + cv::Mat ComputeF21(const vector &vP1, const vector &vP2); + + float CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector &vbMatchesInliers, float sigma); + + float CheckFundamental(const cv::Mat &F21, vector &vbMatchesInliers, float sigma); + + bool ReconstructF(vector &vbMatchesInliers, cv::Mat &F21, cv::Mat &K, + cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated); + + bool ReconstructH(vector &vbMatchesInliers, cv::Mat &H21, cv::Mat &K, + cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated); + + void Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D); + + void Normalize(const vector &vKeys, vector &vNormalizedPoints, cv::Mat &T); + // void Normalize(const vector &vKeys, vector &vNormalizedPoints, cv::Mat &T); + + int CheckRT(const cv::Mat &R, const cv::Mat &t, const vector &vKeys1, const vector &vKeys2, + const vector &vMatches12, vector &vbInliers, + const cv::Mat &K, vector &vP3D, float th2, vector &vbGood, float ¶llax); + + void DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t); + + + // Keypoints from Reference Frame (Frame 1) + vector mvKeys1; + + // Keypoints from Current Frame (Frame 2) + vector mvKeys2; + + // Current Matches from Reference to Current + vector mvMatches12; + vector mvbMatched1; + + // Calibration + cv::Mat mK; + + // Standard Deviation and Variance + float mSigma, mSigma2; + + // Ransac max iterations + int mMaxIterations; + + // Ransac sets + vector > mvSets; + + GeometricCamera* mpCamera; + +}; + +} //namespace ORB_SLAM + +#endif // INITIALIZER_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/KeyFrame.h b/ORB-SLAM3-UESTC/Workspace/include/KeyFrame.h new file mode 100644 index 0000000..1b8d1c4 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/KeyFrame.h @@ -0,0 +1,544 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef KEYFRAME_H +#define KEYFRAME_H + +#include "MapPoint.h" +#include "Thirdparty/DBoW2/DBoW2/BowVector.h" +#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" +#include "ORBVocabulary.h" +#include "ORBextractor.h" +#include "Frame.h" +#include "KeyFrameDatabase.h" +#include "ImuTypes.h" + +#include "GeometricCamera.h" +#include "SerializationUtils.h" + +#include + +#include +#include +#include + + +namespace ORB_SLAM3 +{ + +class Map; +class MapPoint; +class Frame; +class KeyFrameDatabase; + +class GeometricCamera; + +class KeyFrame +{ + friend class boost::serialization::access; + + template + void serialize(Archive& ar, const unsigned int version) + { + ar & mnId; + ar & const_cast(mnFrameId); + ar & const_cast(mTimeStamp); + // Grid + ar & const_cast(mnGridCols); + ar & const_cast(mnGridRows); + ar & const_cast(mfGridElementWidthInv); + ar & const_cast(mfGridElementHeightInv); + + // Variables of tracking + //ar & mnTrackReferenceForFrame; + //ar & mnFuseTargetForKF; + // Variables of local mapping + //ar & mnBALocalForKF; + //ar & mnBAFixedForKF; + //ar & mnNumberOfOpt; + // Variables used by KeyFrameDatabase + //ar & mnLoopQuery; + //ar & mnLoopWords; + //ar & mLoopScore; + //ar & mnRelocQuery; + //ar & mnRelocWords; + //ar & mRelocScore; + //ar & mnMergeQuery; + //ar & mnMergeWords; + //ar & mMergeScore; + //ar & mnPlaceRecognitionQuery; + //ar & mnPlaceRecognitionWords; + //ar & mPlaceRecognitionScore; + //ar & mbCurrentPlaceRecognition; + // Variables of loop closing + //serializeMatrix(ar,mTcwGBA,version); + //serializeMatrix(ar,mTcwBefGBA,version); + //serializeMatrix(ar,mVwbGBA,version); + //serializeMatrix(ar,mVwbBefGBA,version); + //ar & mBiasGBA; + //ar & mnBAGlobalForKF; + // Variables of Merging + //serializeMatrix(ar,mTcwMerge,version); + //serializeMatrix(ar,mTcwBefMerge,version); + //serializeMatrix(ar,mTwcBefMerge,version); + //serializeMatrix(ar,mVwbMerge,version); + //serializeMatrix(ar,mVwbBefMerge,version); + //ar & mBiasMerge; + //ar & mnMergeCorrectedForKF; + //ar & mnMergeForKF; + //ar & mfScaleMerge; + //ar & mnBALocalForMerge; + + // Scale + ar & mfScale; + // Calibration parameters + ar & const_cast(fx); + ar & const_cast(fy); + ar & const_cast(invfx); + ar & const_cast(invfy); + ar & const_cast(cx); + ar & const_cast(cy); + ar & const_cast(mbf); + ar & const_cast(mb); + ar & const_cast(mThDepth); + serializeMatrix(ar, mDistCoef, version); + // Number of Keypoints + ar & const_cast(N); + // KeyPoints + serializeVectorKeyPoints(ar, mvKeys, version); + serializeVectorKeyPoints(ar, mvKeysUn, version); + ar & const_cast& >(mvuRight); + ar & const_cast& >(mvDepth); + serializeMatrix(ar,mDescriptors,version); + // BOW + ar & mBowVec; + ar & mFeatVec; + // Pose relative to parent + serializeSophusSE3(ar, mTcp, version); + // Scale + ar & const_cast(mnScaleLevels); + ar & const_cast(mfScaleFactor); + ar & const_cast(mfLogScaleFactor); + ar & const_cast& >(mvScaleFactors); + ar & const_cast& >(mvLevelSigma2); + ar & const_cast& >(mvInvLevelSigma2); + // Image bounds and calibration + ar & const_cast(mnMinX); + ar & const_cast(mnMinY); + ar & const_cast(mnMaxX); + ar & const_cast(mnMaxY); + ar & boost::serialization::make_array(mK_.data(), mK_.size()); + // Pose + serializeSophusSE3(ar, mTcw, version); + // MapPointsId associated to keypoints + ar & mvBackupMapPointsId; + // Grid + ar & mGrid; + // Connected KeyFrameWeight + ar & mBackupConnectedKeyFrameIdWeights; + // Spanning Tree and Loop Edges + ar & mbFirstConnection; + ar & mBackupParentId; + ar & mvBackupChildrensId; + ar & mvBackupLoopEdgesId; + ar & mvBackupMergeEdgesId; + // Bad flags + ar & mbNotErase; + ar & mbToBeErased; + ar & mbBad; + + ar & mHalfBaseline; + + ar & mnOriginMapId; + + // Camera variables + ar & mnBackupIdCamera; + ar & mnBackupIdCamera2; + + // Fisheye variables + ar & mvLeftToRightMatch; + ar & mvRightToLeftMatch; + ar & const_cast(NLeft); + ar & const_cast(NRight); + serializeSophusSE3(ar, mTlr, version); + serializeVectorKeyPoints(ar, mvKeysRight, version); + ar & mGridRight; + + // Inertial variables + ar & mImuBias; + ar & mBackupImuPreintegrated; + ar & mImuCalib; + ar & mBackupPrevKFId; + ar & mBackupNextKFId; + ar & bImu; + ar & boost::serialization::make_array(mVw.data(), mVw.size()); + ar & boost::serialization::make_array(mOwb.data(), mOwb.size()); + ar & mbHasVelocity; + } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + KeyFrame(); + KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB); + + // Pose functions + void SetPose(const Sophus::SE3f &Tcw); + void SetVelocity(const Eigen::Vector3f &Vw_); + + Sophus::SE3f GetPose(); + + Sophus::SE3f GetPoseInverse(); + Eigen::Vector3f GetCameraCenter(); + + Eigen::Vector3f GetImuPosition(); + Eigen::Matrix3f GetImuRotation(); + Sophus::SE3f GetImuPose(); + Eigen::Matrix3f GetRotation(); + Eigen::Vector3f GetTranslation(); + Eigen::Vector3f GetVelocity(); + bool isVelocitySet(); + + // Bag of Words Representation + void ComputeBoW(); + + // Covisibility graph functions + void AddConnection(KeyFrame* pKF, const int &weight); + void EraseConnection(KeyFrame* pKF); + + void UpdateConnections(bool upParent=true); + void UpdateBestCovisibles(); + std::set GetConnectedKeyFrames(); + std::vector GetVectorCovisibleKeyFrames(); + std::vector GetBestCovisibilityKeyFrames(const int &N); + std::vector GetCovisiblesByWeight(const int &w); + int GetWeight(KeyFrame* pKF); + + // Spanning tree functions + void AddChild(KeyFrame* pKF); + void EraseChild(KeyFrame* pKF); + void ChangeParent(KeyFrame* pKF); + std::set GetChilds(); + KeyFrame* GetParent(); + bool hasChild(KeyFrame* pKF); + void SetFirstConnection(bool bFirst); + + // Loop Edges + void AddLoopEdge(KeyFrame* pKF); + std::set GetLoopEdges(); + + // Merge Edges + void AddMergeEdge(KeyFrame* pKF); + set GetMergeEdges(); + + // MapPoint observation functions + int GetNumberMPs(); + void AddMapPoint(MapPoint* pMP, const size_t &idx); + void EraseMapPointMatch(const int &idx); + void EraseMapPointMatch(MapPoint* pMP); + void ReplaceMapPointMatch(const int &idx, MapPoint* pMP); + std::set GetMapPoints(); + std::vector GetMapPointMatches(); + int TrackedMapPoints(const int &minObs); + MapPoint* GetMapPoint(const size_t &idx); + + // KeyPoint functions + std::vector GetFeaturesInArea(const float &x, const float &y, const float &r, const bool bRight = false) const; + bool UnprojectStereo(int i, Eigen::Vector3f &x3D); + + // Image + bool IsInImage(const float &x, const float &y) const; + + // Enable/Disable bad flag changes + void SetNotErase(); + void SetErase(); + + // Set/check bad flag + void SetBadFlag(); + bool isBad(); + + // Compute Scene Depth (q=2 median). Used in monocular. + float ComputeSceneMedianDepth(const int q); + + static bool weightComp( int a, int b){ + return a>b; + } + + static bool lId(KeyFrame* pKF1, KeyFrame* pKF2){ + return pKF1->mnIdmnId; + } + + Map* GetMap(); + void UpdateMap(Map* pMap); + + void SetNewBias(const IMU::Bias &b); + Eigen::Vector3f GetGyroBias(); + + Eigen::Vector3f GetAccBias(); + + IMU::Bias GetImuBias(); + + bool ProjectPointDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float &v); + bool ProjectPointUnDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float &v); + + void PreSave(set& spKF,set& spMP, set& spCam); + void PostLoad(map& mpKFid, map& mpMPid, map& mpCamId); + + + void SetORBVocabulary(ORBVocabulary* pORBVoc); + void SetKeyFrameDatabase(KeyFrameDatabase* pKFDB); + + bool bImu; + + // The following variables are accesed from only 1 thread or never change (no mutex needed). +public: + + static long unsigned int nNextId; + long unsigned int mnId; + const long unsigned int mnFrameId; + + const double mTimeStamp; + + // Grid (to speed up feature matching) + const int mnGridCols; + const int mnGridRows; + const float mfGridElementWidthInv; + const float mfGridElementHeightInv; + + // Variables used by the tracking + long unsigned int mnTrackReferenceForFrame; + long unsigned int mnFuseTargetForKF; + + // Variables used by the local mapping + long unsigned int mnBALocalForKF; + long unsigned int mnBAFixedForKF; + + //Number of optimizations by BA(amount of iterations in BA) + long unsigned int mnNumberOfOpt; + + // Variables used by the keyframe database + long unsigned int mnLoopQuery; + int mnLoopWords; + float mLoopScore; + long unsigned int mnRelocQuery; + int mnRelocWords; + float mRelocScore; + long unsigned int mnMergeQuery; + int mnMergeWords; + float mMergeScore; + long unsigned int mnPlaceRecognitionQuery; + int mnPlaceRecognitionWords; + float mPlaceRecognitionScore; + + bool mbCurrentPlaceRecognition; + + + // Variables used by loop closing + Sophus::SE3f mTcwGBA; + Sophus::SE3f mTcwBefGBA; + Eigen::Vector3f mVwbGBA; + Eigen::Vector3f mVwbBefGBA; + IMU::Bias mBiasGBA; + long unsigned int mnBAGlobalForKF; + + // Variables used by merging + Sophus::SE3f mTcwMerge; + Sophus::SE3f mTcwBefMerge; + Sophus::SE3f mTwcBefMerge; + Eigen::Vector3f mVwbMerge; + Eigen::Vector3f mVwbBefMerge; + IMU::Bias mBiasMerge; + long unsigned int mnMergeCorrectedForKF; + long unsigned int mnMergeForKF; + float mfScaleMerge; + long unsigned int mnBALocalForMerge; + + float mfScale; + + // Calibration parameters + const float fx, fy, cx, cy, invfx, invfy, mbf, mb, mThDepth; + cv::Mat mDistCoef; + + // Number of KeyPoints + const int N; + + // KeyPoints, stereo coordinate and descriptors (all associated by an index) + const std::vector mvKeys; + const std::vector mvKeysUn; + const std::vector mvuRight; // negative value for monocular points + const std::vector mvDepth; // negative value for monocular points + const cv::Mat mDescriptors; + + //BoW + DBoW2::BowVector mBowVec; + DBoW2::FeatureVector mFeatVec; + + // Pose relative to parent (this is computed when bad flag is activated) + Sophus::SE3f mTcp; + + // Scale + const int mnScaleLevels; + const float mfScaleFactor; + const float mfLogScaleFactor; + const std::vector mvScaleFactors; + const std::vector mvLevelSigma2; + const std::vector mvInvLevelSigma2; + + // Image bounds and calibration + const int mnMinX; + const int mnMinY; + const int mnMaxX; + const int mnMaxY; + + // Preintegrated IMU measurements from previous keyframe + KeyFrame* mPrevKF; + KeyFrame* mNextKF; + + IMU::Preintegrated* mpImuPreintegrated; + IMU::Calib mImuCalib; + + unsigned int mnOriginMapId; + + string mNameFile; + + int mnDataset; + + std::vector mvpLoopCandKFs; + std::vector mvpMergeCandKFs; + + //bool mbHasHessian; + //cv::Mat mHessianPose; + + // The following variables need to be accessed trough a mutex to be thread safe. +protected: + // sophus poses + Sophus::SE3 mTcw; + Eigen::Matrix3f mRcw; + Sophus::SE3 mTwc; + Eigen::Matrix3f mRwc; + + // IMU position + Eigen::Vector3f mOwb; + // Velocity (Only used for inertial SLAM) + Eigen::Vector3f mVw; + bool mbHasVelocity; + + //Transformation matrix between cameras in stereo fisheye + Sophus::SE3 mTlr; + Sophus::SE3 mTrl; + + // Imu bias + IMU::Bias mImuBias; + + // MapPoints associated to keypoints + std::vector mvpMapPoints; + // For save relation without pointer, this is necessary for save/load function + std::vector mvBackupMapPointsId; + + // BoW + KeyFrameDatabase* mpKeyFrameDB; + ORBVocabulary* mpORBvocabulary; + + // Grid over the image to speed up feature matching + std::vector< std::vector > > mGrid; + + std::map mConnectedKeyFrameWeights; + std::vector mvpOrderedConnectedKeyFrames; + std::vector mvOrderedWeights; + // For save relation without pointer, this is necessary for save/load function + std::map mBackupConnectedKeyFrameIdWeights; + + // Spanning Tree and Loop Edges + bool mbFirstConnection; + KeyFrame* mpParent; + std::set mspChildrens; + std::set mspLoopEdges; + std::set mspMergeEdges; + // For save relation without pointer, this is necessary for save/load function + long long int mBackupParentId; + std::vector mvBackupChildrensId; + std::vector mvBackupLoopEdgesId; + std::vector mvBackupMergeEdgesId; + + // Bad flags + bool mbNotErase; + bool mbToBeErased; + bool mbBad; + + float mHalfBaseline; // Only for visualization + + Map* mpMap; + + // Backup variables for inertial + long long int mBackupPrevKFId; + long long int mBackupNextKFId; + IMU::Preintegrated mBackupImuPreintegrated; + + // Backup for Cameras + unsigned int mnBackupIdCamera, mnBackupIdCamera2; + + // Calibration + Eigen::Matrix3f mK_; + + // Mutex + std::mutex mMutexPose; // for pose, velocity and biases + std::mutex mMutexConnections; + std::mutex mMutexFeatures; + std::mutex mMutexMap; + +public: + GeometricCamera* mpCamera, *mpCamera2; + + //Indexes of stereo observations correspondences + std::vector mvLeftToRightMatch, mvRightToLeftMatch; + + Sophus::SE3f GetRelativePoseTrl(); + Sophus::SE3f GetRelativePoseTlr(); + + //KeyPoints in the right image (for stereo fisheye, coordinates are needed) + const std::vector mvKeysRight; + + const int NLeft, NRight; + + std::vector< std::vector > > mGridRight; + + Sophus::SE3 GetRightPose(); + Sophus::SE3 GetRightPoseInverse(); + + Eigen::Vector3f GetRightCameraCenter(); + Eigen::Matrix GetRightRotation(); + Eigen::Vector3f GetRightTranslation(); + + void PrintPointDistribution(){ + int left = 0, right = 0; + int Nlim = (NLeft != -1) ? NLeft : N; + for(int i = 0; i < N; i++){ + if(mvpMapPoints[i]){ + if(i < Nlim) left++; + else right++; + } + } + cout << "Point distribution in KeyFrame: left-> " << left << " --- right-> " << right << endl; + } + + +}; + +} //namespace ORB_SLAM + +#endif // KEYFRAME_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/KeyFrameDatabase.h b/ORB-SLAM3-UESTC/Workspace/include/KeyFrameDatabase.h new file mode 100644 index 0000000..e1236c9 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/KeyFrameDatabase.h @@ -0,0 +1,103 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef KEYFRAMEDATABASE_H +#define KEYFRAMEDATABASE_H + +#include +#include +#include + +#include "KeyFrame.h" +#include "Frame.h" +#include "ORBVocabulary.h" +#include "Map.h" + +#include +#include +#include + +#include + + +namespace ORB_SLAM3 +{ + +class KeyFrame; +class Frame; +class Map; + + +class KeyFrameDatabase +{ + friend class boost::serialization::access; + + template + void serialize(Archive& ar, const unsigned int version) + { + ar & mvBackupInvertedFileId; + } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + KeyFrameDatabase(){} + KeyFrameDatabase(const ORBVocabulary &voc); + + void add(KeyFrame* pKF); + + void erase(KeyFrame* pKF); + + void clear(); + void clearMap(Map* pMap); + + // Loop Detection(DEPRECATED) + std::vector DetectLoopCandidates(KeyFrame* pKF, float minScore); + + // Loop and Merge Detection + void DetectCandidates(KeyFrame* pKF, float minScore,vector& vpLoopCand, vector& vpMergeCand); + void DetectBestCandidates(KeyFrame *pKF, vector &vpLoopCand, vector &vpMergeCand, int nMinWords); + void DetectNBestCandidates(KeyFrame *pKF, vector &vpLoopCand, vector &vpMergeCand, int nNumCandidates); + + // Relocalization + std::vector DetectRelocalizationCandidates(Frame* F, Map* pMap); + + void PreSave(); + void PostLoad(map mpKFid); + void SetORBVocabulary(ORBVocabulary* pORBVoc); + +protected: + + // Associated vocabulary + const ORBVocabulary* mpVoc; + + // Inverted file + std::vector > mvInvertedFile; + + // For save relation without pointer, this is necessary for save/load function + std::vector > mvBackupInvertedFileId; + + // Mutex + std::mutex mMutex; + +}; + +} //namespace ORB_SLAM + +#endif diff --git a/ORB-SLAM3-UESTC/Workspace/include/LocalMapping.h b/ORB-SLAM3-UESTC/Workspace/include/LocalMapping.h new file mode 100644 index 0000000..dd04a3d --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/LocalMapping.h @@ -0,0 +1,202 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef LOCALMAPPING_H +#define LOCALMAPPING_H + +#include "KeyFrame.h" +#include "Atlas.h" +#include "LoopClosing.h" +#include "Tracking.h" +#include "KeyFrameDatabase.h" +#include "Settings.h" + +#include + + +namespace ORB_SLAM3 +{ + +class System; +class Tracking; +class LoopClosing; +class Atlas; + +class LocalMapping +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + LocalMapping(System* pSys, Atlas* pAtlas, const float bMonocular, bool bInertial, const string &_strSeqName=std::string()); + + void SetLoopCloser(LoopClosing* pLoopCloser); + + void SetTracker(Tracking* pTracker); + + // Main function + void Run(); + + void InsertKeyFrame(KeyFrame* pKF); + void EmptyQueue(); + + // Thread Synch + void RequestStop(); + void RequestReset(); + void RequestResetActiveMap(Map* pMap); + bool Stop(); + void Release(); + bool isStopped(); + bool stopRequested(); + bool AcceptKeyFrames(); + void SetAcceptKeyFrames(bool flag); + bool SetNotStop(bool flag); + + void InterruptBA(); + + void RequestFinish(); + bool isFinished(); + + int KeyframesInQueue(){ + unique_lock lock(mMutexNewKFs); + return mlNewKeyFrames.size(); + } + + bool IsInitializing(); + double GetCurrKFTime(); + KeyFrame* GetCurrKF(); + + std::mutex mMutexImuInit; + + Eigen::MatrixXd mcovInertial; + Eigen::Matrix3d mRwg; + Eigen::Vector3d mbg; + Eigen::Vector3d mba; + double mScale; + double mInitTime; + double mCostTime; + + unsigned int mInitSect; + unsigned int mIdxInit; + unsigned int mnKFs; + double mFirstTs; + int mnMatchesInliers; + + // For debugging (erase in normal mode) + int mInitFr; + int mIdxIteration; + string strSequence; + + bool mbNotBA1; + bool mbNotBA2; + bool mbBadImu; + + bool mbWriteStats; + + // not consider far points (clouds) + bool mbFarPoints; + float mThFarPoints; + +#ifdef REGISTER_TIMES + vector vdKFInsert_ms; + vector vdMPCulling_ms; + vector vdMPCreation_ms; + vector vdLBA_ms; + vector vdKFCulling_ms; + vector vdLMTotal_ms; + + + vector vdLBASync_ms; + vector vdKFCullingSync_ms; + vector vnLBA_edges; + vector vnLBA_KFopt; + vector vnLBA_KFfixed; + vector vnLBA_MPs; + int nLBA_exec; + int nLBA_abort; +#endif +protected: + + bool CheckNewKeyFrames(); + void ProcessNewKeyFrame(); + void CreateNewMapPoints(); + + void MapPointCulling(); + void SearchInNeighbors(); + void KeyFrameCulling(); + + System *mpSystem; + + bool mbMonocular; + bool mbInertial; + + void ResetIfRequested(); + bool mbResetRequested; + bool mbResetRequestedActiveMap; + Map* mpMapToReset; + std::mutex mMutexReset; + + bool CheckFinish(); + void SetFinish(); + bool mbFinishRequested; + bool mbFinished; + std::mutex mMutexFinish; + + Atlas* mpAtlas; + + LoopClosing* mpLoopCloser; + Tracking* mpTracker; + + std::list mlNewKeyFrames; + + KeyFrame* mpCurrentKeyFrame; + + std::list mlpRecentAddedMapPoints; + + std::mutex mMutexNewKFs; + + bool mbAbortBA; + + bool mbStopped; + bool mbStopRequested; + bool mbNotStop; + std::mutex mMutexStop; + + bool mbAcceptKeyFrames; + std::mutex mMutexAccept; + + void InitializeIMU(float priorG = 1e2, float priorA = 1e6, bool bFirst = false); + void ScaleRefinement(); + + bool bInitializing; + + Eigen::MatrixXd infoInertial; + int mNumLM; + int mNumKFCulling; + + float mTinit; + + int countRefinement; + + //DEBUG + ofstream f_lm; + + }; + +} //namespace ORB_SLAM + +#endif // LOCALMAPPING_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/LoopClosing.h b/ORB-SLAM3-UESTC/Workspace/include/LoopClosing.h new file mode 100644 index 0000000..3077aa3 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/LoopClosing.h @@ -0,0 +1,250 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef LOOPCLOSING_H +#define LOOPCLOSING_H + +#include "KeyFrame.h" +#include "LocalMapping.h" +#include "Atlas.h" +#include "ORBVocabulary.h" +#include "Tracking.h" + +#include "KeyFrameDatabase.h" + +#include +#include +#include +#include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" + +namespace ORB_SLAM3 +{ + +class Tracking; +class LocalMapping; +class KeyFrameDatabase; +class Map; + + +class LoopClosing +{ +public: + + typedef pair,int> ConsistentGroup; + typedef map, + Eigen::aligned_allocator > > KeyFrameAndPose; + +public: + + LoopClosing(Atlas* pAtlas, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale, const bool bActiveLC); + + void SetTracker(Tracking* pTracker); + + void SetLocalMapper(LocalMapping* pLocalMapper); + + // Main function + void Run(); + + void InsertKeyFrame(KeyFrame *pKF); + + void RequestReset(); + void RequestResetActiveMap(Map* pMap); + + // This function will run in a separate thread + void RunGlobalBundleAdjustment(Map* pActiveMap, unsigned long nLoopKF); + + bool isRunningGBA(){ + unique_lock lock(mMutexGBA); + return mbRunningGBA; + } + bool isFinishedGBA(){ + unique_lock lock(mMutexGBA); + return mbFinishedGBA; + } + + void RequestFinish(); + + bool isFinished(); + + Viewer* mpViewer; + +#ifdef REGISTER_TIMES + + vector vdDataQuery_ms; + vector vdEstSim3_ms; + vector vdPRTotal_ms; + + vector vdMergeMaps_ms; + vector vdWeldingBA_ms; + vector vdMergeOptEss_ms; + vector vdMergeTotal_ms; + vector vnMergeKFs; + vector vnMergeMPs; + int nMerges; + + vector vdLoopFusion_ms; + vector vdLoopOptEss_ms; + vector vdLoopTotal_ms; + vector vnLoopKFs; + int nLoop; + + vector vdGBA_ms; + vector vdUpdateMap_ms; + vector vdFGBATotal_ms; + vector vnGBAKFs; + vector vnGBAMPs; + int nFGBA_exec; + int nFGBA_abort; + +#endif + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +protected: + + bool CheckNewKeyFrames(); + + + //Methods to implement the new place recognition algorithm + bool NewDetectCommonRegions(); + bool DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches, + std::vector &vpMPs, std::vector &vpMatchedMPs); + bool DetectCommonRegionsFromBoW(std::vector &vpBowCand, KeyFrame* &pMatchedKF, KeyFrame* &pLastCurrentKF, g2o::Sim3 &g2oScw, + int &nNumCoincidences, std::vector &vpMPs, std::vector &vpMatchedMPs); + bool DetectCommonRegionsFromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches, + std::vector &vpMPs, std::vector &vpMatchedMPs); + int FindMatchesByProjection(KeyFrame* pCurrentKF, KeyFrame* pMatchedKFw, g2o::Sim3 &g2oScw, + set &spMatchedMPinOrigin, vector &vpMapPoints, + vector &vpMatchedMapPoints); + + + void SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap, vector &vpMapPoints); + void SearchAndFuse(const vector &vConectedKFs, vector &vpMapPoints); + + void CorrectLoop(); + + void MergeLocal(); + void MergeLocal2(); + + void CheckObservations(set &spKFsMap1, set &spKFsMap2); + + void ResetIfRequested(); + bool mbResetRequested; + bool mbResetActiveMapRequested; + Map* mpMapToReset; + std::mutex mMutexReset; + + bool CheckFinish(); + void SetFinish(); + bool mbFinishRequested; + bool mbFinished; + std::mutex mMutexFinish; + + Atlas* mpAtlas; + Tracking* mpTracker; + + KeyFrameDatabase* mpKeyFrameDB; + ORBVocabulary* mpORBVocabulary; + + LocalMapping *mpLocalMapper; + + std::list mlpLoopKeyFrameQueue; + + std::mutex mMutexLoopQueue; + + // Loop detector parameters + float mnCovisibilityConsistencyTh; + + // Loop detector variables + KeyFrame* mpCurrentKF; + KeyFrame* mpLastCurrentKF; + KeyFrame* mpMatchedKF; + std::vector mvConsistentGroups; + std::vector mvpEnoughConsistentCandidates; + std::vector mvpCurrentConnectedKFs; + std::vector mvpCurrentMatchedPoints; + std::vector mvpLoopMapPoints; + + g2o::Sim3 mg2oScw; + + //------- + Map* mpLastMap; + + bool mbLoopDetected; + int mnLoopNumCoincidences; + int mnLoopNumNotFound; + KeyFrame* mpLoopLastCurrentKF; + g2o::Sim3 mg2oLoopSlw; + g2o::Sim3 mg2oLoopScw; + KeyFrame* mpLoopMatchedKF; + std::vector mvpLoopMPs; + std::vector mvpLoopMatchedMPs; + bool mbMergeDetected; + int mnMergeNumCoincidences; + int mnMergeNumNotFound; + KeyFrame* mpMergeLastCurrentKF; + g2o::Sim3 mg2oMergeSlw; + g2o::Sim3 mg2oMergeSmw; + g2o::Sim3 mg2oMergeScw; + KeyFrame* mpMergeMatchedKF; + std::vector mvpMergeMPs; + std::vector mvpMergeMatchedMPs; + std::vector mvpMergeConnectedKFs; + + g2o::Sim3 mSold_new; + //------- + + long unsigned int mLastLoopKFid; + + // Variables related to Global Bundle Adjustment + bool mbRunningGBA; + bool mbFinishedGBA; + bool mbStopGBA; + std::mutex mMutexGBA; + std::thread* mpThreadGBA; + + // Fix scale in the stereo/RGB-D case + bool mbFixScale; + + + bool mnFullBAIdx; + + + + vector vdPR_CurrentTime; + vector vdPR_MatchedTime; + vector vnPR_TypeRecogn; + + //DEBUG + string mstrFolderSubTraj; + int mnNumCorrection; + int mnCorrectionGBA; + + + // To (de)activate LC + bool mbActiveLC = true; + +#ifdef REGISTER_LOOP + string mstrFolderLoop; +#endif +}; + +} //namespace ORB_SLAM + +#endif // LOOPCLOSING_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/MLPnPsolver.h b/ORB-SLAM3-UESTC/Workspace/include/MLPnPsolver.h new file mode 100644 index 0000000..963612c --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/MLPnPsolver.h @@ -0,0 +1,257 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +/****************************************************************************** +* Author: Steffen Urban * +* Contact: urbste@gmail.com * +* License: Copyright (c) 2016 Steffen Urban, ANU. All rights reserved. * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions * +* are met: * +* * Redistributions of source code must retain the above copyright * +* notice, this list of conditions and the following disclaimer. * +* * Redistributions in binary form must reproduce the above copyright * +* notice, this list of conditions and the following disclaimer in the * +* documentation and/or other materials provided with the distribution. * +* * Neither the name of ANU nor the names of its contributors may be * +* used to endorse or promote products derived from this software without * +* specific prior written permission. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * +* ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE * +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR * +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * +* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * +* SUCH DAMAGE. * +******************************************************************************/ + +#ifndef ORB_SLAM3_MLPNPSOLVER_H +#define ORB_SLAM3_MLPNPSOLVER_H + +#include "MapPoint.h" +#include "Frame.h" + +#include +#include + +namespace ORB_SLAM3{ + class MLPnPsolver { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + MLPnPsolver(const Frame &F, const vector &vpMapPointMatches); + + ~MLPnPsolver(); + + void SetRansacParameters(double probability = 0.99, int minInliers = 8, int maxIterations = 300, int minSet = 6, float epsilon = 0.4, + float th2 = 5.991); + + //Find metod is necessary? + + bool iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers, Eigen::Matrix4f &Tout); + + //Type definitions needed by the original code + + /** A 3-vector of unit length used to describe landmark observations/bearings + * in camera frames (always expressed in camera frames) + */ + typedef Eigen::Vector3d bearingVector_t; + + /** An array of bearing-vectors */ + typedef std::vector > + bearingVectors_t; + + /** A 2-matrix containing the 2D covariance information of a bearing vector + */ + typedef Eigen::Matrix2d cov2_mat_t; + + /** A 3-matrix containing the 3D covariance information of a bearing vector */ + typedef Eigen::Matrix3d cov3_mat_t; + + /** An array of 3D covariance matrices */ + typedef std::vector > + cov3_mats_t; + + /** A 3-vector describing a point in 3D-space */ + typedef Eigen::Vector3d point_t; + + /** An array of 3D-points */ + typedef std::vector > + points_t; + + /** A homogeneous 3-vector describing a point in 3D-space */ + typedef Eigen::Vector4d point4_t; + + /** An array of homogeneous 3D-points */ + typedef std::vector > + points4_t; + + /** A 3-vector containing the rodrigues parameters of a rotation matrix */ + typedef Eigen::Vector3d rodrigues_t; + + /** A rotation matrix */ + typedef Eigen::Matrix3d rotation_t; + + /** A 3x4 transformation matrix containing rotation \f$ \mathbf{R} \f$ and + * translation \f$ \mathbf{t} \f$ as follows: + * \f$ \left( \begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array} \right) \f$ + */ + typedef Eigen::Matrix transformation_t; + + /** A 3-vector describing a translation/camera position */ + typedef Eigen::Vector3d translation_t; + + + + private: + void CheckInliers(); + bool Refine(); + + //Functions from de original MLPnP code + + /* + * Computes the camera pose given 3D points coordinates (in the camera reference + * system), the camera rays and (optionally) the covariance matrix of those camera rays. + * Result is stored in solution + */ + void computePose( + const bearingVectors_t & f, + const points_t & p, + const cov3_mats_t & covMats, + const std::vector& indices, + transformation_t & result); + + void mlpnp_gn(Eigen::VectorXd& x, + const points_t& pts, + const std::vector& nullspaces, + const Eigen::SparseMatrix Kll, + bool use_cov); + + void mlpnp_residuals_and_jacs( + const Eigen::VectorXd& x, + const points_t& pts, + const std::vector& nullspaces, + Eigen::VectorXd& r, + Eigen::MatrixXd& fjac, + bool getJacs); + + void mlpnpJacs( + const point_t& pt, + const Eigen::Vector3d& nullspace_r, + const Eigen::Vector3d& nullspace_s, + const rodrigues_t& w, + const translation_t& t, + Eigen::MatrixXd& jacs); + + //Auxiliar methods + + /** + * \brief Compute a rotation matrix from Rodrigues axis angle. + * + * \param[in] omega The Rodrigues-parameters of a rotation. + * \return The 3x3 rotation matrix. + */ + Eigen::Matrix3d rodrigues2rot(const Eigen::Vector3d & omega); + + /** + * \brief Compute the Rodrigues-parameters of a rotation matrix. + * + * \param[in] R The 3x3 rotation matrix. + * \return The Rodrigues-parameters. + */ + Eigen::Vector3d rot2rodrigues(const Eigen::Matrix3d & R); + + //---------------------------------------------------- + //Fields of the solver + //---------------------------------------------------- + vector mvpMapPointMatches; + + // 2D Points + vector mvP2D; + //Substitued by bearing vectors + bearingVectors_t mvBearingVecs; + + vector mvSigma2; + + // 3D Points + //vector mvP3Dw; + points_t mvP3Dw; + + // Index in Frame + vector mvKeyPointIndices; + + // Current Estimation + double mRi[3][3]; + double mti[3]; + Eigen::Matrix4f mTcwi; + vector mvbInliersi; + int mnInliersi; + + // Current Ransac State + int mnIterations; + vector mvbBestInliers; + int mnBestInliers; + Eigen::Matrix4f mBestTcw; + + // Refined + Eigen::Matrix4f mRefinedTcw; + vector mvbRefinedInliers; + int mnRefinedInliers; + + // Number of Correspondences + int N; + + // Indices for random selection [0 .. N-1] + vector mvAllIndices; + + // RANSAC probability + double mRansacProb; + + // RANSAC min inliers + int mRansacMinInliers; + + // RANSAC max iterations + int mRansacMaxIts; + + // RANSAC expected inliers/total ratio + float mRansacEpsilon; + + // RANSAC Threshold inlier/outlier. Max error e = dist(P1,T_12*P2)^2 + float mRansacTh; + + // RANSAC Minimun Set used at each iteration + int mRansacMinSet; + + // Max square error associated with scale level. Max error = th*th*sigma(level)*sigma(level) + vector mvMaxError; + + GeometricCamera* mpCamera; + }; + +} + + + + +#endif //ORB_SLAM3_MLPNPSOLVER_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/Map.h b/ORB-SLAM3-UESTC/Workspace/include/Map.h new file mode 100644 index 0000000..0e0a49c --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/Map.h @@ -0,0 +1,208 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef MAP_H +#define MAP_H + +#include "MapPoint.h" +#include "KeyFrame.h" + +#include +#include +#include + +#include + + +namespace ORB_SLAM3 +{ + +class MapPoint; +class KeyFrame; +class Atlas; +class KeyFrameDatabase; + +class Map +{ + friend class boost::serialization::access; + + template + void serialize(Archive &ar, const unsigned int version) + { + ar & mnId; + ar & mnInitKFid; + ar & mnMaxKFid; + ar & mnBigChangeIdx; + + // Save/load a set structure, the set structure is broken in libboost 1.58 for ubuntu 16.04, a vector is serializated + //ar & mspKeyFrames; + //ar & mspMapPoints; + ar & mvpBackupKeyFrames; + ar & mvpBackupMapPoints; + + ar & mvBackupKeyFrameOriginsId; + + ar & mnBackupKFinitialID; + ar & mnBackupKFlowerID; + + ar & mbImuInitialized; + ar & mbIsInertial; + ar & mbIMU_BA1; + ar & mbIMU_BA2; + } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Map(); + Map(int initKFid); + ~Map(); + + void AddKeyFrame(KeyFrame* pKF); + void AddMapPoint(MapPoint* pMP); + void EraseMapPoint(MapPoint* pMP); + void EraseKeyFrame(KeyFrame* pKF); + void SetReferenceMapPoints(const std::vector &vpMPs); + void InformNewBigChange(); + int GetLastBigChangeIdx(); + + std::vector GetAllKeyFrames(); + std::vector GetAllMapPoints(); + std::vector GetReferenceMapPoints(); + + long unsigned int MapPointsInMap(); + long unsigned KeyFramesInMap(); + + long unsigned int GetId(); + + long unsigned int GetInitKFid(); + void SetInitKFid(long unsigned int initKFif); + long unsigned int GetMaxKFid(); + + KeyFrame* GetOriginKF(); + + void SetCurrentMap(); + void SetStoredMap(); + + bool HasThumbnail(); + bool IsInUse(); + + void SetBad(); + bool IsBad(); + + void clear(); + + int GetMapChangeIndex(); + void IncreaseChangeIndex(); + int GetLastMapChange(); + void SetLastMapChange(int currentChangeId); + + void SetImuInitialized(); + bool isImuInitialized(); + + void ApplyScaledRotation(const Sophus::SE3f &T, const float s, const bool bScaledVel=false); + + void SetInertialSensor(); + bool IsInertial(); + void SetIniertialBA1(); + void SetIniertialBA2(); + bool GetIniertialBA1(); + bool GetIniertialBA2(); + + void PrintEssentialGraph(); + bool CheckEssentialGraph(); + void ChangeId(long unsigned int nId); + + unsigned int GetLowerKFID(); + + void PreSave(std::set &spCams); + void PostLoad(KeyFrameDatabase* pKFDB, ORBVocabulary* pORBVoc/*, map& mpKeyFrameId*/, map &mpCams); + + void printReprojectionError(list &lpLocalWindowKFs, KeyFrame* mpCurrentKF, string &name, string &name_folder); + + vector mvpKeyFrameOrigins; + vector mvBackupKeyFrameOriginsId; + KeyFrame* mpFirstRegionKF; + std::mutex mMutexMapUpdate; + + // This avoid that two points are created simultaneously in separate threads (id conflict) + std::mutex mMutexPointCreation; + + bool mbFail; + + // Size of the thumbnail (always in power of 2) + static const int THUMB_WIDTH = 512; + static const int THUMB_HEIGHT = 512; + + static long unsigned int nNextId; + + // DEBUG: show KFs which are used in LBA + std::set msOptKFs; + std::set msFixedKFs; + +protected: + + long unsigned int mnId; + + std::set mspMapPoints; + std::set mspKeyFrames; + + // Save/load, the set structure is broken in libboost 1.58 for ubuntu 16.04, a vector is serializated + std::vector mvpBackupMapPoints; + std::vector mvpBackupKeyFrames; + + KeyFrame* mpKFinitial; + KeyFrame* mpKFlowerID; + + unsigned long int mnBackupKFinitialID; + unsigned long int mnBackupKFlowerID; + + std::vector mvpReferenceMapPoints; + + bool mbImuInitialized; + + int mnMapChange; + int mnMapChangeNotified; + + long unsigned int mnInitKFid; + long unsigned int mnMaxKFid; + //long unsigned int mnLastLoopKFid; + + // Index related to a big change in the map (loop closure, global BA) + int mnBigChangeIdx; + + + // View of the map in aerial sight (for the AtlasViewer) + GLubyte* mThumbnail; + + bool mIsInUse; + bool mHasTumbnail; + bool mbBad = false; + + bool mbIsInertial; + bool mbIMU_BA1; + bool mbIMU_BA2; + + // Mutex + std::mutex mMutexMap; + +}; + +} //namespace ORB_SLAM3 + +#endif // MAP_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/MapDrawer.h b/ORB-SLAM3-UESTC/Workspace/include/MapDrawer.h new file mode 100644 index 0000000..635fb21 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/MapDrawer.h @@ -0,0 +1,79 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef MAPDRAWER_H +#define MAPDRAWER_H + +#include"Atlas.h" +#include"MapPoint.h" +#include"KeyFrame.h" +#include "Settings.h" +#include + +#include + +namespace ORB_SLAM3 +{ + +class Settings; + +class MapDrawer +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + MapDrawer(Atlas* pAtlas, const string &strSettingPath, Settings* settings); + + void newParameterLoader(Settings* settings); + + Atlas* mpAtlas; + + void DrawMapPoints(); + void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const bool bDrawInertialGraph, const bool bDrawOptLba); + void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc); + void SetCurrentCameraPose(const Sophus::SE3f &Tcw); + void SetReferenceKeyFrame(KeyFrame *pKF); + void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw); + +private: + + bool ParseViewerParamFile(cv::FileStorage &fSettings); + + float mKeyFrameSize; + float mKeyFrameLineWidth; + float mGraphLineWidth; + float mPointSize; + float mCameraSize; + float mCameraLineWidth; + + Sophus::SE3f mCameraPose; + + std::mutex mMutexCamera; + + float mfFrameColors[6][3] = {{0.0f, 0.0f, 1.0f}, + {0.8f, 0.4f, 1.0f}, + {1.0f, 0.2f, 0.4f}, + {0.6f, 0.0f, 1.0f}, + {1.0f, 1.0f, 0.0f}, + {0.0f, 1.0f, 1.0f}}; + +}; + +} //namespace ORB_SLAM + +#endif // MAPDRAWER_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/MapPoint.h b/ORB-SLAM3-UESTC/Workspace/include/MapPoint.h new file mode 100644 index 0000000..51762d4 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/MapPoint.h @@ -0,0 +1,256 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef MAPPOINT_H +#define MAPPOINT_H + +#include "KeyFrame.h" +#include "Frame.h" +#include "Map.h" +#include "Converter.h" + +#include "SerializationUtils.h" + +#include +#include + +#include +#include +#include + +namespace ORB_SLAM3 +{ + +class KeyFrame; +class Map; +class Frame; + +class MapPoint +{ + + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & mnId; + ar & mnFirstKFid; + ar & mnFirstFrame; + ar & nObs; + // Variables used by the tracking + //ar & mTrackProjX; + //ar & mTrackProjY; + //ar & mTrackDepth; + //ar & mTrackDepthR; + //ar & mTrackProjXR; + //ar & mTrackProjYR; + //ar & mbTrackInView; + //ar & mbTrackInViewR; + //ar & mnTrackScaleLevel; + //ar & mnTrackScaleLevelR; + //ar & mTrackViewCos; + //ar & mTrackViewCosR; + //ar & mnTrackReferenceForFrame; + //ar & mnLastFrameSeen; + + // Variables used by local mapping + //ar & mnBALocalForKF; + //ar & mnFuseCandidateForKF; + + // Variables used by loop closing and merging + //ar & mnLoopPointForKF; + //ar & mnCorrectedByKF; + //ar & mnCorrectedReference; + //serializeMatrix(ar,mPosGBA,version); + //ar & mnBAGlobalForKF; + //ar & mnBALocalForMerge; + //serializeMatrix(ar,mPosMerge,version); + //serializeMatrix(ar,mNormalVectorMerge,version); + + // Protected variables + ar & boost::serialization::make_array(mWorldPos.data(), mWorldPos.size()); + ar & boost::serialization::make_array(mNormalVector.data(), mNormalVector.size()); + //ar & BOOST_SERIALIZATION_NVP(mBackupObservationsId); + //ar & mObservations; + ar & mBackupObservationsId1; + ar & mBackupObservationsId2; + serializeMatrix(ar,mDescriptor,version); + ar & mBackupRefKFId; + //ar & mnVisible; + //ar & mnFound; + + ar & mbBad; + ar & mBackupReplacedId; + + ar & mfMinDistance; + ar & mfMaxDistance; + + } + + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + MapPoint(); + + MapPoint(const Eigen::Vector3f &Pos, KeyFrame* pRefKF, Map* pMap); + MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF, KeyFrame* pHostKF, Map* pMap); + MapPoint(const Eigen::Vector3f &Pos, Map* pMap, Frame* pFrame, const int &idxF); + + void SetWorldPos(const Eigen::Vector3f &Pos); + Eigen::Vector3f GetWorldPos(); + + Eigen::Vector3f GetNormal(); + void SetNormalVector(const Eigen::Vector3f& normal); + + KeyFrame* GetReferenceKeyFrame(); + + std::map> GetObservations(); + int Observations(); + + void AddObservation(KeyFrame* pKF,int idx); + void EraseObservation(KeyFrame* pKF, bool erase = true); + + std::tuple GetIndexInKeyFrame(KeyFrame* pKF); + bool IsInKeyFrame(KeyFrame* pKF); + + void SetBadFlag(bool erase = true); + bool isBad(); + + void Replace(MapPoint* pMP); + MapPoint* GetReplaced(); + + void IncreaseVisible(int n=1); + void IncreaseFound(int n=1); + float GetFoundRatio(); + inline int GetFound(){ + return mnFound; + } + + void ComputeDistinctiveDescriptors(); + + cv::Mat GetDescriptor(); + + void UpdateNormalAndDepth(); + + float GetMinDistanceInvariance(); + float GetMaxDistanceInvariance(); + int PredictScale(const float ¤tDist, KeyFrame*pKF); + int PredictScale(const float ¤tDist, Frame* pF); + + Map* GetMap(); + void UpdateMap(Map* pMap); + + void PrintObservations(); + + void PreSave(set& spKF,set& spMP); + void PostLoad(map& mpKFid, map& mpMPid); + +public: + long unsigned int mnId; + static long unsigned int nNextId; + long int mnFirstKFid; + long int mnFirstFrame; + int nObs; + + // Variables used by the tracking + float mTrackProjX; + float mTrackProjY; + float mTrackDepth; + float mTrackDepthR; + float mTrackProjXR; + float mTrackProjYR; + bool mbTrackInView, mbTrackInViewR; + int mnTrackScaleLevel, mnTrackScaleLevelR; + float mTrackViewCos, mTrackViewCosR; + long unsigned int mnTrackReferenceForFrame; + long unsigned int mnLastFrameSeen; + + // Variables used by local mapping + long unsigned int mnBALocalForKF; + long unsigned int mnFuseCandidateForKF; + + // Variables used by loop closing + long unsigned int mnLoopPointForKF; + long unsigned int mnCorrectedByKF; + long unsigned int mnCorrectedReference; + Eigen::Vector3f mPosGBA; + long unsigned int mnBAGlobalForKF; + long unsigned int mnBALocalForMerge; + + // Variable used by merging + Eigen::Vector3f mPosMerge; + Eigen::Vector3f mNormalVectorMerge; + + + // Fopr inverse depth optimization + double mInvDepth; + double mInitU; + double mInitV; + KeyFrame* mpHostKF; + + static std::mutex mGlobalMutex; + + unsigned int mnOriginMapId; + +protected: + + // Position in absolute coordinates + Eigen::Vector3f mWorldPos; + + // Keyframes observing the point and associated index in keyframe + std::map > mObservations; + // For save relation without pointer, this is necessary for save/load function + std::map mBackupObservationsId1; + std::map mBackupObservationsId2; + + // Mean viewing direction + Eigen::Vector3f mNormalVector; + + // Best descriptor to fast matching + cv::Mat mDescriptor; + + // Reference KeyFrame + KeyFrame* mpRefKF; + long unsigned int mBackupRefKFId; + + // Tracking counters + int mnVisible; + int mnFound; + + // Bad flag (we do not currently erase MapPoint from memory) + bool mbBad; + MapPoint* mpReplaced; + // For save relation without pointer, this is necessary for save/load function + long long int mBackupReplacedId; + + // Scale invariance distances + float mfMinDistance; + float mfMaxDistance; + + Map* mpMap; + + // Mutex + std::mutex mMutexPos; + std::mutex mMutexFeatures; + std::mutex mMutexMap; + +}; + +} //namespace ORB_SLAM + +#endif // MAPPOINT_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/ORBVocabulary.h b/ORB-SLAM3-UESTC/Workspace/include/ORBVocabulary.h new file mode 100644 index 0000000..71628b8 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/ORBVocabulary.h @@ -0,0 +1,34 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef ORBVOCABULARY_H +#define ORBVOCABULARY_H + +#include"Thirdparty/DBoW2/DBoW2/FORB.h" +#include"Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h" + +namespace ORB_SLAM3 +{ + +typedef DBoW2::TemplatedVocabulary + ORBVocabulary; + +} //namespace ORB_SLAM + +#endif // ORBVOCABULARY_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/ORBextractor.h b/ORB-SLAM3-UESTC/Workspace/include/ORBextractor.h new file mode 100644 index 0000000..bb7aace --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/ORBextractor.h @@ -0,0 +1,114 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#ifndef ORBEXTRACTOR_H +#define ORBEXTRACTOR_H + +#include +#include +#include + + +namespace ORB_SLAM3 +{ + +class ExtractorNode +{ +public: + ExtractorNode():bNoMore(false){} + + void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4); + + std::vector vKeys; + cv::Point2i UL, UR, BL, BR; + std::list::iterator lit; + bool bNoMore; +}; + +class ORBextractor +{ +public: + + enum {HARRIS_SCORE=0, FAST_SCORE=1 }; + + ORBextractor(int nfeatures, float scaleFactor, int nlevels, + int iniThFAST, int minThFAST); + + ~ORBextractor(){} + + // Compute the ORB features and descriptors on an image. + // ORB are dispersed on the image using an octree. + // Mask is ignored in the current implementation. + int operator()( cv::InputArray _image, cv::InputArray _mask, + std::vector& _keypoints, + cv::OutputArray _descriptors, std::vector &vLappingArea); + + int inline GetLevels(){ + return nlevels;} + + float inline GetScaleFactor(){ + return scaleFactor;} + + std::vector inline GetScaleFactors(){ + return mvScaleFactor; + } + + std::vector inline GetInverseScaleFactors(){ + return mvInvScaleFactor; + } + + std::vector inline GetScaleSigmaSquares(){ + return mvLevelSigma2; + } + + std::vector inline GetInverseScaleSigmaSquares(){ + return mvInvLevelSigma2; + } + + std::vector mvImagePyramid; + +protected: + + void ComputePyramid(cv::Mat image); + void ComputeKeyPointsOctTree(std::vector >& allKeypoints); + std::vector DistributeOctTree(const std::vector& vToDistributeKeys, const int &minX, + const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level); + + void ComputeKeyPointsOld(std::vector >& allKeypoints); + std::vector pattern; + + int nfeatures; + double scaleFactor; + int nlevels; + int iniThFAST; + int minThFAST; + + std::vector mnFeaturesPerLevel; + + std::vector umax; + + std::vector mvScaleFactor; + std::vector mvInvScaleFactor; + std::vector mvLevelSigma2; + std::vector mvInvLevelSigma2; +}; + +} //namespace ORB_SLAM + +#endif + diff --git a/ORB-SLAM3-UESTC/Workspace/include/ORBmatcher.h b/ORB-SLAM3-UESTC/Workspace/include/ORBmatcher.h new file mode 100644 index 0000000..92bedd7 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/ORBmatcher.h @@ -0,0 +1,107 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef ORBMATCHER_H +#define ORBMATCHER_H + +#include +#include +#include +#include"sophus/sim3.hpp" + +#include"MapPoint.h" +#include"KeyFrame.h" +#include"Frame.h" + + +namespace ORB_SLAM3 +{ + + class ORBmatcher + { + public: + + ORBmatcher(float nnratio=0.6, bool checkOri=true); + + // Computes the Hamming distance between two ORB descriptors + static int DescriptorDistance(const cv::Mat &a, const cv::Mat &b); + + // Search matches between Frame keypoints and projected MapPoints. Returns number of matches + // Used to track the local map (Tracking) + int SearchByProjection(Frame &F, const std::vector &vpMapPoints, const float th=3, const bool bFarPoints = false, const float thFarPoints = 50.0f); + + // Project MapPoints tracked in last frame into the current frame and search matches. + // Used to track from previous frame (Tracking) + int SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono); + + // Project MapPoints seen in KeyFrame into the Frame and search matches. + // Used in relocalisation (Tracking) + int SearchByProjection(Frame &CurrentFrame, KeyFrame* pKF, const std::set &sAlreadyFound, const float th, const int ORBdist); + + // Project MapPoints using a Similarity Transformation and search matches. + // Used in loop detection (Loop Closing) + int SearchByProjection(KeyFrame* pKF, Sophus::Sim3 &Scw, const std::vector &vpPoints, std::vector &vpMatched, int th, float ratioHamming=1.0); + + // Project MapPoints using a Similarity Transformation and search matches. + // Used in Place Recognition (Loop Closing and Merging) + int SearchByProjection(KeyFrame* pKF, Sophus::Sim3 &Scw, const std::vector &vpPoints, const std::vector &vpPointsKFs, std::vector &vpMatched, std::vector &vpMatchedKF, int th, float ratioHamming=1.0); + + // Search matches between MapPoints in a KeyFrame and ORB in a Frame. + // Brute force constrained to ORB that belong to the same vocabulary node (at a certain level) + // Used in Relocalisation and Loop Detection + int SearchByBoW(KeyFrame *pKF, Frame &F, std::vector &vpMapPointMatches); + int SearchByBoW(KeyFrame *pKF1, KeyFrame* pKF2, std::vector &vpMatches12); + + // Matching for the Map Initialization (only used in the monocular case) + int SearchForInitialization(Frame &F1, Frame &F2, std::vector &vbPrevMatched, std::vector &vnMatches12, int windowSize=10); + + // Matching to triangulate new MapPoints. Check Epipolar Constraint. + int SearchForTriangulation(KeyFrame *pKF1, KeyFrame* pKF2, + std::vector > &vMatchedPairs, const bool bOnlyStereo, const bool bCoarse = false); + + // Search matches between MapPoints seen in KF1 and KF2 transforming by a Sim3 [s12*R12|t12] + // In the stereo and RGB-D case, s12=1 + // int SearchBySim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches12, const float &s12, const cv::Mat &R12, const cv::Mat &t12, const float th); + int SearchBySim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches12, const Sophus::Sim3f &S12, const float th); + + // Project MapPoints into KeyFrame and search for duplicated MapPoints. + int Fuse(KeyFrame* pKF, const vector &vpMapPoints, const float th=3.0, const bool bRight = false); + + // Project MapPoints into KeyFrame using a given Sim3 and search for duplicated MapPoints. + int Fuse(KeyFrame* pKF, Sophus::Sim3f &Scw, const std::vector &vpPoints, float th, vector &vpReplacePoint); + + public: + + static const int TH_LOW; + static const int TH_HIGH; + static const int HISTO_LENGTH; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + protected: + float RadiusByViewingCos(const float &viewCos); + + void ComputeThreeMaxima(std::vector* histo, const int L, int &ind1, int &ind2, int &ind3); + + float mfNNratio; + bool mbCheckOrientation; + }; + +}// namespace ORB_SLAM + +#endif // ORBMATCHER_H \ No newline at end of file diff --git a/ORB-SLAM3-UESTC/Workspace/include/OptimizableTypes.h b/ORB-SLAM3-UESTC/Workspace/include/OptimizableTypes.h new file mode 100644 index 0000000..9b1722f --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/OptimizableTypes.h @@ -0,0 +1,239 @@ +/** + * This file is part of ORB-SLAM3 + * + * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * + * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public + * License as published by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even + * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with ORB-SLAM3. + * If not, see . + */ + +#ifndef ORB_SLAM3_OPTIMIZABLETYPES_H +#define ORB_SLAM3_OPTIMIZABLETYPES_H + +#include "Thirdparty/g2o/g2o/core/base_unary_edge.h" +#include +#include + +#include +#include + +namespace ORB_SLAM3 +{ +// 左目纯位姿优化的边，左目点的重投影误差相对于左目位姿 +class EdgeSE3ProjectXYZOnlyPose : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EdgeSE3ProjectXYZOnlyPose() {} + + bool read(std::istream &is); + + bool write(std::ostream &os) const; + + void computeError() + { + const g2o::VertexSE3Expmap *v1 = static_cast(_vertices[0]); + Eigen::Vector2d obs(_measurement); + _error = obs - pCamera->project(v1->estimate().map(Xw)); + } + + bool isDepthPositive() + { + const g2o::VertexSE3Expmap *v1 = static_cast(_vertices[0]); + return (v1->estimate().map(Xw))(2) > 0.0; + } + + virtual void linearizeOplus(); + + Eigen::Vector3d Xw; + GeometricCamera *pCamera; +}; + +// 两个相机中的右目上的重投影误差与左目位姿的边 +class EdgeSE3ProjectXYZOnlyPoseToBody : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EdgeSE3ProjectXYZOnlyPoseToBody() {} + + bool read(std::istream &is); + + bool write(std::ostream &os) const; + + void computeError() + { + const g2o::VertexSE3Expmap *v1 = static_cast(_vertices[0]); + Eigen::Vector2d obs(_measurement); + _error = obs - pCamera->project((mTrl * v1->estimate()).map(Xw)); + } + + bool isDepthPositive() + { + const g2o::VertexSE3Expmap *v1 = static_cast(_vertices[0]); + return ((mTrl * v1->estimate()).map(Xw))(2) > 0.0; + } + + virtual void linearizeOplus(); + + Eigen::Vector3d Xw; + GeometricCamera *pCamera; + + g2o::SE3Quat mTrl; +}; + + +// 左目纯位姿优化的边，左目点的重投影误差相对于左目位姿以及三维点 +class EdgeSE3ProjectXYZ : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EdgeSE3ProjectXYZ(); + + bool read(std::istream &is); + + bool write(std::ostream &os) const; + + void computeError() + { + const g2o::VertexSE3Expmap *v1 = static_cast(_vertices[1]); + const g2o::VertexSBAPointXYZ *v2 = static_cast(_vertices[0]); + Eigen::Vector2d obs(_measurement); + _error = obs - pCamera->project(v1->estimate().map(v2->estimate())); + } + + bool isDepthPositive() + { + const g2o::VertexSE3Expmap *v1 = static_cast(_vertices[1]); + const g2o::VertexSBAPointXYZ *v2 = static_cast(_vertices[0]); + return ((v1->estimate().map(v2->estimate()))(2) > 0.0); + } + + virtual void linearizeOplus(); + + GeometricCamera *pCamera; +}; + +// 两个相机中的右目上的重投影误差与左目位姿以及三维点的边 +class EdgeSE3ProjectXYZToBody : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EdgeSE3ProjectXYZToBody(); + + bool read(std::istream &is); + + bool write(std::ostream &os) const; + + void computeError() + { + const g2o::VertexSE3Expmap *v1 = static_cast(_vertices[1]); + const g2o::VertexSBAPointXYZ *v2 = static_cast(_vertices[0]); + Eigen::Vector2d obs(_measurement); + _error = obs - pCamera->project((mTrl * v1->estimate()).map(v2->estimate())); + } + + bool isDepthPositive() + { + const g2o::VertexSE3Expmap *v1 = static_cast(_vertices[1]); + const g2o::VertexSBAPointXYZ *v2 = static_cast(_vertices[0]); + return ((mTrl * v1->estimate()).map(v2->estimate()))(2) > 0.0; + } + + virtual void linearizeOplus(); + + GeometricCamera *pCamera; + g2o::SE3Quat mTrl; +}; + +// sim3节点 +class VertexSim3Expmap : public g2o::BaseVertex<7, g2o::Sim3> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + VertexSim3Expmap(); + virtual bool read(std::istream &is); + virtual bool write(std::ostream &os) const; + + // 原始值 + virtual void setToOriginImpl() + { + _estimate = g2o::Sim3(); + } + + // 更新 + virtual void oplusImpl(const double *update_) + { + Eigen::Map update(const_cast(update_)); + + if (_fix_scale) + update[6] = 0; + + g2o::Sim3 s(update); + setEstimate(s * estimate()); + } + + GeometricCamera *pCamera1, *pCamera2; + + bool _fix_scale; +}; + +// sim3边 +class EdgeSim3ProjectXYZ : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, ORB_SLAM3::VertexSim3Expmap> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EdgeSim3ProjectXYZ(); + virtual bool read(std::istream &is); + virtual bool write(std::ostream &os) const; + + void computeError() + { + const ORB_SLAM3::VertexSim3Expmap *v1 = static_cast(_vertices[1]); + const g2o::VertexSBAPointXYZ *v2 = static_cast(_vertices[0]); + + Eigen::Vector2d obs(_measurement); + _error = obs - v1->pCamera1->project(v1->estimate().map(v2->estimate())); + } + + // 自动求导，没错g2o也有自动求导 + // virtual void linearizeOplus(); +}; + +// sim3反投的边 +class EdgeInverseSim3ProjectXYZ : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, VertexSim3Expmap> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EdgeInverseSim3ProjectXYZ(); + virtual bool read(std::istream &is); + virtual bool write(std::ostream &os) const; + + void computeError() + { + const ORB_SLAM3::VertexSim3Expmap *v1 = static_cast(_vertices[1]); + const g2o::VertexSBAPointXYZ *v2 = static_cast(_vertices[0]); + + Eigen::Vector2d obs(_measurement); + _error = obs - v1->pCamera2->project((v1->estimate().inverse().map(v2->estimate()))); + } + + // 自动求导，没错g2o也有自动求导 + // virtual void linearizeOplus(); +}; + +} + +#endif // ORB_SLAM3_OPTIMIZABLETYPES_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/Optimizer.h b/ORB-SLAM3-UESTC/Workspace/include/Optimizer.h new file mode 100644 index 0000000..5013f9b --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/Optimizer.h @@ -0,0 +1,106 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef OPTIMIZER_H +#define OPTIMIZER_H + +#include "Map.h" +#include "MapPoint.h" +#include "KeyFrame.h" +#include "LoopClosing.h" +#include "Frame.h" + +#include + +#include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" +#include "Thirdparty/g2o/g2o/core/sparse_block_matrix.h" +#include "Thirdparty/g2o/g2o/core/block_solver.h" +#include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h" +#include "Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h" +#include "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h" +#include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" +#include "Thirdparty/g2o/g2o/core/robust_kernel_impl.h" +#include "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h" + +namespace ORB_SLAM3 +{ + +class LoopClosing; + +class Optimizer +{ +public: + + void static BundleAdjustment(const std::vector &vpKF, const std::vector &vpMP, + int nIterations = 5, bool *pbStopFlag=NULL, const unsigned long nLoopKF=0, + const bool bRobust = true); + void static GlobalBundleAdjustemnt(Map* pMap, int nIterations=5, bool *pbStopFlag=NULL, + const unsigned long nLoopKF=0, const bool bRobust = true); + void static FullInertialBA(Map *pMap, int its, const bool bFixLocal=false, const unsigned long nLoopKF=0, bool *pbStopFlag=NULL, bool bInit=false, float priorG = 1e2, float priorA=1e6, Eigen::VectorXd *vSingVal = NULL, bool *bHess=NULL); + + void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, Map *pMap, int& num_fixedKF, int& num_OptKF, int& num_MPs, int& num_edges); + + int static PoseOptimization(Frame* pFrame); + int static PoseInertialOptimizationLastKeyFrame(Frame* pFrame, bool bRecInit = false); + int static PoseInertialOptimizationLastFrame(Frame *pFrame, bool bRecInit = false); + + // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise (mono) + void static OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, + const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, + const LoopClosing::KeyFrameAndPose &CorrectedSim3, + const map > &LoopConnections, + const bool &bFixScale); + void static OptimizeEssentialGraph(KeyFrame* pCurKF, vector &vpFixedKFs, vector &vpFixedCorrectedKFs, + vector &vpNonFixedKFs, vector &vpNonCorrectedMPs); + + // For inertial loopclosing + void static OptimizeEssentialGraph4DoF(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, + const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, + const LoopClosing::KeyFrameAndPose &CorrectedSim3, + const map > &LoopConnections); + + + // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono) (NEW) + static int OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches1, + g2o::Sim3 &g2oS12, const float th2, const bool bFixScale, + Eigen::Matrix &mAcumHessian, const bool bAllPoints=false); + + // For inertial systems + + void static LocalInertialBA(KeyFrame* pKF, bool *pbStopFlag, Map *pMap, int& num_fixedKF, int& num_OptKF, int& num_MPs, int& num_edges, bool bLarge = false, bool bRecInit = false); + void static MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbStopFlag, Map *pMap, LoopClosing::KeyFrameAndPose &corrPoses); + + // Local BA in welding area when two maps are merged + void static LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdjustKF, vector vpFixedKF, bool *pbStopFlag); + + // Marginalize block element (start:end,start:end). Perform Schur complement. + // Marginalized elements are filled with zeros. + static Eigen::MatrixXd Marginalize(const Eigen::MatrixXd &H, const int &start, const int &end); + + // Inertial pose-graph + void static InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale, Eigen::Vector3d &bg, Eigen::Vector3d &ba, bool bMono, Eigen::MatrixXd &covInertial, bool bFixedVel=false, bool bGauss=false, float priorG = 1e2, float priorA = 1e6); + void static InertialOptimization(Map *pMap, Eigen::Vector3d &bg, Eigen::Vector3d &ba, float priorG = 1e2, float priorA = 1e6); + void static InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale); + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; +}; + +} //namespace ORB_SLAM3 + +#endif // OPTIMIZER_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/PnPsolver.h b/ORB-SLAM3-UESTC/Workspace/include/PnPsolver.h new file mode 100644 index 0000000..c10a141 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/PnPsolver.h @@ -0,0 +1,195 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +/** +* Copyright (c) 2009, V. Lepetit, EPFL +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +* +* The views and conclusions contained in the software and documentation are those +* of the authors and should not be interpreted as representing official policies, +* either expressed or implied, of the FreeBSD Project +*/ + +#ifndef PNPSOLVER_H +#define PNPSOLVER_H + +#include +#include "MapPoint.h" +#include "Frame.h" + +namespace ORB_SLAM3 +{ + +class PnPsolver { + public: + PnPsolver(const Frame &F, const vector &vpMapPointMatches); + + ~PnPsolver(); + + void SetRansacParameters(double probability = 0.99, int minInliers = 8 , int maxIterations = 300, int minSet = 4, float epsilon = 0.4, + float th2 = 5.991); + + cv::Mat find(vector &vbInliers, int &nInliers); + + cv::Mat iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers); + + private: + + void CheckInliers(); + bool Refine(); + + // Functions from the original EPnP code + void set_maximum_number_of_correspondences(const int n); + void reset_correspondences(void); + void add_correspondence(const double X, const double Y, const double Z, + const double u, const double v); + + double compute_pose(double R[3][3], double T[3]); + + void relative_error(double & rot_err, double & transl_err, + const double Rtrue[3][3], const double ttrue[3], + const double Rest[3][3], const double test[3]); + + void print_pose(const double R[3][3], const double t[3]); + double reprojection_error(const double R[3][3], const double t[3]); + + void choose_control_points(void); + void compute_barycentric_coordinates(void); + void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v); + void compute_ccs(const double * betas, const double * ut); + void compute_pcs(void); + + void solve_for_sign(void); + + void find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, double * betas); + void find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, double * betas); + void find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, double * betas); + void qr_solve(CvMat * A, CvMat * b, CvMat * X); + + double dot(const double * v1, const double * v2); + double dist2(const double * p1, const double * p2); + + void compute_rho(double * rho); + void compute_L_6x10(const double * ut, double * l_6x10); + + void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]); + void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho, + double cb[4], CvMat * A, CvMat * b); + + double compute_R_and_t(const double * ut, const double * betas, + double R[3][3], double t[3]); + + void estimate_R_and_t(double R[3][3], double t[3]); + + void copy_R_and_t(const double R_dst[3][3], const double t_dst[3], + double R_src[3][3], double t_src[3]); + + void mat_to_quat(const double R[3][3], double q[4]); + + + double uc, vc, fu, fv; + + double * pws, * us, * alphas, * pcs; + int maximum_number_of_correspondences; + int number_of_correspondences; + + double cws[4][3], ccs[4][3]; + double cws_determinant; + + vector mvpMapPointMatches; + + // 2D Points + vector mvP2D; + vector mvSigma2; + + // 3D Points + vector mvP3Dw; + + // Index in Frame + vector mvKeyPointIndices; + + // Current Estimation + double mRi[3][3]; + double mti[3]; + cv::Mat mTcwi; + vector mvbInliersi; + int mnInliersi; + + // Current Ransac State + int mnIterations; + vector mvbBestInliers; + int mnBestInliers; + cv::Mat mBestTcw; + + // Refined + cv::Mat mRefinedTcw; + vector mvbRefinedInliers; + int mnRefinedInliers; + + // Number of Correspondences + int N; + + // Indices for random selection [0 .. N-1] + vector mvAllIndices; + + // RANSAC probability + double mRansacProb; + + // RANSAC min inliers + int mRansacMinInliers; + + // RANSAC max iterations + int mRansacMaxIts; + + // RANSAC expected inliers/total ratio + float mRansacEpsilon; + + // RANSAC Threshold inlier/outlier. Max error e = dist(P1,T_12*P2)^2 + float mRansacTh; + + // RANSAC Minimun Set used at each iteration + int mRansacMinSet; + + // Max square error associated with scale level. Max error = th*th*sigma(level)*sigma(level) + vector mvMaxError; + +}; + +} //namespace ORB_SLAM + +#endif //PNPSOLVER_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/SerializationUtils.h b/ORB-SLAM3-UESTC/Workspace/include/SerializationUtils.h new file mode 100644 index 0000000..814d325 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/SerializationUtils.h @@ -0,0 +1,166 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#ifndef SERIALIZATION_UTILS_H +#define SERIALIZATION_UTILS_H + +#include +#include + +#include +#include + +#include +#include + +#include + +namespace ORB_SLAM3 +{ + +template +void serializeSophusSE3(Archive &ar, Sophus::SE3f &T, const unsigned int version) +{ + Eigen::Vector4f quat; + Eigen::Vector3f transl; + + if (Archive::is_saving::value) + { + Eigen::Quaternionf q = T.unit_quaternion(); + quat << q.w(), q.x(), q.y(), q.z(); + transl = T.translation(); + } + + ar & boost::serialization::make_array(quat.data(), quat.size()); + ar & boost::serialization::make_array(transl.data(), transl.size()); + + if (Archive::is_loading::value) + { + Eigen::Quaternionf q(quat[0], quat[1], quat[2], quat[3]); + T = Sophus::SE3f(q, transl); + } +} + +/*template +void serializeDiagonalMatrix(Archive &ar, Eigen::DiagonalMatrix &D, const unsigned int version) +{ + Eigen::Matrix dense; + if(Archive::is_saving::value) + { + dense = D.toDenseMatrix(); + } + + ar & boost::serialization::make_array(dense.data(), dense.size()); + + if (Archive::is_loading::value) + { + D = dense.diagonal().asDiagonal(); + } +}*/ + +template +void serializeMatrix(Archive& ar, cv::Mat& mat, const unsigned int version) +{ + int cols, rows, type; + bool continuous; + + if (Archive::is_saving::value) { + cols = mat.cols; rows = mat.rows; type = mat.type(); + continuous = mat.isContinuous(); + } + + ar & cols & rows & type & continuous; + + if (Archive::is_loading::value) + mat.create(rows, cols, type); + + if (continuous) { + const unsigned int data_size = rows * cols * mat.elemSize(); + ar & boost::serialization::make_array(mat.ptr(), data_size); + } else { + const unsigned int row_size = cols*mat.elemSize(); + for (int i = 0; i < rows; i++) { + ar & boost::serialization::make_array(mat.ptr(i), row_size); + } + } +} + +template +void serializeMatrix(Archive& ar, const cv::Mat& mat, const unsigned int version) +{ + cv::Mat matAux = mat; + + serializeMatrix(ar, matAux,version); + + if (Archive::is_loading::value) + { + cv::Mat* ptr; + ptr = (cv::Mat*)( &mat ); + *ptr = matAux; + } +} + +template +void serializeVectorKeyPoints(Archive& ar, const std::vector& vKP, const unsigned int version) +{ + int NumEl; + + if (Archive::is_saving::value) { + NumEl = vKP.size(); + } + + ar & NumEl; + + std::vector vKPaux = vKP; + if (Archive::is_loading::value) + vKPaux.reserve(NumEl); + + for(int i=0; i < NumEl; ++i) + { + cv::KeyPoint KPi; + + if (Archive::is_loading::value) + KPi = cv::KeyPoint(); + + if (Archive::is_saving::value) + KPi = vKPaux[i]; + + ar & KPi.angle; + ar & KPi.response; + ar & KPi.size; + ar & KPi.pt.x; + ar & KPi.pt.y; + ar & KPi.class_id; + ar & KPi.octave; + + if (Archive::is_loading::value) + vKPaux.push_back(KPi); + } + + + if (Archive::is_loading::value) + { + std::vector *ptr; + ptr = (std::vector*)( &vKP ); + *ptr = vKPaux; + } +} + +} // namespace ORB_SLAM3 + +#endif // SERIALIZATION_UTILS_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/Settings.h b/ORB-SLAM3-UESTC/Workspace/include/Settings.h new file mode 100644 index 0000000..beab7de --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/Settings.h @@ -0,0 +1,235 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#ifndef ORB_SLAM3_SETTINGS_H +#define ORB_SLAM3_SETTINGS_H + + +// Flag to activate the measurement of time in each process (track,localmap, place recognition). +//#define REGISTER_TIMES + +#include "CameraModels/GeometricCamera.h" + +#include +#include +#include +#include + +namespace ORB_SLAM3 { + + class System; + + //TODO: change to double instead of float + + class Settings { + public: + /* + * Enum for the different camera types implemented + */ + enum CameraType { + PinHole = 0, + Rectified = 1, + KannalaBrandt = 2 + }; + + /* + * Delete default constructor + */ + Settings() = delete; + + /* + * Constructor from file + */ + Settings(const std::string &configFile, const int& sensor); + + /* + * Ostream operator overloading to dump settings to the terminal + */ + friend std::ostream &operator<<(std::ostream &output, const Settings &s); + + /* + * Getter methods + */ + CameraType cameraType() {return cameraType_;} + GeometricCamera* camera1() {return calibration1_;} + GeometricCamera* camera2() {return calibration2_;} + cv::Mat camera1DistortionCoef() {return cv::Mat(vPinHoleDistorsion1_.size(),1,CV_32F,vPinHoleDistorsion1_.data());} + cv::Mat camera2DistortionCoef() {return cv::Mat(vPinHoleDistorsion2_.size(),1,CV_32F,vPinHoleDistorsion1_.data());} + + Sophus::SE3f Tlr() {return Tlr_;} + float bf() {return bf_;} + float b() {return b_;} + float thDepth() {return thDepth_;} + + bool needToUndistort() {return bNeedToUndistort_;} + + cv::Size newImSize() {return newImSize_;} + float fps() {return fps_;} + bool rgb() {return bRGB_;} + bool needToResize() {return bNeedToResize1_;} + bool needToRectify() {return bNeedToRectify_;} + + float noiseGyro() {return noiseGyro_;} + float noiseAcc() {return noiseAcc_;} + float gyroWalk() {return gyroWalk_;} + float accWalk() {return accWalk_;} + float imuFrequency() {return imuFrequency_;} + Sophus::SE3f Tbc() {return Tbc_;} + bool insertKFsWhenLost() {return insertKFsWhenLost_;} + + float depthMapFactor() {return depthMapFactor_;} + + int nFeatures() {return nFeatures_;} + int nLevels() {return nLevels_;} + float initThFAST() {return initThFAST_;} + float minThFAST() {return minThFAST_;} + float scaleFactor() {return scaleFactor_;} + + float keyFrameSize() {return keyFrameSize_;} + float keyFrameLineWidth() {return keyFrameLineWidth_;} + float graphLineWidth() {return graphLineWidth_;} + float pointSize() {return pointSize_;} + float cameraSize() {return cameraSize_;} + float cameraLineWidth() {return cameraLineWidth_;} + float viewPointX() {return viewPointX_;} + float viewPointY() {return viewPointY_;} + float viewPointZ() {return viewPointZ_;} + float viewPointF() {return viewPointF_;} + float imageViewerScale() {return imageViewerScale_;} + + std::string atlasLoadFile() {return sLoadFrom_;} + std::string atlasSaveFile() {return sSaveto_;} + + float thFarPoints() {return thFarPoints_;} + + cv::Mat M1l() {return M1l_;} + cv::Mat M2l() {return M2l_;} + cv::Mat M1r() {return M1r_;} + cv::Mat M2r() {return M2r_;} + + private: + template + T readParameter(cv::FileStorage& fSettings, const std::string& name, bool& found,const bool required = true){ + cv::FileNode node = fSettings[name]; + if(node.empty()){ + if(required){ + std::cerr << name << " required parameter does not exist, aborting..." << std::endl; + exit(-1); + } + else{ + std::cerr << name << " optional parameter does not exist..." << std::endl; + found = false; + return T(); + } + + } + else{ + found = true; + return (T) node; + } + } + + void readCamera1(cv::FileStorage& fSettings); + void readCamera2(cv::FileStorage& fSettings); + void readImageInfo(cv::FileStorage& fSettings); + void readIMU(cv::FileStorage& fSettings); + void readRGBD(cv::FileStorage& fSettings); + void readORB(cv::FileStorage& fSettings); + void readViewer(cv::FileStorage& fSettings); + void readLoadAndSave(cv::FileStorage& fSettings); + void readOtherParameters(cv::FileStorage& fSettings); + + void precomputeRectificationMaps(); + + int sensor_; + CameraType cameraType_; //Camera type + + /* + * Visual stuff + */ + GeometricCamera* calibration1_, *calibration2_; //Camera calibration + GeometricCamera* originalCalib1_, *originalCalib2_; + std::vector vPinHoleDistorsion1_, vPinHoleDistorsion2_; + + cv::Size originalImSize_, newImSize_; + float fps_; + bool bRGB_; + + bool bNeedToUndistort_; + bool bNeedToRectify_; + bool bNeedToResize1_, bNeedToResize2_; + + Sophus::SE3f Tlr_; + float thDepth_; + float bf_, b_; + + /* + * Rectification stuff + */ + cv::Mat M1l_, M2l_; + cv::Mat M1r_, M2r_; + + /* + * Inertial stuff + */ + float noiseGyro_, noiseAcc_; + float gyroWalk_, accWalk_; + float imuFrequency_; + Sophus::SE3f Tbc_; + bool insertKFsWhenLost_; + + /* + * RGBD stuff + */ + float depthMapFactor_; + + /* + * ORB stuff + */ + int nFeatures_; + float scaleFactor_; + int nLevels_; + int initThFAST_, minThFAST_; + + /* + * Viewer stuff + */ + float keyFrameSize_; + float keyFrameLineWidth_; + float graphLineWidth_; + float pointSize_; + float cameraSize_; + float cameraLineWidth_; + float viewPointX_, viewPointY_, viewPointZ_, viewPointF_; + float imageViewerScale_; + + /* + * Save & load maps + */ + std::string sLoadFrom_, sSaveto_; + + /* + * Other stuff + */ + float thFarPoints_; + + }; +}; + + +#endif //ORB_SLAM3_SETTINGS_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/Sim3Solver.h b/ORB-SLAM3-UESTC/Workspace/include/Sim3Solver.h new file mode 100644 index 0000000..5539987 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/Sim3Solver.h @@ -0,0 +1,135 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef SIM3SOLVER_H +#define SIM3SOLVER_H + +#include +#include + +#include "KeyFrame.h" + + + +namespace ORB_SLAM3 +{ + +class Sim3Solver +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Sim3Solver(KeyFrame* pKF1, KeyFrame* pKF2, const std::vector &vpMatched12, const bool bFixScale = true, + const vector vpKeyFrameMatchedMP = vector()); + + void SetRansacParameters(double probability = 0.99, int minInliers = 6 , int maxIterations = 300); + + Eigen::Matrix4f find(std::vector &vbInliers12, int &nInliers); + + Eigen::Matrix4f iterate(int nIterations, bool &bNoMore, std::vector &vbInliers, int &nInliers); + Eigen::Matrix4f iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers, bool &bConverge); + + Eigen::Matrix4f GetEstimatedTransformation(); + Eigen::Matrix3f GetEstimatedRotation(); + Eigen::Vector3f GetEstimatedTranslation(); + float GetEstimatedScale(); + +protected: + + void ComputeCentroid(Eigen::Matrix3f &P, Eigen::Matrix3f &Pr, Eigen::Vector3f &C); + + void ComputeSim3(Eigen::Matrix3f &P1, Eigen::Matrix3f &P2); + + void CheckInliers(); + + void Project(const std::vector &vP3Dw, std::vector &vP2D, Eigen::Matrix4f Tcw, GeometricCamera* pCamera); + void FromCameraToImage(const std::vector &vP3Dc, std::vector &vP2D, GeometricCamera* pCamera); + + +protected: + + // KeyFrames and matches + KeyFrame* mpKF1; + KeyFrame* mpKF2; + + std::vector mvX3Dc1; + std::vector mvX3Dc2; + std::vector mvpMapPoints1; + std::vector mvpMapPoints2; + std::vector mvpMatches12; + std::vector mvnIndices1; + std::vector mvSigmaSquare1; + std::vector mvSigmaSquare2; + std::vector mvnMaxError1; + std::vector mvnMaxError2; + + int N; + int mN1; + + // Current Estimation + Eigen::Matrix3f mR12i; + Eigen::Vector3f mt12i; + float ms12i; + Eigen::Matrix4f mT12i; + Eigen::Matrix4f mT21i; + std::vector mvbInliersi; + int mnInliersi; + + // Current Ransac State + int mnIterations; + std::vector mvbBestInliers; + int mnBestInliers; + Eigen::Matrix4f mBestT12; + Eigen::Matrix3f mBestRotation; + Eigen::Vector3f mBestTranslation; + float mBestScale; + + // Scale is fixed to 1 in the stereo/RGBD case + bool mbFixScale; + + // Indices for random selection + std::vector mvAllIndices; + + // Projections + std::vector mvP1im1; + std::vector mvP2im2; + + // RANSAC probability + double mRansacProb; + + // RANSAC min inliers + int mRansacMinInliers; + + // RANSAC max iterations + int mRansacMaxIts; + + // Threshold inlier/outlier. e = dist(Pi,T_ij*Pj)^2 < 5.991*mSigma2 + float mTh; + float mSigma2; + + // Calibration + //cv::Mat mK1; + //cv::Mat mK2; + + GeometricCamera* pCamera1, *pCamera2; + +}; + +} //namespace ORB_SLAM + +#endif // SIM3SOLVER_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/System.h b/ORB-SLAM3-UESTC/Workspace/include/System.h new file mode 100644 index 0000000..872c86e --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/System.h @@ -0,0 +1,269 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef SYSTEM_H +#define SYSTEM_H + + +#include +#include +#include +#include +#include +#include + +#include "Tracking.h" +#include "FrameDrawer.h" +#include "MapDrawer.h" +#include "Atlas.h" +#include "LocalMapping.h" +#include "LoopClosing.h" +#include "KeyFrameDatabase.h" +#include "ORBVocabulary.h" +#include "Viewer.h" +#include "ImuTypes.h" +#include "Settings.h" + + +namespace ORB_SLAM3 +{ + +class Verbose +{ +public: + enum eLevel + { + VERBOSITY_QUIET=0, + VERBOSITY_NORMAL=1, + VERBOSITY_VERBOSE=2, + VERBOSITY_VERY_VERBOSE=3, + VERBOSITY_DEBUG=4 + }; + + static eLevel th; + +public: + static void PrintMess(std::string str, eLevel lev) + { + if(lev <= th) + cout << str << endl; + } + + static void SetTh(eLevel _th) + { + th = _th; + } +}; + +class Viewer; +class FrameDrawer; +class MapDrawer; +class Atlas; +class Tracking; +class LocalMapping; +class LoopClosing; +class Settings; + +class System +{ +public: + // Input sensor + enum eSensor{ + MONOCULAR=0, + STEREO=1, + RGBD=2, + IMU_MONOCULAR=3, + IMU_STEREO=4, + IMU_RGBD=5, + }; + + // File type + enum FileType{ + TEXT_FILE=0, + BINARY_FILE=1, + }; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads. + System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true, const int initFr = 0, const string &strSequence = std::string()); + + // Proccess the given stereo frame. Images must be synchronized and rectified. + // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. + // Returns the camera pose (empty if tracking fails). + Sophus::SE3f TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp, const vector& vImuMeas = vector(), string filename=""); + + // Process the given rgbd frame. Depthmap must be registered to the RGB frame. + // Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. + // Input depthmap: Float (CV_32F). + // Returns the camera pose (empty if tracking fails). + Sophus::SE3f TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp, const vector& vImuMeas = vector(), string filename=""); + + // Proccess the given monocular frame and optionally imu data + // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. + // Returns the camera pose (empty if tracking fails). + Sophus::SE3f TrackMonocular(const cv::Mat &im, const double ×tamp, const vector& vImuMeas = vector(), string filename=""); + + + // This stops local mapping thread (map building) and performs only camera tracking. + void ActivateLocalizationMode(); + // This resumes local mapping thread and performs SLAM again. + void DeactivateLocalizationMode(); + + // Returns true if there have been a big map change (loop closure, global BA) + // since last call to this function + bool MapChanged(); + + // Reset the system (clear Atlas or the active map) + void Reset(); + void ResetActiveMap(); + + // All threads will be requested to finish. + // It waits until all threads have finished. + // This function must be called before saving the trajectory. + void Shutdown(); + bool isShutDown(); + + // Save camera trajectory in the TUM RGB-D dataset format. + // Only for stereo and RGB-D. This method does not work for monocular. + // Call first Shutdown() + // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset + void SaveTrajectoryTUM(const string &filename); + + // Save keyframe poses in the TUM RGB-D dataset format. + // This method works for all sensor input. + // Call first Shutdown() + // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset + void SaveKeyFrameTrajectoryTUM(const string &filename); + + void SaveTrajectoryEuRoC(const string &filename); + void SaveKeyFrameTrajectoryEuRoC(const string &filename); + + void SaveTrajectoryEuRoC(const string &filename, Map* pMap); + void SaveKeyFrameTrajectoryEuRoC(const string &filename, Map* pMap); + + // Save data used for initialization debug + void SaveDebugData(const int &iniIdx); + + // Save camera trajectory in the KITTI dataset format. + // Only for stereo and RGB-D. This method does not work for monocular. + // Call first Shutdown() + // See format details at: http://www.cvlibs.net/datasets/kitti/eval_odometry.php + void SaveTrajectoryKITTI(const string &filename); + + // TODO: Save/Load functions + // SaveMap(const string &filename); + // LoadMap(const string &filename); + + // Information from most recent processed frame + // You can call this right after TrackMonocular (or stereo or RGBD) + int GetTrackingState(); + std::vector GetTrackedMapPoints(); + std::vector GetTrackedKeyPointsUn(); + + // For debugging + double GetTimeFromIMUInit(); + bool isLost(); + bool isFinished(); + + void ChangeDataset(); + + float GetImageScale(); + +#ifdef REGISTER_TIMES + void InsertRectTime(double& time); + void InsertResizeTime(double& time); + void InsertTrackTime(double& time); +#endif + +private: + + void SaveAtlas(int type); + bool LoadAtlas(int type); + + string CalculateCheckSum(string filename, int type); + + // Input sensor + eSensor mSensor; + + // ORB vocabulary used for place recognition and feature matching. + ORBVocabulary* mpVocabulary; + + // KeyFrame database for place recognition (relocalization and loop detection). + KeyFrameDatabase* mpKeyFrameDatabase; + + // Map structure that stores the pointers to all KeyFrames and MapPoints. + //Map* mpMap; + Atlas* mpAtlas; + + // Tracker. It receives a frame and computes the associated camera pose. + // It also decides when to insert a new keyframe, create some new MapPoints and + // performs relocalization if tracking fails. + Tracking* mpTracker; + + // Local Mapper. It manages the local map and performs local bundle adjustment. + LocalMapping* mpLocalMapper; + + // Loop Closer. It searches loops with every new keyframe. If there is a loop it performs + // a pose graph optimization and full bundle adjustment (in a new thread) afterwards. + LoopClosing* mpLoopCloser; + + // The viewer draws the map and the current camera pose. It uses Pangolin. + Viewer* mpViewer; + + FrameDrawer* mpFrameDrawer; + MapDrawer* mpMapDrawer; + + // System threads: Local Mapping, Loop Closing, Viewer. + // The Tracking thread "lives" in the main execution thread that creates the System object. + std::thread* mptLocalMapping; + std::thread* mptLoopClosing; + std::thread* mptViewer; + + // Reset flag + std::mutex mMutexReset; + bool mbReset; + bool mbResetActiveMap; + + // Change mode flags + std::mutex mMutexMode; + bool mbActivateLocalizationMode; + bool mbDeactivateLocalizationMode; + + // Shutdown flag + bool mbShutDown; + + // Tracking state + int mTrackingState; + std::vector mTrackedMapPoints; + std::vector mTrackedKeyPointsUn; + std::mutex mMutexState; + + // + string mStrLoadAtlasFromFile; + string mStrSaveAtlasToFile; + + string mStrVocabularyFilePath; + + Settings* settings_; +}; + +}// namespace ORB_SLAM + +#endif // SYSTEM_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/Tracking.h b/ORB-SLAM3-UESTC/Workspace/include/Tracking.h new file mode 100644 index 0000000..d09daff --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/Tracking.h @@ -0,0 +1,375 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef TRACKING_H +#define TRACKING_H + +#include +#include + +#include "Viewer.h" +#include "FrameDrawer.h" +#include "Atlas.h" +#include "LocalMapping.h" +#include "LoopClosing.h" +#include "Frame.h" +#include "ORBVocabulary.h" +#include "KeyFrameDatabase.h" +#include "ORBextractor.h" +#include "MapDrawer.h" +#include "System.h" +#include "ImuTypes.h" +#include "Settings.h" + +#include "GeometricCamera.h" + +#include +#include + +namespace ORB_SLAM3 +{ + +class Viewer; +class FrameDrawer; +class Atlas; +class LocalMapping; +class LoopClosing; +class System; +class Settings; + +class Tracking +{ + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Atlas* pAtlas, + KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor, Settings* settings, const string &_nameSeq=std::string()); + + ~Tracking(); + + // Parse the config file + bool ParseCamParamFile(cv::FileStorage &fSettings); + bool ParseORBParamFile(cv::FileStorage &fSettings); + bool ParseIMUParamFile(cv::FileStorage &fSettings); + + // Preprocess the input and call Track(). Extract features and performs stereo matching. + Sophus::SE3f GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double ×tamp, string filename); + Sophus::SE3f GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp, string filename); + Sophus::SE3f GrabImageMonocular(const cv::Mat &im, const double ×tamp, string filename); + + void GrabImuData(const IMU::Point &imuMeasurement); + + void SetLocalMapper(LocalMapping* pLocalMapper); + void SetLoopClosing(LoopClosing* pLoopClosing); + void SetViewer(Viewer* pViewer); + void SetStepByStep(bool bSet); + bool GetStepByStep(); + + // Load new settings + // The focal lenght should be similar or scale prediction will fail when projecting points + void ChangeCalibration(const string &strSettingPath); + + // Use this function if you have deactivated local mapping and you only want to localize the camera. + void InformOnlyTracking(const bool &flag); + + void UpdateFrameIMU(const float s, const IMU::Bias &b, KeyFrame* pCurrentKeyFrame); + KeyFrame* GetLastKeyFrame() + { + return mpLastKeyFrame; + } + + void CreateMapInAtlas(); + //std::mutex mMutexTracks; + + //-- + void NewDataset(); + int GetNumberDataset(); + int GetMatchesInliers(); + + //DEBUG + void SaveSubTrajectory(string strNameFile_frames, string strNameFile_kf, string strFolder=""); + void SaveSubTrajectory(string strNameFile_frames, string strNameFile_kf, Map* pMap); + + float GetImageScale(); + +#ifdef REGISTER_LOOP + void RequestStop(); + bool isStopped(); + void Release(); + bool stopRequested(); +#endif + +public: + + // Tracking states + enum eTrackingState{ + SYSTEM_NOT_READY=-1, + NO_IMAGES_YET=0, + NOT_INITIALIZED=1, + OK=2, + RECENTLY_LOST=3, + LOST=4, + OK_KLT=5 + }; + + eTrackingState mState; + eTrackingState mLastProcessedState; + + // Input sensor + int mSensor; + + // Current Frame + Frame mCurrentFrame; + Frame mLastFrame; + + cv::Mat mImGray; + + // Initialization Variables (Monocular) + std::vector mvIniLastMatches; + std::vector mvIniMatches; + std::vector mvbPrevMatched; + std::vector mvIniP3D; + Frame mInitialFrame; + + // Lists used to recover the full camera trajectory at the end of the execution. + // Basically we store the reference keyframe for each frame and its relative transformation + list mlRelativeFramePoses; + list mlpReferences; + list mlFrameTimes; + list mlbLost; + + // frames with estimated pose + int mTrackedFr; + bool mbStep; + + // True if local mapping is deactivated and we are performing only localization + bool mbOnlyTracking; + + void Reset(bool bLocMap = false); + void ResetActiveMap(bool bLocMap = false); + + float mMeanTrack; + bool mbInitWith3KFs; + double t0; // time-stamp of first read frame + double t0vis; // time-stamp of first inserted keyframe + double t0IMU; // time-stamp of IMU initialization + bool mFastInit = false; + + + vector GetLocalMapMPS(); + + bool mbWriteStats; + +#ifdef REGISTER_TIMES + void LocalMapStats2File(); + void TrackStats2File(); + void PrintTimeStats(); + + vector vdRectStereo_ms; + vector vdResizeImage_ms; + vector vdORBExtract_ms; + vector vdStereoMatch_ms; + vector vdIMUInteg_ms; + vector vdPosePred_ms; + vector vdLMTrack_ms; + vector vdNewKF_ms; + vector vdTrackTotal_ms; +#endif + +protected: + + // Main tracking function. It is independent of the input sensor. + void Track(); + + // Map initialization for stereo and RGB-D + void StereoInitialization(); + + // Map initialization for monocular + void MonocularInitialization(); + //void CreateNewMapPoints(); + void CreateInitialMapMonocular(); + + void CheckReplacedInLastFrame(); + bool TrackReferenceKeyFrame(); + void UpdateLastFrame(); + bool TrackWithMotionModel(); + bool PredictStateIMU(); + + bool Relocalization(); + + void UpdateLocalMap(); + void UpdateLocalPoints(); + void UpdateLocalKeyFrames(); + + bool TrackLocalMap(); + void SearchLocalPoints(); + + bool NeedNewKeyFrame(); + void CreateNewKeyFrame(); + + // Perform preintegration from last frame + void PreintegrateIMU(); + + // Reset IMU biases and compute frame velocity + void ResetFrameIMU(); + + bool mbMapUpdated; + + // Imu preintegration from last frame + IMU::Preintegrated *mpImuPreintegratedFromLastKF; + + // Queue of IMU measurements between frames + std::list mlQueueImuData; + +// Vector of IMU measurements from previous to current frame (to be filled by PreintegrateIMU) + std::vector mvImuFromLastFrame; + std::mutex mMutexImuQueue; + + // Imu calibration parameters + IMU::Calib *mpImuCalib; + + // Last Bias Estimation (at keyframe creation) + IMU::Bias mLastBias; + + // In case of performing only localization, this flag is true when there are no matches to + // points in the map. Still tracking will continue if there are enough matches with temporal points. + // In that case we are doing visual odometry. The system will try to do relocalization to recover + // "zero-drift" localization to the map. + bool mbVO; + + //Other Thread Pointers + LocalMapping* mpLocalMapper; + LoopClosing* mpLoopClosing; + + //ORB + ORBextractor* mpORBextractorLeft, *mpORBextractorRight; + ORBextractor* mpIniORBextractor; + + //BoW + ORBVocabulary* mpORBVocabulary; + KeyFrameDatabase* mpKeyFrameDB; + + // Initalization (only for monocular) + bool mbReadyToInitializate; + bool mbSetInit; + + //Local Map + KeyFrame* mpReferenceKF; + std::vector mvpLocalKeyFrames; + std::vector mvpLocalMapPoints; + + // System + System* mpSystem; + + //Drawers + Viewer* mpViewer; + FrameDrawer* mpFrameDrawer; + MapDrawer* mpMapDrawer; + bool bStepByStep; + + //Atlas + Atlas* mpAtlas; + + //Calibration matrix + cv::Mat mK; + Eigen::Matrix3f mK_; + cv::Mat mDistCoef; + float mbf; + float mImageScale; + + float mImuFreq; + double mImuPer; + bool mInsertKFsLost; + + //New KeyFrame rules (according to fps) + int mMinFrames; + int mMaxFrames; + + int mnFirstImuFrameId; + int mnFramesToResetIMU; + + // Threshold close/far points + // Points seen as close by the stereo/RGBD sensor are considered reliable + // and inserted from just one frame. Far points requiere a match in two keyframes. + float mThDepth; + + // For RGB-D inputs only. For some datasets (e.g. TUM) the depthmap values are scaled. + float mDepthMapFactor; + + //Current matches in frame + int mnMatchesInliers; + + //Last Frame, KeyFrame and Relocalisation Info + KeyFrame* mpLastKeyFrame; + unsigned int mnLastKeyFrameId; + unsigned int mnLastRelocFrameId; + double mTimeStampLost; + double time_recently_lost; + + unsigned int mnFirstFrameId; + unsigned int mnInitialFrameId; + unsigned int mnLastInitFrameId; + + bool mbCreatedMap; + + //Motion Model + bool mbVelocity{false}; + Sophus::SE3f mVelocity; + + //Color order (true RGB, false BGR, ignored if grayscale) + bool mbRGB; + + list mlpTemporalPoints; + + //int nMapChangeIndex; + + int mnNumDataset; + + ofstream f_track_stats; + + ofstream f_track_times; + double mTime_PreIntIMU; + double mTime_PosePred; + double mTime_LocalMapTrack; + double mTime_NewKF_Dec; + + GeometricCamera* mpCamera, *mpCamera2; + + int initID, lastID; + + Sophus::SE3f mTlr; + + void newParameterLoader(Settings* settings); + +#ifdef REGISTER_LOOP + bool Stop(); + + bool mbStopped; + bool mbStopRequested; + bool mbNotStop; + std::mutex mMutexStop; +#endif + +public: + cv::Mat mImRight; +}; + +} //namespace ORB_SLAM + +#endif // TRACKING_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/TwoViewReconstruction.h b/ORB-SLAM3-UESTC/Workspace/include/TwoViewReconstruction.h new file mode 100644 index 0000000..044563c --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/TwoViewReconstruction.h @@ -0,0 +1,99 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#ifndef TwoViewReconstruction_H +#define TwoViewReconstruction_H + +#include +#include +#include + +#include + +namespace ORB_SLAM3 +{ + + class TwoViewReconstruction + { + typedef std::pair Match; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + // Fix the reference frame + TwoViewReconstruction(const Eigen::Matrix3f& k, float sigma = 1.0, int iterations = 200); + + // Computes in parallel a fundamental matrix and a homography + // Selects a model and tries to recover the motion and the structure from motion + bool Reconstruct(const std::vector& vKeys1, const std::vector& vKeys2, const std::vector &vMatches12, + Sophus::SE3f &T21, std::vector &vP3D, std::vector &vbTriangulated); + + private: + + void FindHomography(std::vector &vbMatchesInliers, float &score, Eigen::Matrix3f &H21); + void FindFundamental(std::vector &vbInliers, float &score, Eigen::Matrix3f &F21); + + Eigen::Matrix3f ComputeH21(const std::vector &vP1, const std::vector &vP2); + Eigen::Matrix3f ComputeF21(const std::vector &vP1, const std::vector &vP2); + + float CheckHomography(const Eigen::Matrix3f &H21, const Eigen::Matrix3f &H12, std::vector &vbMatchesInliers, float sigma); + + float CheckFundamental(const Eigen::Matrix3f &F21, std::vector &vbMatchesInliers, float sigma); + + bool ReconstructF(std::vector &vbMatchesInliers, Eigen::Matrix3f &F21, Eigen::Matrix3f &K, + Sophus::SE3f &T21, std::vector &vP3D, std::vector &vbTriangulated, float minParallax, int minTriangulated); + + bool ReconstructH(std::vector &vbMatchesInliers, Eigen::Matrix3f &H21, Eigen::Matrix3f &K, + Sophus::SE3f &T21, std::vector &vP3D,std:: vector &vbTriangulated, float minParallax, int minTriangulated); + + void Normalize(const std::vector &vKeys, std::vector &vNormalizedPoints, Eigen::Matrix3f &T); + + + int CheckRT(const Eigen::Matrix3f &R, const Eigen::Vector3f &t, const std::vector &vKeys1, const std::vector &vKeys2, + const std::vector &vMatches12, std::vector &vbMatchesInliers, + const Eigen::Matrix3f &K, std::vector &vP3D, float th2, std::vector &vbGood, float ¶llax); + + void DecomposeE(const Eigen::Matrix3f &E, Eigen::Matrix3f &R1, Eigen::Matrix3f &R2, Eigen::Vector3f &t); + + + // Keypoints from Reference Frame (Frame 1) + std::vector mvKeys1; + + // Keypoints from Current Frame (Frame 2) + std::vector mvKeys2; + + // Current Matches from Reference to Current + std::vector mvMatches12; + std::vector mvbMatched1; + + // Calibration + Eigen::Matrix3f mK; + + // Standard Deviation and Variance + float mSigma, mSigma2; + + // Ransac max iterations + int mMaxIterations; + + // Ransac sets + std::vector > mvSets; + + }; + +} //namespace ORB_SLAM + +#endif // TwoViewReconstruction_H diff --git a/ORB-SLAM3-UESTC/Workspace/include/Viewer.h b/ORB-SLAM3-UESTC/Workspace/include/Viewer.h new file mode 100644 index 0000000..920c628 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/include/Viewer.h @@ -0,0 +1,104 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#ifndef VIEWER_H +#define VIEWER_H + +#include "FrameDrawer.h" +#include "MapDrawer.h" +#include "Tracking.h" +#include "System.h" +#include "Settings.h" + +#include + +namespace ORB_SLAM3 +{ + +class Tracking; +class FrameDrawer; +class MapDrawer; +class System; +class Settings; + +class Viewer +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Viewer(System* pSystem, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Tracking *pTracking, const string &strSettingPath, Settings* settings); + + void newParameterLoader(Settings* settings); + + // Main thread function. Draw points, keyframes, the current camera pose and the last processed + // frame. Drawing is refreshed according to the camera fps. We use Pangolin. + void Run(); + + void RequestFinish(); + + void RequestStop(); + + bool isFinished(); + + bool isStopped(); + + bool isStepByStep(); + + void Release(); + + //void SetTrackingPause(); + + bool both; +private: + + bool ParseViewerParamFile(cv::FileStorage &fSettings); + + bool Stop(); + + System* mpSystem; + FrameDrawer* mpFrameDrawer; + MapDrawer* mpMapDrawer; + Tracking* mpTracker; + + // 1/fps in ms + double mT; + float mImageWidth, mImageHeight; + float mImageViewerScale; + + float mViewpointX, mViewpointY, mViewpointZ, mViewpointF; + + bool CheckFinish(); + void SetFinish(); + bool mbFinishRequested; + bool mbFinished; + std::mutex mMutexFinish; + + bool mbStopped; + bool mbStopRequested; + std::mutex mMutexStop; + + bool mbStopTrack; + +}; + +} + + +#endif // VIEWER_H + + diff --git a/ORB-SLAM3-UESTC/Workspace/src/Atlas.cc b/ORB-SLAM3-UESTC/Workspace/src/Atlas.cc new file mode 100644 index 0000000..390120c --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/Atlas.cc @@ -0,0 +1,455 @@ +/** + * This file is part of ORB-SLAM3 + * + * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * + * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public + * License as published by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even + * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with ORB-SLAM3. + * If not, see . + */ + +#include "Atlas.h" +#include "Viewer.h" + +#include "GeometricCamera.h" +#include "Pinhole.h" +#include "KannalaBrandt8.h" + +namespace ORB_SLAM3 +{ + +Atlas::Atlas() +{ + mpCurrentMap = static_cast(NULL); +} + +Atlas::Atlas(int initKFid) : mnLastInitKFidMap(initKFid), mHasViewer(false) +{ + mpCurrentMap = static_cast(NULL); + CreateNewMap(); +} + +Atlas::~Atlas() +{ + for (std::set::iterator it = mspMaps.begin(), end = mspMaps.end(); it != end;) + { + Map *pMi = *it; + + if (pMi) + { + delete pMi; + pMi = static_cast(NULL); + + it = mspMaps.erase(it); + } + else + ++it; + } +} + +/** + * @brief 创建新地图，如果当前活跃地图有效，先存储当前地图为不活跃地图，然后新建地图；否则，可以直接新建地图。 + * + */ +void Atlas::CreateNewMap() +{ + // 锁住地图集 + unique_lock lock(mMutexAtlas); + cout << "Creation of new map with id: " << Map::nNextId << endl; + // 如果当前活跃地图有效，先存储当前地图为不活跃地图后退出 + if (mpCurrentMap) + { + // mnLastInitKFidMap为当前地图创建时第1个关键帧的id，它是在上一个地图最大关键帧id的基础上增加1 + if (!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid()) + mnLastInitKFidMap = mpCurrentMap->GetMaxKFid() + 1; // The init KF is the next of current maximum + + // 将当前地图储存起来，其实就是把mIsInUse标记为false + mpCurrentMap->SetStoredMap(); + cout << "Stored map with ID: " << mpCurrentMap->GetId() << endl; + + // if(mHasViewer) + // mpViewer->AddMapToCreateThumbnail(mpCurrentMap); + } + cout << "Creation of new map with last KF id: " << mnLastInitKFidMap << endl; + + mpCurrentMap = new Map(mnLastInitKFidMap); //新建地图 + mpCurrentMap->SetCurrentMap(); //设置为活跃地图 + mspMaps.insert(mpCurrentMap); //插入地图集 +} + +void Atlas::ChangeMap(Map *pMap) +{ + unique_lock lock(mMutexAtlas); + cout << "Change to map with id: " << pMap->GetId() << endl; + if (mpCurrentMap) + { + mpCurrentMap->SetStoredMap(); + } + + mpCurrentMap = pMap; + mpCurrentMap->SetCurrentMap(); +} + +unsigned long int Atlas::GetLastInitKFid() +{ + unique_lock lock(mMutexAtlas); + return mnLastInitKFidMap; +} + +void Atlas::SetViewer(Viewer *pViewer) +{ + mpViewer = pViewer; + mHasViewer = true; +} + +void Atlas::AddKeyFrame(KeyFrame *pKF) +{ + Map *pMapKF = pKF->GetMap(); + pMapKF->AddKeyFrame(pKF); +} + +void Atlas::AddMapPoint(MapPoint *pMP) +{ + Map *pMapMP = pMP->GetMap(); + pMapMP->AddMapPoint(pMP); +} + +/** + * @brief 添加相机，跟保存地图相关 + * @param pCam 相机 + */ +GeometricCamera *Atlas::AddCamera(GeometricCamera *pCam) +{ + // Check if the camera already exists + bool bAlreadyInMap = false; + int index_cam = -1; + // 遍历地图中现有的相机看看跟输入的相机一不一样，不一样的话则向mvpCameras添加 + for (size_t i = 0; i < mvpCameras.size(); ++i) + { + GeometricCamera *pCam_i = mvpCameras[i]; + if (!pCam) + std::cout << "Not pCam" << std::endl; + if (!pCam_i) + std::cout << "Not pCam_i" << std::endl; + if (pCam->GetType() != pCam_i->GetType()) + continue; + + if (pCam->GetType() == GeometricCamera::CAM_PINHOLE) + { + if (((Pinhole *)pCam_i)->IsEqual(pCam)) + { + bAlreadyInMap = true; + index_cam = i; + } + } + else if (pCam->GetType() == GeometricCamera::CAM_FISHEYE) + { + if (((KannalaBrandt8 *)pCam_i)->IsEqual(pCam)) + { + bAlreadyInMap = true; + index_cam = i; + } + } + } + + if (bAlreadyInMap) + { + return mvpCameras[index_cam]; + } + else + { + mvpCameras.push_back(pCam); + return pCam; + } +} + +std::vector Atlas::GetAllCameras() +{ + return mvpCameras; +} + +void Atlas::SetReferenceMapPoints(const std::vector &vpMPs) +{ + unique_lock lock(mMutexAtlas); + mpCurrentMap->SetReferenceMapPoints(vpMPs); +} + +void Atlas::InformNewBigChange() +{ + unique_lock lock(mMutexAtlas); + mpCurrentMap->InformNewBigChange(); +} + +int Atlas::GetLastBigChangeIdx() +{ + unique_lock lock(mMutexAtlas); + return mpCurrentMap->GetLastBigChangeIdx(); +} + +long unsigned int Atlas::MapPointsInMap() +{ + unique_lock lock(mMutexAtlas); + return mpCurrentMap->MapPointsInMap(); +} + +long unsigned Atlas::KeyFramesInMap() +{ + unique_lock lock(mMutexAtlas); + return mpCurrentMap->KeyFramesInMap(); +} + +std::vector Atlas::GetAllKeyFrames() +{ + unique_lock lock(mMutexAtlas); + return mpCurrentMap->GetAllKeyFrames(); +} + +std::vector Atlas::GetAllMapPoints() +{ + unique_lock lock(mMutexAtlas); + return mpCurrentMap->GetAllMapPoints(); +} + +std::vector Atlas::GetReferenceMapPoints() +{ + unique_lock lock(mMutexAtlas); + return mpCurrentMap->GetReferenceMapPoints(); +} + +vector Atlas::GetAllMaps() +{ + unique_lock lock(mMutexAtlas); + struct compFunctor + { + inline bool operator()(Map *elem1, Map *elem2) + { + return elem1->GetId() < elem2->GetId(); + } + }; + vector vMaps(mspMaps.begin(), mspMaps.end()); + sort(vMaps.begin(), vMaps.end(), compFunctor()); + return vMaps; +} + +int Atlas::CountMaps() +{ + unique_lock lock(mMutexAtlas); + return mspMaps.size(); +} + +void Atlas::clearMap() +{ + unique_lock lock(mMutexAtlas); + mpCurrentMap->clear(); +} + +void Atlas::clearAtlas() +{ + unique_lock lock(mMutexAtlas); + /*for(std::set::iterator it=mspMaps.begin(), send=mspMaps.end(); it!=send; it++) + { + (*it)->clear(); + delete *it; + }*/ + mspMaps.clear(); + mpCurrentMap = static_cast(NULL); + mnLastInitKFidMap = 0; +} + +Map *Atlas::GetCurrentMap() +{ + unique_lock lock(mMutexAtlas); + if (!mpCurrentMap) + CreateNewMap(); + while (mpCurrentMap->IsBad()) + usleep(3000); + + return mpCurrentMap; +} + +void Atlas::SetMapBad(Map *pMap) +{ + mspMaps.erase(pMap); + pMap->SetBad(); + + mspBadMaps.insert(pMap); +} + +void Atlas::RemoveBadMaps() +{ + /*for(Map* pMap : mspBadMaps) + { + delete pMap; + pMap = static_cast(NULL); + }*/ + mspBadMaps.clear(); +} + +bool Atlas::isInertial() +{ + unique_lock lock(mMutexAtlas); + return mpCurrentMap->IsInertial(); +} + +void Atlas::SetInertialSensor() +{ + unique_lock lock(mMutexAtlas); + // 接着又调用Map类的SetInertialSensor成员函数,将其设置为imu属性,以后的跟踪和预积分将和这个标志有关 + mpCurrentMap->SetInertialSensor(); +} + +void Atlas::SetImuInitialized() +{ + unique_lock lock(mMutexAtlas); + mpCurrentMap->SetImuInitialized(); +} + +bool Atlas::isImuInitialized() +{ + unique_lock lock(mMutexAtlas); + return mpCurrentMap->isImuInitialized(); +} + +/** + * @brief 预保存，意思是在保存成地图文件之前，要保存到对应变量里面 + */ +void Atlas::PreSave() +{ + // 1. 更新mnLastInitKFidMap + if (mpCurrentMap) + { + if (!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid()) + mnLastInitKFidMap = mpCurrentMap->GetMaxKFid() + 1; // The init KF is the next of current maximum + } + + // 比较map id + struct compFunctor + { + inline bool operator()(Map *elem1, Map *elem2) + { + return elem1->GetId() < elem2->GetId(); + } + }; + std::copy(mspMaps.begin(), mspMaps.end(), std::back_inserter(mvpBackupMaps)); + // 2. 按照id从小到大排列 + sort(mvpBackupMaps.begin(), mvpBackupMaps.end(), compFunctor()); + + std::set spCams(mvpCameras.begin(), mvpCameras.end()); + + // 3. 遍历所有地图，执行每个地图的预保存 + for (Map *pMi : mvpBackupMaps) + { + if (!pMi || pMi->IsBad()) + continue; + + // 如果地图为空，则跳过 + if (pMi->GetAllKeyFrames().size() == 0) + { + // Empty map, erase before of save it. + SetMapBad(pMi); + continue; + } + pMi->PreSave(spCams); + } + + // 4. 删除坏地图 + RemoveBadMaps(); +} + +/** + * @brief 后加载，意思是读取地图文件后加载各个信息 + */ +void Atlas::PostLoad() +{ + // 1. 读取当前所有相机 + map mpCams; + for (GeometricCamera *pCam : mvpCameras) + { + mpCams[pCam->GetId()] = pCam; + } + + mspMaps.clear(); + unsigned long int numKF = 0, numMP = 0; + // 2. 加载各个地图 + for (Map *pMi : mvpBackupMaps) + { + mspMaps.insert(pMi); + pMi->PostLoad(mpKeyFrameDB, mpORBVocabulary, mpCams); + numKF += pMi->GetAllKeyFrames().size(); + numMP += pMi->GetAllMapPoints().size(); + } + mvpBackupMaps.clear(); +} + +void Atlas::SetKeyFrameDababase(KeyFrameDatabase *pKFDB) +{ + mpKeyFrameDB = pKFDB; +} + +KeyFrameDatabase *Atlas::GetKeyFrameDatabase() +{ + return mpKeyFrameDB; +} + +void Atlas::SetORBVocabulary(ORBVocabulary *pORBVoc) +{ + mpORBVocabulary = pORBVoc; +} + +/** + * @brief 以下函数暂未用到 + */ +ORBVocabulary *Atlas::GetORBVocabulary() +{ + return mpORBVocabulary; +} + +long unsigned int Atlas::GetNumLivedKF() +{ + unique_lock lock(mMutexAtlas); + long unsigned int num = 0; + for (Map *pMap_i : mspMaps) + { + num += pMap_i->GetAllKeyFrames().size(); + } + + return num; +} + +long unsigned int Atlas::GetNumLivedMP() +{ + unique_lock lock(mMutexAtlas); + long unsigned int num = 0; + for (Map *pMap_i : mspMaps) + { + num += pMap_i->GetAllMapPoints().size(); + } + + return num; +} + +map Atlas::GetAtlasKeyframes() +{ + map mpIdKFs; + for (Map *pMap_i : mvpBackupMaps) + { + vector vpKFs_Mi = pMap_i->GetAllKeyFrames(); + + for (KeyFrame *pKF_j_Mi : vpKFs_Mi) + { + mpIdKFs[pKF_j_Mi->mnId] = pKF_j_Mi; + } + } + + return mpIdKFs; +} + +} // namespace ORB_SLAM3 diff --git a/ORB-SLAM3-UESTC/Workspace/src/CameraModels/KannalaBrandt8.cpp b/ORB-SLAM3-UESTC/Workspace/src/CameraModels/KannalaBrandt8.cpp new file mode 100644 index 0000000..cd0297a --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/CameraModels/KannalaBrandt8.cpp @@ -0,0 +1,600 @@ +/** + * This file is part of ORB-SLAM3 + * + * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * + * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public + * License as published by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even + * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with ORB-SLAM3. + * If not, see . + */ + +#include "KannalaBrandt8.h" + +#include + +// BOOST_CLASS_EXPORT_IMPLEMENT(ORB_SLAM3::KannalaBrandt8) + +namespace ORB_SLAM3 +{ +// BOOST_CLASS_EXPORT_GUID(KannalaBrandt8, "KannalaBrandt8") + +/** + * @brief 投影 + * xc​ = Xc/Zc, yc = Yc/Zc + * r^2 = xc^2 + yc^2 + * θ = arctan(r) + * θd = k0*θ + k1*θ^3 + k2*θ^5 + k3*θ^7 + k4*θ^9 + * xd = θd/r * xc yd = θd/r * yc + * u = fx*xd + cx v = fy*yd + cy + * @param p3D 三维点 + * @return 像素坐标 + */ +cv::Point2f KannalaBrandt8::project(const cv::Point3f &p3D) +{ + const float x2_plus_y2 = p3D.x * p3D.x + p3D.y * p3D.y; + const float theta = atan2f(sqrtf(x2_plus_y2), p3D.z); + const float psi = atan2f(p3D.y, p3D.x); + + const float theta2 = theta * theta; + const float theta3 = theta * theta2; + const float theta5 = theta3 * theta2; + const float theta7 = theta5 * theta2; + const float theta9 = theta7 * theta2; + const float r = theta + mvParameters[4] * theta3 + mvParameters[5] * theta5 + mvParameters[6] * theta7 + mvParameters[7] * theta9; + + return cv::Point2f(mvParameters[0] * r * cos(psi) + mvParameters[2], + mvParameters[1] * r * sin(psi) + mvParameters[3]); +} + +/** + * @brief 投影 + * @param v3D 三维点 + * @return 像素坐标 + */ +Eigen::Vector2d KannalaBrandt8::project(const Eigen::Vector3d &v3D) +{ + const double x2_plus_y2 = v3D[0] * v3D[0] + v3D[1] * v3D[1]; + const double theta = atan2f(sqrtf(x2_plus_y2), v3D[2]); + const double psi = atan2f(v3D[1], v3D[0]); + + const double theta2 = theta * theta; + const double theta3 = theta * theta2; + const double theta5 = theta3 * theta2; + const double theta7 = theta5 * theta2; + const double theta9 = theta7 * theta2; + const double r = theta + mvParameters[4] * theta3 + mvParameters[5] * theta5 + mvParameters[6] * theta7 + mvParameters[7] * theta9; + + Eigen::Vector2d res; + res[0] = mvParameters[0] * r * cos(psi) + mvParameters[2]; + res[1] = mvParameters[1] * r * sin(psi) + mvParameters[3]; + + return res; +} + +/** + * @brief 投影 + * @param v3D 三维点 + * @return 像素坐标 + */ +Eigen::Vector2f KannalaBrandt8::project(const Eigen::Vector3f &v3D) +{ + const float x2_plus_y2 = v3D[0] * v3D[0] + v3D[1] * v3D[1]; + const float theta = atan2f(sqrtf(x2_plus_y2), v3D[2]); + const float psi = atan2f(v3D[1], v3D[0]); + + const float theta2 = theta * theta; + const float theta3 = theta * theta2; + const float theta5 = theta3 * theta2; + const float theta7 = theta5 * theta2; + const float theta9 = theta7 * theta2; + const float r = theta + mvParameters[4] * theta3 + mvParameters[5] * theta5 + mvParameters[6] * theta7 + mvParameters[7] * theta9; + + Eigen::Vector2f res; + res[0] = mvParameters[0] * r * cos(psi) + mvParameters[2]; + res[1] = mvParameters[1] * r * sin(psi) + mvParameters[3]; + + return res; + + /*cv::Point2f cvres = this->project(cv::Point3f(v3D[0],v3D[1],v3D[2])); + + Eigen::Vector2d res; + res[0] = cvres.x; + res[1] = cvres.y; + + return res;*/ +} + +/** + * @brief 投影 + * @param p3D 三维点 + * @return 像素坐标 + */ +Eigen::Vector2f KannalaBrandt8::projectMat(const cv::Point3f &p3D) +{ + cv::Point2f point = this->project(p3D); + return Eigen::Vector2f(point.x, point.y); +} + +float KannalaBrandt8::uncertainty2(const Eigen::Matrix &p2D) +{ + /*Eigen::Matrix c; + c << mvParameters[2], mvParameters[3]; + if ((p2D-c).squaredNorm()>57600) // 240*240 (256) + return 100.f; + else + return 1.0f;*/ + return 1.f; +} + +/** + * @brief 反投影,以Eigen::Vector3f形式返回，KannalaBrandt8::TriangulateMatches中调用 + * @param p2D 特征点 + * @return + */ +Eigen::Vector3f KannalaBrandt8::unprojectEig(const cv::Point2f &p2D) +{ + cv::Point3f ray = this->unproject(p2D); + return Eigen::Vector3f(ray.x, ray.y, ray.z); +} + +/** + * @brief 反投影 + * 投影过程 + * xc​ = Xc/Zc, yc = Yc/Zc + * r^2 = xc^2 + yc^2 + * θ = arctan(r) + * θd = k0*θ + k1*θ^3 + k2*θ^5 + k3*θ^7 + k4*θ^9 + * xd = θd/r * xc yd = θd/r * yc + * u = fx*xd + cx v = fy*yd + cy + * + * + * 已知u与v 未矫正的特征点像素坐标 + * xd = (u - cx) / fx yd = (v - cy) / fy + * 待求的 xc = xd * r / θd yc = yd * r / θd + * 待求的 xc = xd * tan(θ) / θd yc = yd * tan(θ) / θd + * 其中 θd的算法如下： + * xd^2 + yd^2 = θd^2/r^2 * (xc^2 + yc^2) + * θd = sqrt(xd^2 + yd^2) / sqrt(xc^2 + yc^2) * r + * 其中r = sqrt(xc^2 + yc^2) + * 所以 θd = sqrt(xd^2 + yd^2) 已知 + * 所以待求的只有tan(θ),也就是θ + * 这里θd = θ + k1*θ^3 + k2*θ^5 + k3*θ^7 + k4*θ^9 + * 直接求解θ比较麻烦，这里用迭代的方式通过误差的一阶导数求θ + * θ的初始值定为了θd，设θ + k1*θ^3 + k2*θ^5 + k3*θ^7 + k4*θ^9 = f(θ) + * e(θ) = f(θ) - θd 目标是求解一个θ值另e(θ) = 0 + * 泰勒展开e(θ+δθ) = e(θ) + e`(θ) * δθ = 0 + * e`(θ) = 1 + 3*k1*θ^*2 + 5*k2*θ^4 + 7*k3*θ^6 + 8*k4*θ^8 + * δθ = -e(θ)/e`(θ) 经过多次迭代可求得θ + * 最后xc = xd * tan(θ) / θd yc = yd * tan(θ) / θd + * @param p2D 特征点 + * @return + */ +cv::Point3f KannalaBrandt8::unproject(const cv::Point2f &p2D) +{ + // Use Newthon method to solve for theta with good precision (err ~ e-6) + cv::Point2f pw((p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1]); + float scale = 1.f; + float theta_d = sqrtf(pw.x * pw.x + pw.y * pw.y); // sin(psi) = yc / r + theta_d = fminf(fmaxf(-CV_PI / 2.f, theta_d), CV_PI / 2.f); // 不能超过180度 + + if (theta_d > 1e-8) + { + // Compensate distortion iteratively + // θ的初始值定为了θd + float theta = theta_d; + + // 开始迭代 + for (int j = 0; j < 10; j++) + { + float theta2 = theta * theta, + theta4 = theta2 * theta2, + theta6 = theta4 * theta2, + theta8 = theta4 * theta4; + float k0_theta2 = mvParameters[4] * theta2, + k1_theta4 = mvParameters[5] * theta4; + float k2_theta6 = mvParameters[6] * theta6, + k3_theta8 = mvParameters[7] * theta8; + float theta_fix = (theta * (1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8) - theta_d) / + (1 + 3 * k0_theta2 + 5 * k1_theta4 + 7 * k2_theta6 + 9 * k3_theta8); + theta = theta - theta_fix; + if (fabsf(theta_fix) < precision) // 如果更新量变得很小，表示接近最终值 + break; + } + // scale = theta - theta_d; + // 求得tan(θ) / θd + scale = std::tan(theta) / theta_d; + } + + return cv::Point3f(pw.x * scale, pw.y * scale, 1.f); +} + +/** + * @brief 求解二维像素坐标关于三维点的雅克比矩阵 + * u = fx * θd * x / sqrt(x^2 + y^2) + cx + * v = fy * θd * y / sqrt(x^2 + y^2) + cy + * 这两个式子分别对 xyz 求导 + * θd = θ + k1*θ^3 + k2*θ^5 + k3*θ^7 + k4*θ^9 + * θ = arctan(sqrt(x^2 + y^2), z) + * @param p3D 三维点 + * @return + */ +Eigen::Matrix KannalaBrandt8::projectJac(const Eigen::Vector3d &v3D) +{ + double x2 = v3D[0] * v3D[0], y2 = v3D[1] * v3D[1], z2 = v3D[2] * v3D[2]; + double r2 = x2 + y2; + double r = sqrt(r2); + double r3 = r2 * r; + double theta = atan2(r, v3D[2]); + + double theta2 = theta * theta, theta3 = theta2 * theta; + double theta4 = theta2 * theta2, theta5 = theta4 * theta; + double theta6 = theta2 * theta4, theta7 = theta6 * theta; + double theta8 = theta4 * theta4, theta9 = theta8 * theta; + + double f = theta + theta3 * mvParameters[4] + theta5 * mvParameters[5] + theta7 * mvParameters[6] + + theta9 * mvParameters[7]; + double fd = 1 + 3 * mvParameters[4] * theta2 + 5 * mvParameters[5] * theta4 + 7 * mvParameters[6] * theta6 + + 9 * mvParameters[7] * theta8; + + Eigen::Matrix JacGood; + JacGood(0, 0) = mvParameters[0] * (fd * v3D[2] * x2 / (r2 * (r2 + z2)) + f * y2 / r3); // ∂u/∂x + JacGood(1, 0) = + mvParameters[1] * (fd * v3D[2] * v3D[1] * v3D[0] / (r2 * (r2 + z2)) - f * v3D[1] * v3D[0] / r3); // ∂v/∂x + + JacGood(0, 1) = + mvParameters[0] * (fd * v3D[2] * v3D[1] * v3D[0] / (r2 * (r2 + z2)) - f * v3D[1] * v3D[0] / r3); // ∂u/∂y + JacGood(1, 1) = mvParameters[1] * (fd * v3D[2] * y2 / (r2 * (r2 + z2)) + f * x2 / r3); // ∂v/∂y + + JacGood(0, 2) = -mvParameters[0] * fd * v3D[0] / (r2 + z2); // ∂u/∂z + JacGood(1, 2) = -mvParameters[1] * fd * v3D[1] / (r2 + z2); // ∂v/∂z + + return JacGood; +} + +/** 三角化恢复三维点，但要提前做去畸变 单目初始化中使用 + * @param vKeys1 第一帧的关键点 + * @param vKeys2 第二帧的关键点 + * @param vMatches12 匹配关系，长度与vKeys1一样，对应位置存放vKeys2中关键点的下标 + * @param T21 顾名思义 + * @param vP3D 恢复出的三维点 + * @param vbTriangulated 是否三角化成功，用于统计匹配点数量 + */ +bool KannalaBrandt8::ReconstructWithTwoViews( + const std::vector &vKeys1, const std::vector &vKeys2, const std::vector &vMatches12, + Sophus::SE3f &T21, std::vector &vP3D, std::vector &vbTriangulated) +{ + if (!tvr) + { + Eigen::Matrix3f K = this->toK_(); + tvr = new TwoViewReconstruction(K); + } + + // Correct FishEye distortion + std::vector vKeysUn1 = vKeys1, vKeysUn2 = vKeys2; + std::vector vPts1(vKeys1.size()), vPts2(vKeys2.size()); + + for (size_t i = 0; i < vKeys1.size(); i++) + vPts1[i] = vKeys1[i].pt; + for (size_t i = 0; i < vKeys2.size(); i++) + vPts2[i] = vKeys2[i].pt; + + cv::Mat D = (cv::Mat_(4, 1) << mvParameters[4], mvParameters[5], mvParameters[6], mvParameters[7]); + cv::Mat R = cv::Mat::eye(3, 3, CV_32F); + cv::Mat K = this->toK(); + cv::fisheye::undistortPoints(vPts1, vPts1, K, D, R, K); + cv::fisheye::undistortPoints(vPts2, vPts2, K, D, R, K); + + for (size_t i = 0; i < vKeys1.size(); i++) + vKeysUn1[i].pt = vPts1[i]; + for (size_t i = 0; i < vKeys2.size(); i++) + vKeysUn2[i].pt = vPts2[i]; + + return tvr->Reconstruct(vKeysUn1, vKeysUn2, vMatches12, T21, vP3D, vbTriangulated); +} + +/** + * @brief 返回内参矩阵 + * @return K + */ +cv::Mat KannalaBrandt8::toK() +{ + cv::Mat K = (cv::Mat_(3, 3) << + mvParameters[0], 0.f, mvParameters[2], + 0.f, mvParameters[1], mvParameters[3], + 0.f, 0.f, 1.f); + return K; +} +Eigen::Matrix3f KannalaBrandt8::toK_() +{ + Eigen::Matrix3f K; + K << mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f; + return K; +} + +bool KannalaBrandt8::epipolarConstrain(GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, + const Eigen::Matrix3f &R12, const Eigen::Vector3f &t12, const float sigmaLevel, const float unc) +{ + Eigen::Vector3f p3D; + // 用三角化出点并验证的这个过程代替极线验证 + return this->TriangulateMatches(pCamera2, kp1, kp2, R12, t12, sigmaLevel, unc, p3D) > 0.0001f; +} + +// 没用 +bool KannalaBrandt8::matchAndtriangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, GeometricCamera *pOther, + Sophus::SE3f &Tcw1, Sophus::SE3f &Tcw2, + const float sigmaLevel1, const float sigmaLevel2, + Eigen::Vector3f &x3Dtriangulated) +{ + Eigen::Matrix eigTcw1 = Tcw1.matrix3x4(); + Eigen::Matrix3f Rcw1 = eigTcw1.block<3, 3>(0, 0); + Eigen::Matrix3f Rwc1 = Rcw1.transpose(); + Eigen::Matrix eigTcw2 = Tcw2.matrix3x4(); + Eigen::Matrix3f Rcw2 = eigTcw2.block<3, 3>(0, 0); + Eigen::Matrix3f Rwc2 = Rcw2.transpose(); + + cv::Point3f ray1c = this->unproject(kp1.pt); + cv::Point3f ray2c = pOther->unproject(kp2.pt); + + // 获得点1在帧1的归一化坐标 + Eigen::Vector3f r1(ray1c.x, ray1c.y, ray1c.z); + // 获得点2在帧2的归一化坐标 + Eigen::Vector3f r2(ray2c.x, ray2c.y, ray2c.z); + + // Check parallax between rays + // 射线在世界坐标系下 + Eigen::Vector3f ray1 = Rwc1 * r1; + Eigen::Vector3f ray2 = Rwc2 * r2; + + const float cosParallaxRays = ray1.dot(ray2) / (ray1.norm() * ray2.norm()); + + // If parallax is lower than 0.9998, reject this match + // 夹角几乎为0时返回，因为表示这个点过远，三角化会带来大量误差 + if (cosParallaxRays > 0.9998) + { + return false; + } + + // Parallax is good, so we try to triangulate + cv::Point2f p11, p22; + + p11.x = ray1c.x; + p11.y = ray1c.y; + + p22.x = ray2c.x; + p22.y = ray2c.y; + + Eigen::Vector3f x3D; + + // 三角化 + Triangulate(p11, p22, eigTcw1, eigTcw2, x3D); + + // Check triangulation in front of cameras + // 查看点是否位于相机前面 + float z1 = Rcw1.row(2).dot(x3D) + Tcw1.translation()(2); + if (z1 <= 0) + { // Point is not in front of the first camera + return false; + } + + float z2 = Rcw2.row(2).dot(x3D) + Tcw2.translation()(2); + if (z2 <= 0) + { // Point is not in front of the first camera + return false; + } + + // Check reprojection error in first keyframe + // -Transform point into camera reference system + // 查看投影误差 + Eigen::Vector3f x3D1 = Rcw1 * x3D + Tcw1.translation(); + Eigen::Vector2f uv1 = this->project(x3D1); + + float errX1 = uv1(0) - kp1.pt.x; + float errY1 = uv1(1) - kp1.pt.y; + + if ((errX1 * errX1 + errY1 * errY1) > 5.991 * sigmaLevel1) + { // Reprojection error is high + return false; + } + + // Check reprojection error in second keyframe; + // -Transform point into camera reference system + Eigen::Vector3f x3D2 = Rcw2 * x3D + Tcw2.translation(); // avoid using q + Eigen::Vector2f uv2 = pOther->project(x3D2); + + float errX2 = uv2(0) - kp2.pt.x; + float errY2 = uv2(1) - kp2.pt.y; + + if ((errX2 * errX2 + errY2 * errY2) > 5.991 * sigmaLevel2) + { // Reprojection error is high + return false; + } + + // Since parallax is big enough and reprojection errors are low, this pair of points + // can be considered as a match + x3Dtriangulated = x3D; + + return true; +} + +/** + * @brief 通过三角化后的重投影误差判断点匹配的情况，如果比较好则返回深度值 + * @param pCamera2 右相机 + * @param kp1 左相机特征点 + * @param kp2 右相机特征点 + * @param R12 2->1的旋转 + * @param t12 2->1的平移 + * @param sigmaLevel 特征点1的尺度的平方 + * @param unc 特征点2的尺度的平方 + * @param p3D 恢复的三维点 + * @return 点的深度值 + */ +float KannalaBrandt8::TriangulateMatches( + GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, + const Eigen::Matrix3f &R12, const Eigen::Vector3f &t12, const float sigmaLevel, + const float unc, Eigen::Vector3f &p3D) +{ + // 1. 得到对应特征点的归一化平面坐标 + Eigen::Vector3f r1 = this->unprojectEig(kp1.pt); + Eigen::Vector3f r2 = pCamera2->unprojectEig(kp2.pt); + + // Check parallax + // 2. 查看射线夹角 + // 这里有点像极线约束，但并不是，将r2通过R12旋转到与r1同方向的坐标系 + // 然后计算他们的夹角，看其是否超过1.14° + Eigen::Vector3f r21 = R12 * r2; + + const float cosParallaxRays = r1.dot(r21) / (r1.norm() * r21.norm()); + + if (cosParallaxRays > 0.9998) + { + return -1; + } + + // Parallax is good, so we try to triangulate + cv::Point2f p11, p22; + + p11.x = r1[0]; + p11.y = r1[1]; + + p22.x = r2[0]; + p22.y = r2[1]; + + Eigen::Vector3f x3D; + Eigen::Matrix Tcw1; + + // 3. 设定变换矩阵用于三角化 + Tcw1 << Eigen::Matrix3f::Identity(), Eigen::Vector3f::Zero(); + + Eigen::Matrix Tcw2; + + Eigen::Matrix3f R21 = R12.transpose(); + Tcw2 << R21, -R21 * t12; + + // 4. 三角化 + Triangulate(p11, p22, Tcw1, Tcw2, x3D); + // cv::Mat x3Dt = x3D.t(); + + // 深度值是否正常 + float z1 = x3D(2); + if (z1 <= 0) + { + return -2; + } + + float z2 = R21.row(2).dot(x3D) + Tcw2(2, 3); + if (z2 <= 0) + { + return -3; + } + + // Check reprojection error + // 5. 做下投影计算重投影误差 + Eigen::Vector2f uv1 = this->project(x3D); + + float errX1 = uv1(0) - kp1.pt.x; + float errY1 = uv1(1) - kp1.pt.y; + + if ((errX1 * errX1 + errY1 * errY1) > 5.991 * sigmaLevel) + { // Reprojection error is high + return -4; + } + + Eigen::Vector3f x3D2 = R21 * x3D + Tcw2.col(3); + Eigen::Vector2f uv2 = pCamera2->project(x3D2); + + float errX2 = uv2(0) - kp2.pt.x; + float errY2 = uv2(1) - kp2.pt.y; + + if ((errX2 * errX2 + errY2 * errY2) > 5.991 * unc) + { // Reprojection error is high + return -5; + } + + p3D = x3D; + + return z1; +} + +std::ostream &operator<<(std::ostream &os, const KannalaBrandt8 &kb) +{ + os << kb.mvParameters[0] << " " << kb.mvParameters[1] << " " << kb.mvParameters[2] << " " << kb.mvParameters[3] << " " + << kb.mvParameters[4] << " " << kb.mvParameters[5] << " " << kb.mvParameters[6] << " " << kb.mvParameters[7]; + return os; +} + +std::istream &operator>>(std::istream &is, KannalaBrandt8 &kb) +{ + float nextParam; + for (size_t i = 0; i < 8; i++) + { + assert(is.good()); // Make sure the input stream is good + is >> nextParam; + kb.mvParameters[i] = nextParam; + } + return is; +} + +/** + * @brief 通过三角化恢复归一化坐标的三维点坐标，该三维点为在左相机坐标系下的点 + * @param p1 右相机 + * @param p2 左相机特征点 + * @param Tcw1 3×4 单位矩阵 + * @param Tcw2 3×4 T21 + * @param x3D 恢复的三维点 + */ +void KannalaBrandt8::Triangulate( + const cv::Point2f &p1, const cv::Point2f &p2, const Eigen::Matrix &Tcw1, + const Eigen::Matrix &Tcw2, Eigen::Vector3f &x3D) +{ + Eigen::Matrix A; + // 代码中有用到三角化的地方有：TwoViewReconstruction::Triangulate LocalMapping::CreateNewMapPoints KannalaBrandt8::Triangulate Initializer::Triangulate + // 见Initializer.cpp的Triangulate函数,A矩阵构建的方式类似，不同的是乘的反对称矩阵那个是像素坐标构成的，而这个是归一化坐标构成的 + // Pc = Tcw*Pw， 左右两面乘Pc的反对称矩阵 [Pc]x * Tcw *Pw = 0 构成了A矩阵，中间涉及一个尺度a，因为都是归一化平面，但右面是0所以直接可以约掉不影响最后的尺度 + // 0 -1 y Tcw.row(0) -Tcw.row(1) + y*Tcw.row(2) + // 1 0 -x * Tcw.row(1) = Tcw.row(0) - x*Tcw.row(2) + // -y x 0 Tcw.row(2) x*Tcw.row(1) - y*Tcw.row(0) + // 发现上述矩阵线性相关，所以取前两维，两个点构成了4行的矩阵，就是如下的操作，求出的是4维的结果[X,Y,Z,A]，所以需要除以最后一维使之为1，就成了[X,Y,Z,1]这种齐次形式 + A.row(0) = p1.x * Tcw1.row(2) - Tcw1.row(0); + A.row(1) = p1.y * Tcw1.row(2) - Tcw1.row(1); + A.row(2) = p2.x * Tcw2.row(2) - Tcw2.row(0); + A.row(3) = p2.y * Tcw2.row(2) - Tcw2.row(1); + + Eigen::JacobiSVD svd(A, Eigen::ComputeFullV); + Eigen::Vector4f x3Dh = svd.matrixV().col(3); + x3D = x3Dh.head(3) / x3Dh(3); +} + +bool KannalaBrandt8::IsEqual(GeometricCamera *pCam) +{ + if (pCam->GetType() != GeometricCamera::CAM_FISHEYE) + return false; + + KannalaBrandt8 *pKBCam = (KannalaBrandt8 *)pCam; + + if (abs(precision - pKBCam->GetPrecision()) > 1e-6) + return false; + + if (size() != pKBCam->size()) + return false; + + bool is_same_camera = true; + for (size_t i = 0; i < size(); ++i) + { + if (abs(mvParameters[i] - pKBCam->getParameter(i)) > 1e-6) + { + is_same_camera = false; + break; + } + } + return is_same_camera; +} + +} diff --git a/ORB-SLAM3-UESTC/Workspace/src/CameraModels/Pinhole.cpp b/ORB-SLAM3-UESTC/Workspace/src/CameraModels/Pinhole.cpp new file mode 100644 index 0000000..6b9dfeb --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/CameraModels/Pinhole.cpp @@ -0,0 +1,256 @@ +/** + * This file is part of ORB-SLAM3 + * + * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * + * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public + * License as published by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even + * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with ORB-SLAM3. + * If not, see . + */ + +#include "Pinhole.h" + +#include + +// BOOST_CLASS_EXPORT_IMPLEMENT(ORB_SLAM3::Pinhole) + +namespace ORB_SLAM3 +{ +// BOOST_CLASS_EXPORT_GUID(Pinhole, "Pinhole") + +long unsigned int GeometricCamera::nNextId = 0; + +/** + * @brief 相机坐标系下的三维点投影到无畸变像素平面 + * @param p3D 三维点 + * @return 像素坐标 + */ +cv::Point2f Pinhole::project(const cv::Point3f &p3D) +{ + return cv::Point2f(mvParameters[0] * p3D.x / p3D.z + mvParameters[2], + mvParameters[1] * p3D.y / p3D.z + mvParameters[3]); +} + +/** + * @brief 相机坐标系下的三维点投影到无畸变像素平面 + * @param v3D 三维点 + * @return 像素坐标 + */ +Eigen::Vector2d Pinhole::project(const Eigen::Vector3d &v3D) +{ + Eigen::Vector2d res; + res[0] = mvParameters[0] * v3D[0] / v3D[2] + mvParameters[2]; + res[1] = mvParameters[1] * v3D[1] / v3D[2] + mvParameters[3]; + + return res; +} + +/** + * @brief 相机坐标系下的三维点投影到无畸变像素平面 + * @param v3D 三维点 + * @return 像素坐标 + */ +Eigen::Vector2f Pinhole::project(const Eigen::Vector3f &v3D) +{ + Eigen::Vector2f res; + res[0] = mvParameters[0] * v3D[0] / v3D[2] + mvParameters[2]; + res[1] = mvParameters[1] * v3D[1] / v3D[2] + mvParameters[3]; + + return res; +} + +/** + * @brief 相机坐标系下的三维点投影到无畸变像素平面 + * @param p3D 三维点 + * @return 像素坐标 + */ +Eigen::Vector2f Pinhole::projectMat(const cv::Point3f &p3D) +{ + cv::Point2f point = this->project(p3D); + return Eigen::Vector2f(point.x, point.y); +} + +/** + * @brief 貌似是调试遗留的产物 + */ +float Pinhole::uncertainty2(const Eigen::Matrix &p2D) +{ + return 1.0; +} + +/** + * @brief 反投影 + * @param p2D 特征点 + * @return 归一化坐标 + */ +Eigen::Vector3f Pinhole::unprojectEig(const cv::Point2f &p2D) +{ + return Eigen::Vector3f( + (p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1], + 1.f); +} + +/** + * @brief 反投影 + * @param p2D 特征点 + * @return 归一化坐标 + */ +cv::Point3f Pinhole::unproject(const cv::Point2f &p2D) +{ + return cv::Point3f( + (p2D.x - mvParameters[2]) / mvParameters[0], + (p2D.y - mvParameters[3]) / mvParameters[1], + 1.f); +} + +/** + * @brief 求解二维像素坐标关于三维点坐标的雅克比矩阵 + * @param v3D 三维点 + * @return + */ +Eigen::Matrix Pinhole::projectJac(const Eigen::Vector3d &v3D) +{ + Eigen::Matrix Jac; + Jac(0, 0) = mvParameters[0] / v3D[2]; + Jac(0, 1) = 0.f; + Jac(0, 2) = -mvParameters[0] * v3D[0] / (v3D[2] * v3D[2]); + Jac(1, 0) = 0.f; + Jac(1, 1) = mvParameters[1] / v3D[2]; + Jac(1, 2) = -mvParameters[1] * v3D[1] / (v3D[2] * v3D[2]); + + return Jac; +} + +/** 三角化恢复三维点 单目初始化时使用 + * @param vKeys1 第一帧的关键点 + * @param vKeys2 第二帧的关键点 + * @param vMatches12 匹配关系，长度与vKeys1一样，对应位置存放vKeys2中关键点的下标 + * @param R21 顾名思义 + * @param t21 顾名思义 + * @param vP3D 恢复出的三维点 + * @param vbTriangulated 是否三角化成功，用于统计匹配点数量 + */ +bool Pinhole::ReconstructWithTwoViews(const std::vector &vKeys1, const std::vector &vKeys2, const std::vector &vMatches12, + Sophus::SE3f &T21, std::vector &vP3D, std::vector &vbTriangulated) +{ + if (!tvr) + { + Eigen::Matrix3f K = this->toK_(); + tvr = new TwoViewReconstruction(K); + } + + return tvr->Reconstruct(vKeys1, vKeys2, vMatches12, T21, vP3D, vbTriangulated); +} + +/** + * @brief 返回内参矩阵 + * @return K + */ +cv::Mat Pinhole::toK() +{ + cv::Mat K = (cv::Mat_(3, 3) << mvParameters[0], + 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f); + return K; +} + +/** + * @brief 返回内参矩阵 + * @return K + */ +Eigen::Matrix3f Pinhole::toK_() +{ + Eigen::Matrix3f K; + K << mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f; + return K; +} + +/** + * @brief 极线约束 + * @param pCamera2 右相机 + * @param kp1 左相机特征点 + * @param kp2 右相机特征点 + * @param R12 2->1的旋转 + * @param t12 2->1的平移 + * @param sigmaLevel 特征点1的尺度的平方 + * @param unc 特征点2的尺度的平方，1.2^2n + * @return 三维点恢复的成功与否 + */ +bool Pinhole::epipolarConstrain( + GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, + const Eigen::Matrix3f &R12, const Eigen::Vector3f &t12, const float sigmaLevel, const float unc) +{ + // Compute Fundamental Matrix + Eigen::Matrix3f t12x = Sophus::SO3f::hat(t12); + Eigen::Matrix3f K1 = this->toK_(); + Eigen::Matrix3f K2 = pCamera2->toK_(); + Eigen::Matrix3f F12 = K1.transpose().inverse() * t12x * R12 * K2.inverse(); + + // Epipolar line in second image l = x1'F12 = [a b c] + // u2, + // (u1, v1, 1) * F12 * (v2,) = 0 --> (a, b, c) * (u2, v2, 1)^t = 0 --> a*u2 + b*v2 + c = 0 + // 1 + const float a = kp1.pt.x * F12(0, 0) + kp1.pt.y * F12(1, 0) + F12(2, 0); + const float b = kp1.pt.x * F12(0, 1) + kp1.pt.y * F12(1, 1) + F12(2, 1); + const float c = kp1.pt.x * F12(0, 2) + kp1.pt.y * F12(1, 2) + F12(2, 2); + + // 点到直线距离的公式 + // d = |a*u2 + b*v2 + c| / sqrt(a^2 + b^2) + const float num = a * kp2.pt.x + b * kp2.pt.y + c; + + const float den = a * a + b * b; + + if (den == 0) + return false; + + const float dsqr = num * num / den; + + return dsqr < 3.84 * unc; +} + +std::ostream &operator<<(std::ostream &os, const Pinhole &ph) +{ + os << ph.mvParameters[0] << " " << ph.mvParameters[1] << " " << ph.mvParameters[2] << " " << ph.mvParameters[3]; + return os; +} +std::istream & operator>>(std::istream &is, Pinhole &ph) +{ + float nextParam; + for(size_t i = 0; i < 4; i++){ + assert(is.good()); //Make sure the input stream is good + is >> nextParam; + ph.mvParameters[i] = nextParam; + + } + return is; +} + +bool Pinhole::IsEqual(GeometricCamera *pCam) +{ + if (pCam->GetType() != GeometricCamera::CAM_PINHOLE) + return false; + + Pinhole *pPinholeCam = (Pinhole *)pCam; + + if (size() != pPinholeCam->size()) + return false; + + bool is_same_camera = true; + for (size_t i = 0; i < size(); ++i) + { + if (abs(mvParameters[i] - pPinholeCam->getParameter(i)) > 1e-6) + { + is_same_camera = false; + break; + } + } + return is_same_camera; +} +} diff --git a/ORB-SLAM3-UESTC/Workspace/src/Config.cc b/ORB-SLAM3-UESTC/Workspace/src/Config.cc new file mode 100644 index 0000000..aa39f2a --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/Config.cc @@ -0,0 +1,30 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#include "Config.h" + +namespace ORB_SLAM3 +{ + +bool ConfigParser::ParseConfigFile(std::string &strConfigFile) +{ + return true; +} + +} diff --git a/ORB-SLAM3-UESTC/Workspace/src/Converter.cc b/ORB-SLAM3-UESTC/Workspace/src/Converter.cc new file mode 100644 index 0000000..bf2328e --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/Converter.cc @@ -0,0 +1,415 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#include "Converter.h" + +namespace ORB_SLAM3 +{ + +/** + * @brief 将描述子转换为描述子向量，其实本质上是cv:Mat->std:vector + */ +std::vector Converter::toDescriptorVector(const cv::Mat &Descriptors) +{ + // 存储转换结果的向量 + std::vector vDesc; + // 创建保留空间 + vDesc.reserve(Descriptors.rows); + // 对于每一个特征点的描述子 + for (int j=0;jg2o::SE3Quat + */ +g2o::SE3Quat Converter::toSE3Quat(const cv::Mat &cvT) +{ + // 首先将旋转矩阵提取出来 + Eigen::Matrix R; + R << cvT.at(0,0), cvT.at(0,1), cvT.at(0,2), + cvT.at(1,0), cvT.at(1,1), cvT.at(1,2), + cvT.at(2,0), cvT.at(2,1), cvT.at(2,2); + + // 然后将平移向量提取出来 + Eigen::Matrix t(cvT.at(0,3), cvT.at(1,3), cvT.at(2,3)); + // 构造g2o::SE3Quat类型并返回 + return g2o::SE3Quat(R,t); +} + +/** + * @brief ophus::SE3f->g2o::SE3Quat + */ +g2o::SE3Quat Converter::toSE3Quat(const Sophus::SE3f &T) +{ + return g2o::SE3Quat(T.unit_quaternion().cast(), T.translation().cast()); +} + +/** + * @brief g2o::SE3Quat->cv::Mat + */ +cv::Mat Converter::toCvMat(const g2o::SE3Quat &SE3) +{ + Eigen::Matrix eigMat = SE3.to_homogeneous_matrix(); + return toCvMat(eigMat); +} + +/** + * @brief 将仿射矩阵由g2o::Sim3->cv::Mat + */ +cv::Mat Converter::toCvMat(const g2o::Sim3 &Sim3) +{ + // 首先将仿射矩阵的旋转部分转换成为Eigen下的矩阵格式 + Eigen::Matrix3d eigR = Sim3.rotation().toRotationMatrix(); + // 对于仿射矩阵的平移部分也是要转换成为Eigen下的矩阵格式 + Eigen::Vector3d eigt = Sim3.translation(); + // 获取仿射矩阵的缩放系数 + double s = Sim3.scale(); + // 然后构造cv::Mat格式下的仿射矩阵 + return toCvSE3(s*eigR,eigt); +} + +/** + * @brief Eigen::Matrix -> cv::Mat，用于变换矩阵T的中间转换 + */ +cv::Mat Converter::toCvMat(const Eigen::Matrix &m) +{ + // 首先定义存储计算结果的变量 + cv::Mat cvMat(4,4,CV_32F); + // 然后逐个元素赋值 + for(int i=0;i<4;i++) + for(int j=0; j<4; j++) + cvMat.at(i,j)=m(i,j); + // 返回计算结果，还是用深拷贝函数 + return cvMat.clone(); +} + +/** + * @brief Eigen::Matrix -> cv::Mat，用于变换矩阵T的中间转换 + */ +cv::Mat Converter::toCvMat(const Eigen::Matrix &m) +{ + cv::Mat cvMat(4,4,CV_32F); + for(int i=0;i<4;i++) + for(int j=0; j<4; j++) + cvMat.at(i,j)=m(i,j); + + return cvMat.clone(); +} + +/** + * @brief Eigen::Matrix -> cv::Mat + */ +cv::Mat Converter::toCvMat(const Eigen::Matrix &m) +{ + cv::Mat cvMat(3,4,CV_32F); + for(int i=0;i<3;i++) + for(int j=0; j<4; j++) + cvMat.at(i,j)=m(i,j); + + return cvMat.clone(); +} + +/** + * @brief Eigen::Matrix -> cv::Mat + */ +cv::Mat Converter::toCvMat(const Eigen::Matrix3d &m) +{ + cv::Mat cvMat(3,3,CV_32F); + for(int i=0;i<3;i++) + for(int j=0; j<3; j++) + cvMat.at(i,j)=m(i,j); + + return cvMat.clone(); +} + +/** + * @brief Eigen::Matrix -> cv::Mat + */ +cv::Mat Converter::toCvMat(const Eigen::Matrix3f &m) +{ + cv::Mat cvMat(3,3,CV_32F); + for(int i=0;i<3;i++) + for(int j=0; j<3; j++) + cvMat.at(i,j)=m(i,j); + + return cvMat.clone(); +} + +/** + * @brief Eigen::MatrixXf -> cv::Mat + */ +cv::Mat Converter::toCvMat(const Eigen::MatrixXf &m) +{ + cv::Mat cvMat(m.rows(),m.cols(),CV_32F); + for(int i=0;i(i,j)=m(i,j); + + return cvMat.clone(); +} + +/** + * @brief Eigen::MatrixXd -> cv::Mat + */ +cv::Mat Converter::toCvMat(const Eigen::MatrixXd &m) +{ + cv::Mat cvMat(m.rows(),m.cols(),CV_32F); + for(int i=0;i(i,j)=m(i,j); + + return cvMat.clone(); +} + +/** + * @brief Eigen::Matrix -> cv::Mat + */ +cv::Mat Converter::toCvMat(const Eigen::Matrix &m) +{ + cv::Mat cvMat(3,1,CV_32F); + for(int i=0;i<3;i++) + cvMat.at(i)=m(i); + + return cvMat.clone(); +} + +/** + * @brief Eigen::Matrix -> cv::Mat + */ +cv::Mat Converter::toCvMat(const Eigen::Matrix &m) +{ + cv::Mat cvMat(3,1,CV_32F); + for(int i=0;i<3;i++) + cvMat.at(i)=m(i); + + return cvMat.clone(); +} + +/** + * @brief 将给定的旋转矩阵R和平移向量t转换成为 以cv::Mat格式存储的李群SE3（其实就是变换矩阵） + */ +cv::Mat Converter::toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t) +{ + cv::Mat cvMat = cv::Mat::eye(4,4,CV_32F); + // 将旋转矩阵复制到左上角 + for(int i=0;i<3;i++) + { + for(int j=0;j<3;j++) + { + cvMat.at(i,j)=R(i,j); + } + } + // 将旋转矩阵复制到最右侧的一列 + for(int i=0;i<3;i++) + { + cvMat.at(i,3)=t(i); + } + // 返回计算结果 + return cvMat.clone(); +} + +/** + * @brief 将OpenCV中Mat类型的向量转化为Eigen中Matrix类型的变量 + */ +Eigen::Matrix Converter::toVector3d(const cv::Mat &cvVector) +{ + // 首先生成用于存储转换结果的向量 + Eigen::Matrix v; + // 然后通过逐个赋值的方法完成转换 + v << cvVector.at(0), cvVector.at(1), cvVector.at(2); + // 返回转换结果 + return v; +} + +/** + * @brief 类似上面 + */ +Eigen::Matrix Converter::toVector3f(const cv::Mat &cvVector) +{ + Eigen::Matrix v; + v << cvVector.at(0), cvVector.at(1), cvVector.at(2); + + return v; +} + +/** + * @brief 类似上面 + */ +Eigen::Matrix Converter::toVector3d(const cv::Point3f &cvPoint) +{ + Eigen::Matrix v; + v << cvPoint.x, cvPoint.y, cvPoint.z; + + return v; +} + +/** + * @brief cv::Mat -> Eigen::Matrix + */ +Eigen::Matrix Converter::toMatrix3d(const cv::Mat &cvMat3) +{ + Eigen::Matrix M; + + M << cvMat3.at(0,0), cvMat3.at(0,1), cvMat3.at(0,2), + cvMat3.at(1,0), cvMat3.at(1,1), cvMat3.at(1,2), + cvMat3.at(2,0), cvMat3.at(2,1), cvMat3.at(2,2); + + return M; +} + +/** + * @brief cv::Mat -> Eigen::Matrix + */ +Eigen::Matrix Converter::toMatrix4d(const cv::Mat &cvMat4) +{ + Eigen::Matrix M; + + M << cvMat4.at(0,0), cvMat4.at(0,1), cvMat4.at(0,2), cvMat4.at(0,3), + cvMat4.at(1,0), cvMat4.at(1,1), cvMat4.at(1,2), cvMat4.at(1,3), + cvMat4.at(2,0), cvMat4.at(2,1), cvMat4.at(2,2), cvMat4.at(2,3), + cvMat4.at(3,0), cvMat4.at(3,1), cvMat4.at(3,2), cvMat4.at(3,3); + return M; +} + +/** + * @brief cv::Mat -> Eigen::Matrix + */ +Eigen::Matrix Converter::toMatrix3f(const cv::Mat &cvMat3) +{ + Eigen::Matrix M; + + M << cvMat3.at(0,0), cvMat3.at(0,1), cvMat3.at(0,2), + cvMat3.at(1,0), cvMat3.at(1,1), cvMat3.at(1,2), + cvMat3.at(2,0), cvMat3.at(2,1), cvMat3.at(2,2); + + return M; +} + +/** + * @brief cv::Mat -> Eigen::Matrix + */ +Eigen::Matrix Converter::toMatrix4f(const cv::Mat &cvMat4) +{ + Eigen::Matrix M; + + M << cvMat4.at(0,0), cvMat4.at(0,1), cvMat4.at(0,2), cvMat4.at(0,3), + cvMat4.at(1,0), cvMat4.at(1,1), cvMat4.at(1,2), cvMat4.at(1,3), + cvMat4.at(2,0), cvMat4.at(2,1), cvMat4.at(2,2), cvMat4.at(2,3), + cvMat4.at(3,0), cvMat4.at(3,1), cvMat4.at(3,2), cvMat4.at(3,3); + return M; +} + +/** + * @brief 旋转矩阵转四元数以vector形式输出 + */ +std::vector Converter::toQuaternion(const cv::Mat &M) +{ + Eigen::Matrix eigMat = toMatrix3d(M); + Eigen::Quaterniond q(eigMat); + + std::vector v(4); + v[0] = q.x(); + v[1] = q.y(); + v[2] = q.z(); + v[3] = q.w(); + + return v; +} + +/** + * @brief 反对称 + */ +cv::Mat Converter::tocvSkewMatrix(const cv::Mat &v) +{ + return (cv::Mat_(3,3) << + 0, -v.at(2), v.at(1), + v.at(2), 0, -v.at(0), + -v.at(1), v.at(0), 0); +} + +/** + * @brief 判断是否为旋转矩阵 + */ +bool Converter::isRotationMatrix(const cv::Mat &R) +{ + cv::Mat Rt; + cv::transpose(R, Rt); + cv::Mat shouldBeIdentity = Rt * R; + cv::Mat I = cv::Mat::eye(3,3, shouldBeIdentity.type()); + + return cv::norm(I, shouldBeIdentity) < 1e-6; + +} + +/** + * @brief 旋转矩阵转欧拉角 + */ +std::vector Converter::toEuler(const cv::Mat &R) +{ + assert(isRotationMatrix(R)); + float sy = sqrt(R.at(0,0) * R.at(0,0) + R.at(1,0) * R.at(1,0) ); + + bool singular = sy < 1e-6; // If + + float x, y, z; + if (!singular) + { + x = atan2(R.at(2,1) , R.at(2,2)); + y = atan2(-R.at(2,0), sy); + z = atan2(R.at(1,0), R.at(0,0)); + } + else + { + x = atan2(-R.at(1,2), R.at(1,1)); + y = atan2(-R.at(2,0), sy); + z = 0; + } + + std::vector v_euler(3); + v_euler[0] = x; + v_euler[1] = y; + v_euler[2] = z; + + return v_euler; +} + +/** + * @brief 转成Sophus::SE3 + */ +Sophus::SE3 Converter::toSophus(const cv::Mat &T) { + Eigen::Matrix eigMat = toMatrix3d(T.rowRange(0,3).colRange(0,3)); + Eigen::Quaternionf q(eigMat.cast()); + + Eigen::Matrix t = toVector3d(T.rowRange(0,3).col(3)).cast(); + + return Sophus::SE3(q,t); +} + +/** + * @brief 转成Sophus::Sim3f + */ +Sophus::Sim3f Converter::toSophus(const g2o::Sim3& S) { + return Sophus::Sim3f(Sophus::RxSO3d((float)S.scale(), S.rotation().matrix()).cast() , + S.translation().cast()); +} + +} //namespace ORB_SLAM diff --git a/ORB-SLAM3-UESTC/Workspace/src/Frame.cc b/ORB-SLAM3-UESTC/Workspace/src/Frame.cc new file mode 100644 index 0000000..9b71df3 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/Frame.cc @@ -0,0 +1,1697 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#include "Frame.h" + +#include "G2oTypes.h" +#include "MapPoint.h" +#include "KeyFrame.h" +#include "ORBextractor.h" +#include "Converter.h" +#include "ORBmatcher.h" +#include "GeometricCamera.h" + +#include +#include +#include + +namespace ORB_SLAM3 +{ + +// 下一个生成的帧的ID,这里是初始化类的静态成员变量 +long unsigned int Frame::nNextId=0; + +// 是否要进行初始化操作的标志 +// 这里给这个标志置位的操作是在最初系统开始加载到内存的时候进行的，下一帧就是整个系统的第一帧，所以这个标志要置位 +bool Frame::mbInitialComputations=true; +float Frame::cx, Frame::cy, Frame::fx, Frame::fy, Frame::invfx, Frame::invfy; +float Frame::mnMinX, Frame::mnMinY, Frame::mnMaxX, Frame::mnMaxY; +float Frame::mfGridElementWidthInv, Frame::mfGridElementHeightInv; + +//For stereo fisheye matching +cv::BFMatcher Frame::BFmatcher = cv::BFMatcher(cv::NORM_HAMMING); + +Frame::Frame(): mpcpi(NULL), mpImuPreintegrated(NULL), mpPrevFrame(NULL), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbIsSet(false), mbImuPreintegrated(false), mbHasPose(false), mbHasVelocity(false) +{ +#ifdef REGISTER_TIMES + mTimeStereoMatch = 0; + mTimeORB_Ext = 0; +#endif +} + + +//Copy Constructor +Frame::Frame(const Frame &frame) + :mpcpi(frame.mpcpi),mpORBvocabulary(frame.mpORBvocabulary), mpORBextractorLeft(frame.mpORBextractorLeft), mpORBextractorRight(frame.mpORBextractorRight), + mTimeStamp(frame.mTimeStamp), mK(frame.mK.clone()), mK_(Converter::toMatrix3f(frame.mK)), mDistCoef(frame.mDistCoef.clone()), + mbf(frame.mbf), mb(frame.mb), mThDepth(frame.mThDepth), N(frame.N), mvKeys(frame.mvKeys), + mvKeysRight(frame.mvKeysRight), mvKeysUn(frame.mvKeysUn), mvuRight(frame.mvuRight), + mvDepth(frame.mvDepth), mBowVec(frame.mBowVec), mFeatVec(frame.mFeatVec), + mDescriptors(frame.mDescriptors.clone()), mDescriptorsRight(frame.mDescriptorsRight.clone()), + mvpMapPoints(frame.mvpMapPoints), mvbOutlier(frame.mvbOutlier), mImuCalib(frame.mImuCalib), mnCloseMPs(frame.mnCloseMPs), + mpImuPreintegrated(frame.mpImuPreintegrated), mpImuPreintegratedFrame(frame.mpImuPreintegratedFrame), mImuBias(frame.mImuBias), + mnId(frame.mnId), mpReferenceKF(frame.mpReferenceKF), mnScaleLevels(frame.mnScaleLevels), + mfScaleFactor(frame.mfScaleFactor), mfLogScaleFactor(frame.mfLogScaleFactor), + mvScaleFactors(frame.mvScaleFactors), mvInvScaleFactors(frame.mvInvScaleFactors), mNameFile(frame.mNameFile), mnDataset(frame.mnDataset), + mvLevelSigma2(frame.mvLevelSigma2), mvInvLevelSigma2(frame.mvInvLevelSigma2), mpPrevFrame(frame.mpPrevFrame), mpLastKeyFrame(frame.mpLastKeyFrame), + mbIsSet(frame.mbIsSet), mbImuPreintegrated(frame.mbImuPreintegrated), mpMutexImu(frame.mpMutexImu), + mpCamera(frame.mpCamera), mpCamera2(frame.mpCamera2), Nleft(frame.Nleft), Nright(frame.Nright), + monoLeft(frame.monoLeft), monoRight(frame.monoRight), mvLeftToRightMatch(frame.mvLeftToRightMatch), + mvRightToLeftMatch(frame.mvRightToLeftMatch), mvStereo3Dpoints(frame.mvStereo3Dpoints), + mTlr(frame.mTlr), mRlr(frame.mRlr), mtlr(frame.mtlr), mTrl(frame.mTrl), + mTcw(frame.mTcw), mbHasPose(false), mbHasVelocity(false) +{ + for(int i=0;i 0){ + mGridRight[i][j] = frame.mGridRight[i][j]; + } + } + + if(frame.mbHasPose) + SetPose(frame.GetPose()); + + if(frame.HasVelocity()) + { + SetVelocity(frame.GetVelocity()); + } + + mmProjectPoints = frame.mmProjectPoints; + mmMatchedInImage = frame.mmMatchedInImage; + +#ifdef REGISTER_TIMES + mTimeStereoMatch = frame.mTimeStereoMatch; + mTimeORB_Ext = frame.mTimeORB_Ext; +#endif +} + +// 立体匹配模式下的双目 +Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, Frame* pPrevF, const IMU::Calib &ImuCalib) + :mpcpi(NULL), mpORBvocabulary(voc),mpORBextractorLeft(extractorLeft),mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()), mK_(Converter::toMatrix3f(K)), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), + mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF),mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbIsSet(false), mbImuPreintegrated(false), + mpCamera(pCamera) ,mpCamera2(nullptr), mbHasPose(false), mbHasVelocity(false) +{ + // Frame ID + // Step 1 帧的ID 自增 + mnId=nNextId++; + + // Scale Level Info + // Step 2 计算图像金字塔的参数 + // 获取图像金字塔的层数 + mnScaleLevels = mpORBextractorLeft->GetLevels(); + // 获得层与层之间的缩放比 + mfScaleFactor = mpORBextractorLeft->GetScaleFactor(); + // 计算上面缩放比的对数 + mfLogScaleFactor = log(mfScaleFactor); + // 获取每层图像的缩放因子 + mvScaleFactors = mpORBextractorLeft->GetScaleFactors(); + // 同样获取每层图像缩放因子的倒数 + mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors(); + // 高斯模糊的时候，使用的方差 + mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares(); + // 获取sigma^2的倒数 + mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares(); + + // ORB extraction +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); +#endif + // Step 3 对左目右目图像提取ORB特征点, 第一个参数0-左图， 1-右图。为加速计算，同时开了两个线程计算 + thread threadLeft(&Frame::ExtractORB,this,0,imLeft,0,0); + // 对右目图像提取orb特征 + thread threadRight(&Frame::ExtractORB,this,1,imRight,0,0); + // 等待两张图像特征点提取过程完成 + threadLeft.join(); + threadRight.join(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now(); + + mTimeORB_Ext = std::chrono::duration_cast >(time_EndExtORB - time_StartExtORB).count(); +#endif + + // mvKeys中保存的是左图像中的特征点，这里是获取左侧图像中特征点的个数 + N = mvKeys.size(); + + // 如果左图像中没有成功提取到特征点那么就返回，也意味这这一帧的图像无法使用 + if(mvKeys.empty()) + return; + + // Step 4 用OpenCV的矫正函数、内参对提取到的特征点进行矫正 + UndistortKeyPoints(); + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartStereoMatches = std::chrono::steady_clock::now(); +#endif + + // Step 5 计算双目间特征点的匹配，只有匹配成功的特征点会计算其深度,深度存放在 mvDepth + // mvuRight中存储的应该是左图像中的点所匹配的在右图像中的点的横坐标（纵坐标相同） + ComputeStereoMatches(); + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndStereoMatches = std::chrono::steady_clock::now(); + + mTimeStereoMatch = std::chrono::duration_cast >(time_EndStereoMatches - time_StartStereoMatches).count(); +#endif + + // 初始化本帧的地图点 + mvpMapPoints = vector(N,static_cast(NULL)); + mvbOutlier = vector(N,false); + mmProjectPoints.clear(); + mmMatchedInImage.clear(); + + + // This is done only for the first Frame (or after a change in the calibration) + // Step 5 计算去畸变后图像边界，将特征点分配到网格中。这个过程一般是在第一帧或者是相机标定参数发生变化之后进行 + if(mbInitialComputations) + { + // 计算去畸变后图像的边界 + ComputeImageBounds(imLeft); + + // 表示一个图像像素相当于多少个图像网格列（宽） + mfGridElementWidthInv=static_cast(FRAME_GRID_COLS)/(mnMaxX-mnMinX); + // 表示一个图像像素相当于多少个图像网格行（高） + mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/(mnMaxY-mnMinY); + + + + fx = K.at(0,0); + fy = K.at(1,1); + cx = K.at(0,2); + cy = K.at(1,2); + // 猜测是因为这种除法计算需要的时间略长，所以这里直接存储了这个中间计算结果 + invfx = 1.0f/fx; + invfy = 1.0f/fy; + + // 特殊的初始化过程完成，标志复位 + mbInitialComputations=false; + } + + // 双目相机基线长度 + mb = mbf/fx; + + if(pPrevF) + { + if(pPrevF->HasVelocity()) + SetVelocity(pPrevF->GetVelocity()); + } + else + { + mVw.setZero(); + } + + mpMutexImu = new std::mutex(); + + //Set no stereo fisheye information + Nleft = -1; + Nright = -1; + mvLeftToRightMatch = vector(0); + mvRightToLeftMatch = vector(0); + mvStereo3Dpoints = vector(0); + monoLeft = -1; + monoRight = -1; + + // Step 6 将特征点分配到图像网格中 + // 上个版本这句话放在了new 锁那个上面，放在目前这个位置更合理，因为要把一些当前模式不用的参数赋值，函数里面要用 + AssignFeaturesToGrid(); +} + +// RGBD +Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera,Frame* pPrevF, const IMU::Calib &ImuCalib) + :mpcpi(NULL),mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast(NULL)), + mTimeStamp(timeStamp), mK(K.clone()), mK_(Converter::toMatrix3f(K)),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), + mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbIsSet(false), mbImuPreintegrated(false), + mpCamera(pCamera),mpCamera2(nullptr), mbHasPose(false), mbHasVelocity(false) +{ + // Frame ID + // Step 1 帧的ID 自增 + mnId=nNextId++; + + // Scale Level Info + // Step 2 计算图像金字塔的参数 + // 获取图像金字塔的层数 + mnScaleLevels = mpORBextractorLeft->GetLevels(); + // 获得层与层之间的缩放比 + mfScaleFactor = mpORBextractorLeft->GetScaleFactor(); + // 计算上面缩放比的对数 + mfLogScaleFactor = log(mfScaleFactor); + // 获取每层图像的缩放因子 + mvScaleFactors = mpORBextractorLeft->GetScaleFactors(); + // 同样获取每层图像缩放因子的倒数 + mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors(); + // 高斯模糊的时候，使用的方差 + mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares(); + // 获取sigma^2的倒数 + mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares(); + + // ORB extraction +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); +#endif + ExtractORB(0,imGray,0,0); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now(); + + mTimeORB_Ext = std::chrono::duration_cast >(time_EndExtORB - time_StartExtORB).count(); +#endif + + // 获取特征点的个数 + N = mvKeys.size(); + + // 如果这一帧没有能够提取出特征点，那么就直接返回了 + if(mvKeys.empty()) + return; + + // Step 4 用OpenCV的矫正函数、内参对提取到的特征点进行矫正 + UndistortKeyPoints(); + + // Step 5 获取图像的深度，并且根据这个深度推算其右图中匹配的特征点的视差 + ComputeStereoFromRGBD(imDepth); + + // 初始化本帧的地图点 + mvpMapPoints = vector(N,static_cast(NULL)); + + mmProjectPoints.clear(); + mmMatchedInImage.clear(); + + // 记录地图点是否为外点，初始化均为外点false + mvbOutlier = vector(N,false); + + // This is done only for the first Frame (or after a change in the calibration) + // Step 5 计算去畸变后图像边界，将特征点分配到网格中。这个过程一般是在第一帧或者是相机标定参数发生变化之后进行 + if(mbInitialComputations) + { + // 计算去畸变后图像的边界 + ComputeImageBounds(imGray); + + // 表示一个图像像素相当于多少个图像网格列（宽） + mfGridElementWidthInv=static_cast(FRAME_GRID_COLS)/static_cast(mnMaxX-mnMinX); + // 表示一个图像像素相当于多少个图像网格行（高） + mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/static_cast(mnMaxY-mnMinY); + + fx = K.at(0,0); + fy = K.at(1,1); + cx = K.at(0,2); + cy = K.at(1,2); + // 猜测是因为这种除法计算需要的时间略长，所以这里直接存储了这个中间计算结果 + invfx = 1.0f/fx; + invfy = 1.0f/fy; + + // 特殊的初始化过程完成，标志复位 + mbInitialComputations=false; + } + + // 计算假想的基线长度 baseline= mbf/fx + // 后面要对从RGBD相机输入的特征点,结合相机基线长度,焦距,以及点的深度等信息来计算其在假想的"右侧图像"上的匹配点 + mb = mbf/fx; + + if(pPrevF){ + if(pPrevF->HasVelocity()) + SetVelocity(pPrevF->GetVelocity()); + } + else{ + mVw.setZero(); + } + + mpMutexImu = new std::mutex(); + + //Set no stereo fisheye information + Nleft = -1; + Nright = -1; + mvLeftToRightMatch = vector(0); + mvRightToLeftMatch = vector(0); + mvStereo3Dpoints = vector(0); + monoLeft = -1; + monoRight = -1; + + // 将特征点分配到图像网格中 + AssignFeaturesToGrid(); +} + +// 单目模式 +Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, GeometricCamera* pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, Frame* pPrevF, const IMU::Calib &ImuCalib) + :mpcpi(NULL),mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast(NULL)), + mTimeStamp(timeStamp), mK(static_cast(pCamera)->toK()), mK_(static_cast(pCamera)->toK_()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), + mImuCalib(ImuCalib), mpImuPreintegrated(NULL),mpPrevFrame(pPrevF),mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbIsSet(false), mbImuPreintegrated(false), mpCamera(pCamera), + mpCamera2(nullptr), mbHasPose(false), mbHasVelocity(false) +{ + // Frame ID + // Step 1 帧的ID 自增 + mnId=nNextId++; + + // Step 2 计算图像金字塔的参数 + // Scale Level Info + //获取图像金字塔的层数 + mnScaleLevels = mpORBextractorLeft->GetLevels(); + //获取每层的缩放因子 + mfScaleFactor = mpORBextractorLeft->GetScaleFactor(); + //计算每层缩放因子的自然对数 + mfLogScaleFactor = log(mfScaleFactor); + //获取各层图像的缩放因子 + mvScaleFactors = mpORBextractorLeft->GetScaleFactors(); + //获取各层图像的缩放因子的倒数 + mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors(); + //获取sigma^2 + mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares(); + //获取sigma^2的倒数 + mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares(); + + // ORB extraction +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); +#endif + + // Step 3 对这个单目图像进行提取特征点, 第一个参数0-左图， 1-右图 + ExtractORB(0,imGray,0,1000); + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now(); + + mTimeORB_Ext = std::chrono::duration_cast >(time_EndExtORB - time_StartExtORB).count(); +#endif + + // 提取特征点的个数 + N = mvKeys.size(); + // 如果没有能够成功提取出特征点，那么就直接返回了 + if(mvKeys.empty()) + return; + + // Step 4 用OpenCV的矫正函数、内参对提取到的特征点进行矫正 + UndistortKeyPoints(); + + // Set no stereo information + // 由于单目相机无法直接获得立体信息，所以这里要给右图像对应点和深度赋值-1表示没有相关信息 + mvuRight = vector(N,-1); + mvDepth = vector(N,-1); + mnCloseMPs = 0; + + // 初始化本帧的地图点 + mvpMapPoints = vector(N,static_cast(NULL)); + + mmProjectPoints.clear();// = map(N, static_cast(NULL)); + mmMatchedInImage.clear(); + + // 记录地图点是否为外点，初始化均为外点false + mvbOutlier = vector(N,false); + + // This is done only for the first Frame (or after a change in the calibration) + // Step 5 计算去畸变后图像边界，将特征点分配到网格中。这个过程一般是在第一帧或者是相机标定参数发生变化之后进行 + if(mbInitialComputations) + { + // 计算去畸变后图像的边界 + ComputeImageBounds(imGray); + + // 表示一个图像像素相当于多少个图像网格列（宽） + mfGridElementWidthInv=static_cast(FRAME_GRID_COLS)/static_cast(mnMaxX-mnMinX); + // 表示一个图像像素相当于多少个图像网格行（高） + mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/static_cast(mnMaxY-mnMinY); + + fx = static_cast(mpCamera)->toK().at(0,0); + fy = static_cast(mpCamera)->toK().at(1,1); + cx = static_cast(mpCamera)->toK().at(0,2); + cy = static_cast(mpCamera)->toK().at(1,2); + // 猜测是因为这种除法计算需要的时间略长，所以这里直接存储了这个中间计算结果 + invfx = 1.0f/fx; + invfy = 1.0f/fy; + + // 特殊的初始化过程完成，标志复位 + mbInitialComputations=false; + } + + //计算 basline + mb = mbf/fx; + + //Set no stereo fisheye information + Nleft = -1; + Nright = -1; + mvLeftToRightMatch = vector(0); + mvRightToLeftMatch = vector(0); + mvStereo3Dpoints = vector(0); + monoLeft = -1; + monoRight = -1; + + // 将特征点分配到图像网格中 + AssignFeaturesToGrid(); + + if(pPrevF) + { + if(pPrevF->HasVelocity()) + { + SetVelocity(pPrevF->GetVelocity()); + } + } + else + { + mVw.setZero(); + } + + mpMutexImu = new std::mutex(); +} + +/** + * @brief 特征分网格 + */ +void Frame::AssignFeaturesToGrid() +{ + // Fill matrix with points + // Step 1 给存储特征点的网格数组 Frame::mGrid 预分配空间 + const int nCells = FRAME_GRID_COLS*FRAME_GRID_ROWS; + + int nReserve = 0.5f*N/(nCells); + + // 开始对mGrid这个二维数组中的每一个vector元素遍历并预分配空间 + for(unsigned int i=0; i vLapping = {x0,x1}; + // 判断是左图还是右图 + if(flag==0) + // 左图的话就套使用左图指定的特征点提取器，并将提取结果保存到对应的变量中 + monoLeft = (*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors,vLapping); + else + // 右图的话就需要使用右图指定的特征点提取器，并将提取结果保存到对应的变量中 + monoRight = (*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight,vLapping); +} + +bool Frame::isSet() const { + return mbIsSet; +} + +/** + * @brief 赋值位姿 + * @param Tcw 位姿 + */ +void Frame::SetPose(const Sophus::SE3 &Tcw) +{ + mTcw = Tcw; + + UpdatePoseMatrices(); + mbIsSet = true; + mbHasPose = true; +} + +/** + * @brief 赋值新的偏置 + * @param b 偏置 + */ +void Frame::SetNewBias(const IMU::Bias &b) +{ + mImuBias = b; + if(mpImuPreintegrated) + mpImuPreintegrated->SetNewBias(b); +} + +/** + * @brief 赋值新的速度 + * @param Vwb 速度 + */ +void Frame::SetVelocity(Eigen::Vector3f Vwb) +{ + mVw = Vwb; + mbHasVelocity = true; +} + +/** + * @brief 获取速度 + */ +Eigen::Vector3f Frame::GetVelocity() const +{ + return mVw; +} + +/** + * @brief 赋值位姿与速度 + */ +void Frame::SetImuPoseVelocity(const Eigen::Matrix3f &Rwb, const Eigen::Vector3f &twb, const Eigen::Vector3f &Vwb) +{ + mVw = Vwb; + mbHasVelocity = true; + + Sophus::SE3f Twb(Rwb, twb); + Sophus::SE3f Tbw = Twb.inverse(); + + mTcw = mImuCalib.mTcb * Tbw; + + UpdatePoseMatrices(); + mbIsSet = true; + mbHasPose = true; +} + +/** + * @brief 通过mTcw更新其他关于位姿的变量 + */ +void Frame::UpdatePoseMatrices() +{ + Sophus::SE3 Twc = mTcw.inverse(); + mRwc = Twc.rotationMatrix(); + mOw = Twc.translation(); + mRcw = mTcw.rotationMatrix(); + mtcw = mTcw.translation(); +} + +/** + * @brief 获得imu的平移 + */ +Eigen::Matrix Frame::GetImuPosition() const { + return mRwc * mImuCalib.mTcb.translation() + mOw; +} + +/** + * @brief 获得imu的旋转 + */ +Eigen::Matrix Frame::GetImuRotation() { + return mRwc * mImuCalib.mTcb.rotationMatrix(); +} + +/** + * @brief 获得imu的位姿 + */ +Sophus::SE3 Frame::GetImuPose() { + return mTcw.inverse() * mImuCalib.mTcb; +} + +/** + * @brief 获得左右目的相对位姿 + */ +Sophus::SE3f Frame::GetRelativePoseTrl() +{ + return mTrl; +} + +/** + * @brief 获得左右目的相对位姿 + */ +Sophus::SE3f Frame::GetRelativePoseTlr() +{ + return mTlr; +} + +/** + * @brief 获得左右目的相对旋转 + */ +Eigen::Matrix3f Frame::GetRelativePoseTlr_rotation(){ + return mTlr.rotationMatrix(); +} + +/** + * @brief 获得左右目的相对平移 + */ +Eigen::Vector3f Frame::GetRelativePoseTlr_translation() { + return mTlr.translation(); +} + +/** + * @brief 判断路标点是否在视野中 + * 步骤 + * Step 1 获得这个地图点的世界坐标 + * Step 2 关卡一：检查这个地图点在当前帧的相机坐标系下，是否有正的深度.如果是负的，表示出错，返回false + * Step 3 关卡二：将MapPoint投影到当前帧的像素坐标(u,v), 并判断是否在图像有效范围内 + * Step 4 关卡三：计算MapPoint到相机中心的距离, 并判断是否在尺度变化的距离内 + * Step 5 关卡四：计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值, 若小于设定阈值，返回false + * Step 6 根据地图点到光心的距离来预测一个尺度（仿照特征点金字塔层级） + * Step 7 记录计算得到的一些参数 + * @param[in] pMP 当前地图点 + * @param[in] viewingCosLimit 夹角余弦，用于限制地图点和光心连线和法线的夹角 + * @return true 地图点合格，且在视野内 + * @return false 地图点不合格，抛弃 + */ +bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit) +{ + // 单目，立体匹配双目，rgbd + if(Nleft == -1) + { + // cout << "\na"; + // mbTrackInView是决定一个地图点是否进行重投影的标志 + // 这个标志的确定要经过多个函数的确定，isInFrustum()只是其中的一个验证关卡。这里默认设置为否 + pMP->mbTrackInView = false; + pMP->mTrackProjX = -1; + pMP->mTrackProjY = -1; + + // 3D in absolute coordinates + // Step 1 获得这个地图点的世界坐标 + Eigen::Matrix P = pMP->GetWorldPos(); + + // 3D in camera coordinates + // 根据当前帧(粗糙)位姿转化到当前相机坐标系下的三维点Pc + const Eigen::Matrix Pc = mRcw * P + mtcw; + const float Pc_dist = Pc.norm(); + + // Check positive depth + const float &PcZ = Pc(2); + const float invz = 1.0f/PcZ; + // Step 2 关卡一：检查这个地图点在当前帧的相机坐标系下，是否有正的深度.如果是负的，表示出错，直接返回false + if(PcZ<0.0f) + return false; + + const Eigen::Vector2f uv = mpCamera->project(Pc); + + // Step 3 关卡二：将MapPoint投影到当前帧的像素坐标(u,v), 并判断是否在图像有效范围内 + // 判断是否在图像边界中，只要不在那么就说明无法在当前帧下进行重投影 + if(uv(0)mnMaxX) + return false; + if(uv(1)mnMaxY) + return false; + + pMP->mTrackProjX = uv(0); + pMP->mTrackProjY = uv(1); + + // Check distance is in the scale invariance region of the MapPoint + // Step 4 关卡三：计算MapPoint到相机中心的距离, 并判断是否在尺度变化的距离内 + // 得到认为的可靠距离范围:[0.8f*mfMinDistance, 1.2f*mfMaxDistance] + const float maxDistance = pMP->GetMaxDistanceInvariance(); + const float minDistance = pMP->GetMinDistanceInvariance(); + // 得到当前地图点距离当前帧相机光心的距离,注意P，mOw都是在同一坐标系下才可以 + // mOw：当前相机光心在世界坐标系下坐标 + const Eigen::Vector3f PO = P - mOw; + // 取模就得到了距离 + const float dist = PO.norm(); + + // 如果不在允许的尺度变化范围内，认为重投影不可靠 + if(distmaxDistance) + return false; + + // Check viewing angle + // Step 5 关卡四： + // 计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值, + // 若小于cos(viewingCosLimit), 即夹角大于viewingCosLimit弧度则返回 + Eigen::Vector3f Pn = pMP->GetNormal(); + + // 计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值，注意平均观测方向为单位向量 + const float viewCos = PO.dot(Pn)/dist; + + // 如果大于给定的阈值 cos(60°)=0.5，认为这个点方向太偏了，重投影不可靠，返回false + if(viewCosPredictScale(dist,this); + + // Step 7 记录计算得到的一些参数 + // Data used by the tracking + // 通过置位标记 MapPoint::mbTrackInView 来表示这个地图点要被投影 + pMP->mbTrackInView = true; + // 该地图点投影在当前图像（一般是左图）的像素横坐标 + pMP->mTrackProjX = uv(0); + // bf/z其实是视差，相减得到右图（如有）中对应点的横坐标 + pMP->mTrackProjXR = uv(0) - mbf*invz; + + pMP->mTrackDepth = Pc_dist; + + // 该地图点投影在当前图像（一般是左图）的像素纵坐标 + pMP->mTrackProjY = uv(1); + // 根据地图点到光心距离，预测的该地图点的尺度层级 + pMP->mnTrackScaleLevel= nPredictedLevel; + // 保存当前视角和法线夹角的余弦值 + pMP->mTrackViewCos = viewCos; + + // 执行到这里说明这个地图点在相机的视野中并且进行重投影是可靠的，返回true + return true; + } + // 左右目时分别验证 + else + { + pMP->mbTrackInView = false; + pMP->mbTrackInViewR = false; + pMP -> mnTrackScaleLevel = -1; + pMP -> mnTrackScaleLevelR = -1; + + pMP->mbTrackInView = isInFrustumChecks(pMP,viewingCosLimit); + pMP->mbTrackInViewR = isInFrustumChecks(pMP,viewingCosLimit,true); + + return pMP->mbTrackInView || pMP->mbTrackInViewR; + } +} + +/** + * @brief 暂没用到 + */ +bool Frame::ProjectPointDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float &v) +{ + + // 3D in absolute coordinates + Eigen::Vector3f P = pMP->GetWorldPos(); + + // 3D in camera coordinates + const Eigen::Vector3f Pc = mRcw * P + mtcw; + const float &PcX = Pc(0); + const float &PcY= Pc(1); + const float &PcZ = Pc(2); + + // Check positive depth + if(PcZ<0.0f) + { + cout << "Negative depth: " << PcZ << endl; + return false; + } + + // Project in image and check it is not outside + const float invz = 1.0f/PcZ; + u=fx*PcX*invz+cx; + v=fy*PcY*invz+cy; + + if(umnMaxX) + return false; + if(vmnMaxY) + return false; + + float u_distort, v_distort; + + float x = (u - cx) * invfx; + float y = (v - cy) * invfy; + float r2 = x * x + y * y; + float k1 = mDistCoef.at(0); + float k2 = mDistCoef.at(1); + float p1 = mDistCoef.at(2); + float p2 = mDistCoef.at(3); + float k3 = 0; + if(mDistCoef.total() == 5) + { + k3 = mDistCoef.at(4); + } + + // Radial distorsion + float x_distort = x * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2); + float y_distort = y * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2); + + // Tangential distorsion + x_distort = x_distort + (2 * p1 * x * y + p2 * (r2 + 2 * x * x)); + y_distort = y_distort + (p1 * (r2 + 2 * y * y) + 2 * p2 * x * y); + + u_distort = x_distort * fx + cx; + v_distort = y_distort * fy + cy; + + + u = u_distort; + v = v_distort; + + kp = cv::Point2f(u, v); + + return true; +} + +/** + * @brief 暂没用到 + */ +Eigen::Vector3f Frame::inRefCoordinates(Eigen::Vector3f pCw) +{ + return mRcw * pCw + mtcw; +} + +/** + * @brief 给定一个坐标，返回区域内所有特征点 + * @param x 给定点的x + * @param y 给定点的y + * @param r 搜索半径 + * @param minLevel 金字塔下边界 + * @param maxLevel 金字塔上边界 + * @param bRight 是否是右相机 + */ +vector Frame::GetFeaturesInArea( + const float &x, const float &y, const float &r, + const int minLevel,const int maxLevel, const bool bRight) const +{ + // 存储搜索结果的vector + vector vIndices; + vIndices.reserve(N); + + float factorX = r; + float factorY = r; + + // Step 1 计算半径为r圆左右上下边界所在的网格列和行的id + // 查找半径为r的圆左侧边界所在网格列坐标。这个地方有点绕，慢慢理解下： + // (mnMaxX-mnMinX)/FRAME_GRID_COLS：表示列方向每个网格可以平均分得几个像素（肯定大于1） + // mfGridElementWidthInv=FRAME_GRID_COLS/(mnMaxX-mnMinX) 是上面倒数，表示每个像素可以均分几个网格列（肯定小于1） + // (x-mnMinX-r)，可以看做是从图像的左边界mnMinX到半径r的圆的左边界区域占的像素列数 + // 两者相乘，就是求出那个半径为r的圆的左侧边界在哪个网格列中 + // 保证nMinCellX 结果大于等于0 + const int nMinCellX = max(0,(int)floor((x-mnMinX-factorX)*mfGridElementWidthInv)); + // 如果最终求得的圆的左边界所在的网格列超过了设定了上限，那么就说明计算出错，找不到符合要求的特征点，返回空vector + if(nMinCellX>=FRAME_GRID_COLS) + { + return vIndices; + } + + // 计算圆所在的右边界网格列索引 + const int nMaxCellX = min((int)FRAME_GRID_COLS-1,(int)ceil((x-mnMinX+factorX)*mfGridElementWidthInv)); + // 如果计算出的圆右边界所在的网格不合法，说明该特征点不好，直接返回空vector + if(nMaxCellX<0) + { + return vIndices; + } + + // 后面的操作也都是类似的，计算出这个圆上下边界所在的网格行的id + const int nMinCellY = max(0,(int)floor((y-mnMinY-factorY)*mfGridElementHeightInv)); + if(nMinCellY>=FRAME_GRID_ROWS) + { + return vIndices; + } + + const int nMaxCellY = min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinY+factorY)*mfGridElementHeightInv)); + if(nMaxCellY<0) + { + return vIndices; + } + + // 检查需要搜索的图像金字塔层数范围是否符合要求 + //? 疑似bug。(minLevel>0) 后面条件 (maxLevel>=0)肯定成立 + //? 改为 const bool bCheckLevels = (minLevel>=0) || (maxLevel>=0); + const bool bCheckLevels = (minLevel>0) || (maxLevel>=0); + + // Step 2 遍历圆形区域内的所有网格，寻找满足条件的候选特征点，并将其index放到输出里 + for(int ix = nMinCellX; ix<=nMaxCellX; ix++) + { + for(int iy = nMinCellY; iy<=nMaxCellY; iy++) + { + // 获取这个网格内的所有特征点在 Frame::mvKeysUn 中的索引 + const vector vCell = (!bRight) ? mGrid[ix][iy] : mGridRight[ix][iy]; + // 如果这个网格中没有特征点，那么跳过这个网格继续下一个 + if(vCell.empty()) + continue; + + // 如果这个网格中有特征点，那么遍历这个图像网格中所有的特征点 + for(size_t j=0, jend=vCell.size(); j=0) + if(kpUn.octave>maxLevel) + continue; + } + + // 通过检查，计算候选特征点到圆中心的距离，查看是否是在这个圆形区域之内 + const float distx = kpUn.pt.x-x; + const float disty = kpUn.pt.y-y; + + // 如果x方向和y方向的距离都在指定的半径之内，存储其index为候选特征点 + if(fabs(distx)=FRAME_GRID_COLS || posY<0 || posY>=FRAME_GRID_ROWS) + return false; + + // 计算成功返回true + return true; +} + +/** + * @brief 计算当前帧特征点对应的词袋Bow，主要是mBowVec 和 mFeatVec + * + */ +void Frame::ComputeBoW() +{ + // 判断是否以前已经计算过了，计算过了就跳过 + if(mBowVec.empty()) + { + // 将描述子mDescriptors转换为DBOW要求的输入格式 + vector vCurrentDesc = Converter::toDescriptorVector(mDescriptors); + // 将特征点的描述子转换成词袋向量mBowVec以及特征向量mFeatVec + mpORBvocabulary->transform(vCurrentDesc, //当前的描述子vector + mBowVec, //输出，词袋向量，记录的是单词的id及其对应权重TF-IDF值 + mFeatVec, //输出，记录node id及其对应的图像 feature对应的索引 + 4); //4表示从叶节点向前数的层数 + } +} + +/** + * @brief 用内参对特征点去畸变，结果报存在mvKeysUn中 + * + */ +void Frame::UndistortKeyPoints() +{ + // Step 1 如果第一个畸变参数为0，不需要矫正。第一个畸变参数k1是最重要的，一般不为0，为0的话，说明畸变参数都是0 + // 变量mDistCoef中存储了opencv指定格式的去畸变参数，格式为：(k1,k2,p1,p2,k3) + if(mDistCoef.at(0)==0.0) + { + mvKeysUn=mvKeys; + return; + } + + // Fill matrix with points + // Step 2 如果畸变参数不为0，用OpenCV函数进行畸变矫正 + // Fill matrix with points + // N为提取的特征点数量，为满足OpenCV函数输入要求，将N个特征点保存在N*2的矩阵中 + cv::Mat mat(N,2,CV_32F); + + // 遍历每个特征点，并将它们的坐标保存到矩阵中 + for(int i=0; i(i,0)=mvKeys[i].pt.x; + mat.at(i,1)=mvKeys[i].pt.y; + } + + // Undistort points + // 函数reshape(int cn,int rows=0) 其中cn为更改后的通道数，rows=0表示这个行将保持原来的参数不变 + // 为了能够直接调用opencv的函数来去畸变，需要先将矩阵调整为2通道（对应坐标x,y） + // cv::undistortPoints最后一个矩阵为空矩阵时，得到的点为归一化坐标点 + mat=mat.reshape(2); + cv::undistortPoints(mat,mat, static_cast(mpCamera)->toK(),mDistCoef,cv::Mat(),mK); + // 调整回只有一个通道，回归我们正常的处理方式 + mat=mat.reshape(1); + + + // Fill undistorted keypoint vector + // Step 3 存储校正后的特征点 + mvKeysUn.resize(N); + for(int i=0; i(i,0); + kp.pt.y=mat.at(i,1); + mvKeysUn[i]=kp; + } + +} + +/** + * @brief 计算去畸变图像的边界 + * + * @param[in] imLeft 需要计算边界的图像 + */ +void Frame::ComputeImageBounds(const cv::Mat &imLeft) +{ + // 如果畸变参数不为0，用OpenCV函数进行畸变矫正 + if(mDistCoef.at(0)!=0.0) + { + // 保存矫正前的图像四个边界点坐标： (0,0) (cols,0) (0,rows) (cols,rows) + cv::Mat mat(4,2,CV_32F); + mat.at(0,0)=0.0; mat.at(0,1)=0.0; + mat.at(1,0)=imLeft.cols; mat.at(1,1)=0.0; + mat.at(2,0)=0.0; mat.at(2,1)=imLeft.rows; + mat.at(3,0)=imLeft.cols; mat.at(3,1)=imLeft.rows; + + // 和前面校正特征点一样的操作，将这几个边界点作为输入进行校正 + mat=mat.reshape(2); + cv::undistortPoints(mat,mat,static_cast(mpCamera)->toK(),mDistCoef,cv::Mat(),mK); + mat=mat.reshape(1); + + // Undistort corners + // 校正后的四个边界点已经不能够围成一个严格的矩形，因此在这个四边形的外侧加边框作为坐标的边界 + mnMinX = min(mat.at(0,0),mat.at(2,0)); // 左上和左下横坐标最小的 + mnMaxX = max(mat.at(1,0),mat.at(3,0)); // 右上和右下横坐标最大的 + mnMinY = min(mat.at(0,1),mat.at(1,1)); // 左上和右上纵坐标最小的 + mnMaxY = max(mat.at(2,1),mat.at(3,1)); // 左下和右下纵坐标最小的 + } + else + { + // 如果畸变参数为0，就直接获得图像边界 + mnMinX = 0.0f; + mnMaxX = imLeft.cols; + mnMinY = 0.0f; + mnMaxY = imLeft.rows; + } +} + +/* + * 双目匹配函数 + * + * 为左图的每一个特征点在右图中找到匹配点 \n + * 根据基线(有冗余范围)上描述子距离找到匹配, 再进行SAD精确定位 \n ‘ + * 这里所说的SAD是一种双目立体视觉匹配算法，可参考[https://blog.csdn.net/u012507022/article/details/51446891] + * 最后对所有SAD的值进行排序, 剔除SAD值较大的匹配对，然后利用抛物线拟合得到亚像素精度的匹配 \n + * 这里所谓的亚像素精度，就是使用这个拟合得到一个小于一个单位像素的修正量，这样可以取得更好的估计结果，计算出来的点的深度也就越准确 + * 匹配成功后会更新 mvuRight(ur) 和 mvDepth(Z) + */ +void Frame::ComputeStereoMatches() +{ + /*两帧图像稀疏立体匹配（即：ORB特征点匹配，非逐像素的密集匹配，但依然满足行对齐） + * 输入：两帧立体矫正后的图像img_left 和 img_right 对应的orb特征点集 + * 过程： + 1. 行特征点统计. 统计img_right每一行上的ORB特征点集，便于使用立体匹配思路(行搜索/极线搜索）进行同名点搜索, 避免逐像素的判断. + 2. 粗匹配. 根据步骤1的结果，对img_left第i行的orb特征点pi，在img_right的第i行上的orb特征点集中搜索相似orb特征点, 得到qi + 3. 精确匹配. 以点qi为中心，半径为r的范围内，进行块匹配（归一化SAD），进一步优化匹配结果 + 4. 亚像素精度优化. 步骤3得到的视差为uchar/int类型精度，并不一定是真实视差，通过亚像素差值（抛物线插值)获取float精度的真实视差 + 5. 最优视差值/深度选择. 通过胜者为王算法（WTA）获取最佳匹配点。 + 6. 删除离缺点(outliers). 块匹配相似度阈值判断，归一化sad最小，并不代表就一定是正确匹配，比如光照变化、弱纹理等会造成误匹配 + * 输出：稀疏特征点视差图/深度图（亚像素精度）mvDepth 匹配结果 mvuRight + */ + + // 为匹配结果预先分配内存，数据类型为float型 + // mvuRight存储右图匹配点索引 + // mvDepth存储特征点的深度信息 + mvuRight = vector(N,-1.0f); + mvDepth = vector(N,-1.0f); + + // orb特征相似度阈值 -> mean ～= (max + min) / 2 + const int thOrbDist = (ORBmatcher::TH_HIGH+ORBmatcher::TH_LOW)/2; + + // 金字塔顶层（0层）图像高 nRows + const int nRows = mpORBextractorLeft->mvImagePyramid[0].rows; + + // Assign keypoints to row table + // 二维vector存储每一行的orb特征点的列坐标的索引，为什么是vector，因为每一行的特征点有可能不一样，例如 + // vRowIndices[0] = [1，2，5，8, 11] 第1行有5个特征点,他们的列号（即x坐标）分别是1,2,5,8,11 + // vRowIndices[1] = [2，6，7，9, 13, 17, 20] 第2行有7个特征点.etc + vector > vRowIndices(nRows,vector()); + + for(int i=0; i 3. 粗匹配 + 精匹配 + // 对于立体矫正后的两张图，在列方向(x)存在最大视差maxd和最小视差mind + // 也即是左图中任何一点p，在右图上的匹配点的范围为应该是[p - maxd, p - mind], 而不需要遍历每一行所有的像素 + // maxd = baseline * length_focal / minZ + // mind = baseline * length_focal / maxZ + // Set limits for search + const float minZ = mb; + const float minD = 0; + const float maxD = mbf/minZ; + + // For each left keypoint search a match in the right image + // 保存sad块匹配相似度和左图特征点索引 + vector > vDistIdx; + vDistIdx.reserve(N); + + // 为左图每一个特征点il，在右图搜索最相似的特征点ir + for(int iL=0; iL &vCandidates = vRowIndices[vL]; + + if(vCandidates.empty()) + continue; + + // 计算理论上的最佳搜索范围 + const float minU = uL-maxD; + const float maxU = uL-minD; + + // 最大搜索范围小于0，说明无匹配点 + if(maxU<0) + continue; + + // 初始化最佳相似度，用最大相似度，以及最佳匹配点索引 + int bestDist = ORBmatcher::TH_HIGH; + size_t bestIdxR = 0; + + const cv::Mat &dL = mDescriptors.row(iL); + + // Compare descriptor to right keypoints + // Step2. 粗配准. 左图特征点il与右图中的可能的匹配点进行逐个比较,得到最相似匹配点的相似度和索引 + for(size_t iC=0; iClevelL+1) + continue; + + // 使用列坐标(x)进行匹配，和stereomatch一样 + const float &uR = kpR.pt.x; + + // 超出理论搜索范围[minU, maxU]，可能是误匹配，放弃 + if(uR>=minU && uR<=maxU) + { + // 计算匹配点il和待匹配点ic的相似度dist + const cv::Mat &dR = mDescriptorsRight.row(iR); + const int dist = ORBmatcher::DescriptorDistance(dL,dR); + + // 统计最小相似度及其对应的列坐标(x) + if(distmvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduL-w,scaleduL+w+1); + + // 初始化最佳相似度 + int bestDist = INT_MAX; + // 通过滑动窗口搜索优化，得到的列坐标偏移量 + int bestincR = 0; + // 滑动窗口的滑动范围为（-L, L） + const int L = 5; + // 初始化存储图像块相似度 + vector vDists; + vDists.resize(2*L+1); + + // 计算滑动窗口滑动范围的边界，因为是块匹配，还要算上图像块的尺寸 + // 列方向起点 iniu = r0 + 最大窗口滑动范围 - 图像块尺寸 + // 列方向终点 eniu = r0 + 最大窗口滑动范围 + 图像块尺寸 + 1 + // 此次 + 1 和下面的提取图像块是列坐标+1是一样的，保证提取的图像块的宽是2 * w + 1 + const float iniu = scaleduR0+L-w; + const float endu = scaleduR0+L+w+1; + // 判断搜索是否越界 + if(iniu<0 || endu >= mpORBextractorRight->mvImagePyramid[kpL.octave].cols) + continue; + + // 在搜索范围内从左到右滑动，并计算图像块相似度 + for(int incR=-L; incR<=+L; incR++) + { + // 提取左图中，以特征点(scaleduL,scaledvL)为中心, 半径为w的图像快patch + cv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduR0+incR-w,scaleduR0+incR+w+1); + + // sad 计算 + float dist = cv::norm(IL,IR,cv::NORM_L1); + // 统计最小sad和偏移量 + if(dist> 公式7 + // Sub-pixel match (Parabola fitting) + const float dist1 = vDists[L+bestincR-1]; + const float dist2 = vDists[L+bestincR]; + const float dist3 = vDists[L+bestincR+1]; + + const float deltaR = (dist1-dist3)/(2.0f*(dist1+dist3-2.0f*dist2)); + + // 亚像素精度的修正量应该是在[-1,1]之间，否则就是误匹配 + if(deltaR<-1 || deltaR>1) + continue; + + // Re-scaled coordinate + // 根据亚像素精度偏移量delta调整最佳匹配索引 + float bestuR = mvScaleFactors[kpL.octave]*((float)scaleduR0+(float)bestincR+deltaR); + + float disparity = (uL-bestuR); + + if(disparity>=minD && disparity(bestDist,iL)); + } + } + } + + // Step 6. 删除离缺点(outliers) + // 块匹配相似度阈值判断，归一化sad最小，并不代表就一定是匹配的，比如光照变化、弱纹理、无纹理等同样会造成误匹配 + // 误匹配判断条件 norm_sad > 1.5 * 1.4 * median + sort(vDistIdx.begin(),vDistIdx.end()); + const float median = vDistIdx[vDistIdx.size()/2].first; + const float thDist = 1.5f*1.4f*median; + + for(int i=vDistIdx.size()-1;i>=0;i--) + { + if(vDistIdx[i].first */ + // mvDepth直接由depth图像读取` + // 这里是初始化这两个存储“右图”匹配特征点横坐标和存储特征点深度值的vector + mvuRight = vector(N,-1); + mvDepth = vector(N,-1); + + for(int i=0; i 从未矫正的特征点提供的坐标来读取深度图像拿到这个点的深度数据 */ + // 获取校正前和校正后的特征点 + const cv::KeyPoint &kp = mvKeys[i]; + const cv::KeyPoint &kpU = mvKeysUn[i]; + + // 获取其横纵坐标，注意 NOTICE 是校正前的特征点的 + const float &v = kp.pt.y; + const float &u = kp.pt.x; + + // 从深度图像中获取这个特征点对应的深度点 + // NOTE 从这里看对深度图像进行去畸变处理是没有必要的,我们依旧可以直接通过未矫正的特征点的坐标来直接拿到深度数据 + const float d = imDepth.at(v,u); + + // 如果获取到的深度点合法(d>0), 那么就保存这个特征点的深度,并且计算出等效的\在假想的右图中该特征点所匹配的特征点的横坐标 + if(d>0) + { + mvDepth[i] = d; + mvuRight[i] = kpU.pt.x-mbf/d; + } + } +} + +/** + * @brief 当某个特征点的深度信息或者双目信息有效时,将它反投影到三维世界坐标系中 + */ +bool Frame::UnprojectStereo(const int &i, Eigen::Vector3f &x3D) +{ + const float z = mvDepth[i]; + if(z>0) { + const float u = mvKeysUn[i].pt.x; + const float v = mvKeysUn[i].pt.y; + const float x = (u-cx)*z*invfx; + const float y = (v-cy)*z*invfy; + Eigen::Vector3f x3Dc(x, y, z); + x3D = mRwc * x3Dc + mOw; + return true; + } else + return false; +} + +/** + * @brief 是否做完预积分 + */ +bool Frame::imuIsPreintegrated() +{ + unique_lock lock(*mpMutexImu); + return mbImuPreintegrated; +} + +/** + * @brief 设置为做完预积分 + */ +void Frame::setIntegrated() +{ + unique_lock lock(*mpMutexImu); + mbImuPreintegrated = true; +} + +/** + * @brief 左右目模式 + */ +Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, GeometricCamera* pCamera2, Sophus::SE3f& Tlr,Frame* pPrevF, const IMU::Calib &ImuCalib) + :mpcpi(NULL), mpORBvocabulary(voc),mpORBextractorLeft(extractorLeft),mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()), mK_(Converter::toMatrix3f(K)), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), + mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF),mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbImuPreintegrated(false), mpCamera(pCamera), mpCamera2(pCamera2), + mbHasPose(false), mbHasVelocity(false) + +{ + imgLeft = imLeft.clone(); + imgRight = imRight.clone(); + + // Frame ID + mnId=nNextId++; + + // Scale Level Info + mnScaleLevels = mpORBextractorLeft->GetLevels(); + mfScaleFactor = mpORBextractorLeft->GetScaleFactor(); + mfLogScaleFactor = log(mfScaleFactor); + mvScaleFactors = mpORBextractorLeft->GetScaleFactors(); + mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors(); + mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares(); + mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares(); + + // ORB extraction +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); +#endif + thread threadLeft(&Frame::ExtractORB,this,0,imLeft,static_cast(mpCamera)->mvLappingArea[0],static_cast(mpCamera)->mvLappingArea[1]); + thread threadRight(&Frame::ExtractORB,this,1,imRight,static_cast(mpCamera2)->mvLappingArea[0],static_cast(mpCamera2)->mvLappingArea[1]); + threadLeft.join(); + threadRight.join(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now(); + + mTimeORB_Ext = std::chrono::duration_cast >(time_EndExtORB - time_StartExtORB).count(); +#endif + + // 左图中提取的特征点数目 + Nleft = mvKeys.size(); + // 右图中提取的特征点数目 + Nright = mvKeysRight.size(); + // 特征点总数 + N = Nleft + Nright; + + if(N == 0) + return; + + // This is done only for the first Frame (or after a change in the calibration) + if(mbInitialComputations) + { + ComputeImageBounds(imLeft); + + mfGridElementWidthInv=static_cast(FRAME_GRID_COLS)/(mnMaxX-mnMinX); + mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/(mnMaxY-mnMinY); + + fx = K.at(0,0); + fy = K.at(1,1); + cx = K.at(0,2); + cy = K.at(1,2); + invfx = 1.0f/fx; + invfy = 1.0f/fy; + + mbInitialComputations=false; + } + + mb = mbf / fx; + + // Sophus/Eigen + mTlr = Tlr; + mTrl = mTlr.inverse(); + mRlr = mTlr.rotationMatrix(); + mtlr = mTlr.translation(); + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartStereoMatches = std::chrono::steady_clock::now(); +#endif + ComputeStereoFishEyeMatches(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndStereoMatches = std::chrono::steady_clock::now(); + + mTimeStereoMatch = std::chrono::duration_cast >(time_EndStereoMatches - time_StartStereoMatches).count(); +#endif + + //Put all descriptors in the same matrix + cv::vconcat(mDescriptors,mDescriptorsRight,mDescriptors); + + mvpMapPoints = vector(N,static_cast(nullptr)); + mvbOutlier = vector(N,false); + + AssignFeaturesToGrid(); + + mpMutexImu = new std::mutex(); + + UndistortKeyPoints(); + +} + +/** + * @brief 左右目匹配 + */ +void Frame::ComputeStereoFishEyeMatches() +{ + // 1. 分别取出特征点 + //Speed it up by matching keypoints in the lapping area + vector stereoLeft(mvKeys.begin() + monoLeft, mvKeys.end()); + vector stereoRight(mvKeysRight.begin() + monoRight, mvKeysRight.end()); + + // 2. 分别取出描述子 + cv::Mat stereoDescLeft = mDescriptors.rowRange(monoLeft, mDescriptors.rows); + cv::Mat stereoDescRight = mDescriptorsRight.rowRange(monoRight, mDescriptorsRight.rows); + + // 一些在当前模式用不到的变量给他填一下 + mvLeftToRightMatch = vector(Nleft,-1); + mvRightToLeftMatch = vector(Nright,-1); + mvDepth = vector(Nleft,-1.0f); + mvuRight = vector(Nleft,-1); + mvStereo3Dpoints = vector(Nleft); + mnCloseMPs = 0; + + //Perform a brute force between Keypoint in the left and right image + vector> matches; + + // 3. 暴力匹配 + BFmatcher.knnMatch(stereoDescLeft,stereoDescRight,matches,2); + + int nMatches = 0; + int descMatches = 0; + + //Check matches using Lowe's ratio + for(vector>::iterator it = matches.begin(); it != matches.end(); ++it) + { + // 对于每一对匹配的候选中，最小距离比次小距离的0.7倍还小的认为是好的匹配 + if((*it).size() >= 2 && (*it)[0].distance < (*it)[1].distance * 0.7) + { + //For every good match, check parallax and reprojection error to discard spurious matches + // 对于好的匹配，做三角化，且深度值有效的放入结果 + Eigen::Vector3f p3D; + descMatches++; + float sigma1 = mvLevelSigma2[mvKeys[(*it)[0].queryIdx + monoLeft].octave], + sigma2 = mvLevelSigma2[mvKeysRight[(*it)[0].trainIdx + monoRight].octave]; + // 三角化 + float depth = + static_cast(mpCamera)->TriangulateMatches( + mpCamera2,mvKeys[(*it)[0].queryIdx + monoLeft], + mvKeysRight[(*it)[0].trainIdx + monoRight], + mRlr, mtlr, sigma1, sigma2, p3D); + // 填充数据 + if(depth > 0.0001f) + { + mvLeftToRightMatch[(*it)[0].queryIdx + monoLeft] = (*it)[0].trainIdx + monoRight; + mvRightToLeftMatch[(*it)[0].trainIdx + monoRight] = (*it)[0].queryIdx + monoLeft; + mvStereo3Dpoints[(*it)[0].queryIdx + monoLeft] = p3D; + mvDepth[(*it)[0].queryIdx + monoLeft] = depth; + nMatches++; + } + } + } +} + +/** + * @brief 两个相机模式下单相机的验证，判断路标点是否在视野中 + */ +bool Frame::isInFrustumChecks(MapPoint *pMP, float viewingCosLimit, bool bRight) { + // 3D in absolute coordinates + // Step 1 获得这个地图点的世界坐标 + Eigen::Vector3f P = pMP->GetWorldPos(); + + Eigen::Matrix3f mR; + Eigen::Vector3f mt, twc; + if(bRight) + { + Eigen::Matrix3f Rrl = mTrl.rotationMatrix(); + Eigen::Vector3f trl = mTrl.translation(); + mR = Rrl * mRcw; + mt = Rrl * mtcw + trl; + twc = mRwc * mTlr.translation() + mOw; + } + else + { + mR = mRcw; + mt = mtcw; + twc = mOw; + } + + // 3D in camera coordinates + // 根据当前帧(粗糙)位姿转化到当前相机坐标系下的三维点Pc + Eigen::Vector3f Pc = mR * P + mt; + const float Pc_dist = Pc.norm(); + const float &PcZ = Pc(2); + + // Check positive depth + // Step 2 关卡一：检查这个地图点在当前帧的相机坐标系下，是否有正的深度.如果是负的，表示出错，直接返回false + if(PcZ<0.0f) + return false; + + // Project in image and check it is not outside + Eigen::Vector2f uv; + if(bRight) + uv = mpCamera2->project(Pc); + else + uv = mpCamera->project(Pc); + + // Step 3 关卡二：将MapPoint投影到当前帧的像素坐标(u,v), 并判断是否在图像有效范围内 + // 判断是否在图像边界中，只要不在那么就说明无法在当前帧下进行重投影 + if(uv(0)mnMaxX) + return false; + if(uv(1)mnMaxY) + return false; + + // Check distance is in the scale invariance region of the MapPoint + // Step 4 关卡三：计算MapPoint到相机中心的距离, 并判断是否在尺度变化的距离内 + // 得到认为的可靠距离范围:[0.8f*mfMinDistance, 1.2f*mfMaxDistance] + const float maxDistance = pMP->GetMaxDistanceInvariance(); + const float minDistance = pMP->GetMinDistanceInvariance(); + // 得到当前地图点距离当前帧相机光心的距离,注意P，mOw都是在同一坐标系下才可以 + // mOw：当前相机光心在世界坐标系下坐标 + const Eigen::Vector3f PO = P - twc; + // 取模就得到了距离 + const float dist = PO.norm(); + + // 如果不在允许的尺度变化范围内，认为重投影不可靠 + if(distmaxDistance) + return false; + + // Check viewing angle + // Step 5 关卡四：计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值, 若小于cos(viewingCosLimit), 即夹角大于viewingCosLimit弧度则返回 + Eigen::Vector3f Pn = pMP->GetNormal(); + + // 计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值，注意平均观测方向为单位向量 + const float viewCos = PO.dot(Pn) / dist; + + // 如果大于给定的阈值 cos(60°)=0.5，认为这个点方向太偏了，重投影不可靠，返回false + if(viewCosPredictScale(dist,this); + + // Step 7 记录计算得到的一些参数 + if(bRight) + { + pMP->mTrackProjXR = uv(0); + pMP->mTrackProjYR = uv(1); + pMP->mnTrackScaleLevelR= nPredictedLevel; + pMP->mTrackViewCosR = viewCos; + pMP->mTrackDepthR = Pc_dist; + } + else + { + pMP->mTrackProjX = uv(0); + pMP->mTrackProjY = uv(1); + pMP->mnTrackScaleLevel= nPredictedLevel; + pMP->mTrackViewCos = viewCos; + pMP->mTrackDepth = Pc_dist; + } + + return true; +} + +/** + * @brief 根据位姿将第i个点投到世界坐标系下 + */ +Eigen::Vector3f Frame::UnprojectStereoFishEye(const int &i){ + return mRwc * mvStereo3Dpoints[i] + mOw; +} + +} //namespace ORB_SLAM diff --git a/ORB-SLAM3-UESTC/Workspace/src/FrameDrawer.cc b/ORB-SLAM3-UESTC/Workspace/src/FrameDrawer.cc new file mode 100644 index 0000000..a8cc375 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/FrameDrawer.cc @@ -0,0 +1,439 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#include "FrameDrawer.h" +#include "Tracking.h" + +#include +#include + +#include + +namespace ORB_SLAM3 +{ + +FrameDrawer::FrameDrawer(Atlas* pAtlas):both(false),mpAtlas(pAtlas) +{ + mState=Tracking::SYSTEM_NOT_READY; + mIm = cv::Mat(480,640,CV_8UC3, cv::Scalar(0,0,0)); + mImRight = cv::Mat(480,640,CV_8UC3, cv::Scalar(0,0,0)); +} + +cv::Mat FrameDrawer::DrawFrame(float imageScale) +{ + cv::Mat im; + vector vIniKeys; // Initialization: KeyPoints in reference frame + vector vMatches; // Initialization: correspondeces with reference keypoints + vector vCurrentKeys; // KeyPoints in current frame + vector vbVO, vbMap; // Tracked MapPoints in current frame + vector > vTracks; + int state; // Tracking state + vector vCurrentDepth; + float thDepth; + + Frame currentFrame; + vector vpLocalMap; + vector vMatchesKeys; + vector vpMatchedMPs; + vector vOutlierKeys; + vector vpOutlierMPs; + map mProjectPoints; + map mMatchedInImage; + + cv::Scalar standardColor(0,255,0); + cv::Scalar odometryColor(255,0,0); + + //Copy variables within scoped mutex + { + unique_lock lock(mMutex); + state=mState; + if(mState==Tracking::SYSTEM_NOT_READY) + mState=Tracking::NO_IMAGES_YET; + + mIm.copyTo(im); + + if(mState==Tracking::NOT_INITIALIZED) + { + vCurrentKeys = mvCurrentKeys; + vIniKeys = mvIniKeys; + vMatches = mvIniMatches; + vTracks = mvTracks; + } + else if(mState==Tracking::OK) + { + vCurrentKeys = mvCurrentKeys; + vbVO = mvbVO; + vbMap = mvbMap; + + currentFrame = mCurrentFrame; + vpLocalMap = mvpLocalMap; + vMatchesKeys = mvMatchedKeys; + vpMatchedMPs = mvpMatchedMPs; + vOutlierKeys = mvOutlierKeys; + vpOutlierMPs = mvpOutlierMPs; + mProjectPoints = mmProjectPoints; + mMatchedInImage = mmMatchedInImage; + + vCurrentDepth = mvCurrentDepth; + thDepth = mThDepth; + + } + else if(mState==Tracking::LOST) + { + vCurrentKeys = mvCurrentKeys; + } + } + + if(imageScale != 1.f) + { + int imWidth = im.cols / imageScale; + int imHeight = im.rows / imageScale; + cv::resize(im, im, cv::Size(imWidth, imHeight)); + } + + if(im.channels()<3) //this should be always true + cvtColor(im,im,cv::COLOR_GRAY2BGR); + + //Draw + if(state==Tracking::NOT_INITIALIZED) + { + for(unsigned int i=0; i=0) + { + cv::Point2f pt1,pt2; + if(imageScale != 1.f) + { + pt1 = vIniKeys[i].pt / imageScale; + pt2 = vCurrentKeys[vMatches[i]].pt / imageScale; + } + else + { + pt1 = vIniKeys[i].pt; + pt2 = vCurrentKeys[vMatches[i]].pt; + } + cv::line(im,pt1,pt2,standardColor); + } + } + for(vector >::iterator it=vTracks.begin(); it!=vTracks.end(); it++) + { + cv::Point2f pt1,pt2; + if(imageScale != 1.f) + { + pt1 = (*it).first / imageScale; + pt2 = (*it).second / imageScale; + } + else + { + pt1 = (*it).first; + pt2 = (*it).second; + } + cv::line(im,pt1,pt2, standardColor,5); + } + + } + else if(state==Tracking::OK) //TRACKING + { + mnTracked=0; + mnTrackedVO=0; + const float r = 5; + int n = vCurrentKeys.size(); + for(int i=0;i vIniKeys; // Initialization: KeyPoints in reference frame + vector vMatches; // Initialization: correspondeces with reference keypoints + vector vCurrentKeys; // KeyPoints in current frame + vector vbVO, vbMap; // Tracked MapPoints in current frame + int state; // Tracking state + + //Copy variables within scoped mutex + { + unique_lock lock(mMutex); + state=mState; + if(mState==Tracking::SYSTEM_NOT_READY) + mState=Tracking::NO_IMAGES_YET; + + mImRight.copyTo(im); + + if(mState==Tracking::NOT_INITIALIZED) + { + vCurrentKeys = mvCurrentKeysRight; + vIniKeys = mvIniKeys; + vMatches = mvIniMatches; + } + else if(mState==Tracking::OK) + { + vCurrentKeys = mvCurrentKeysRight; + vbVO = mvbVO; + vbMap = mvbMap; + } + else if(mState==Tracking::LOST) + { + vCurrentKeys = mvCurrentKeysRight; + } + } // destroy scoped mutex -> release mutex + + if(imageScale != 1.f) + { + int imWidth = im.cols / imageScale; + int imHeight = im.rows / imageScale; + cv::resize(im, im, cv::Size(imWidth, imHeight)); + } + + if(im.channels()<3) //this should be always true + cvtColor(im,im,cv::COLOR_GRAY2BGR); + + //Draw + if(state==Tracking::NOT_INITIALIZED) //INITIALIZING + { + for(unsigned int i=0; i=0) + { + cv::Point2f pt1,pt2; + if(imageScale != 1.f) + { + pt1 = vIniKeys[i].pt / imageScale; + pt2 = vCurrentKeys[vMatches[i]].pt / imageScale; + } + else + { + pt1 = vIniKeys[i].pt; + pt2 = vCurrentKeys[vMatches[i]].pt; + } + + cv::line(im,pt1,pt2,cv::Scalar(0,255,0)); + } + } + } + else if(state==Tracking::OK) //TRACKING + { + mnTracked=0; + mnTrackedVO=0; + const float r = 5; + const int n = mvCurrentKeysRight.size(); + const int Nleft = mvCurrentKeys.size(); + + for(int i=0;iCountMaps(); + int nKFs = mpAtlas->KeyFramesInMap(); + int nMPs = mpAtlas->MapPointsInMap(); + s << "Maps: " << nMaps << ", KFs: " << nKFs << ", MPs: " << nMPs << ", Matches: " << mnTracked; + if(mnTrackedVO>0) + s << ", + VO matches: " << mnTrackedVO; + } + else if(nState==Tracking::LOST) + { + s << " TRACK LOST. TRYING TO RELOCALIZE "; + } + else if(nState==Tracking::SYSTEM_NOT_READY) + { + s << " LOADING ORB VOCABULARY. PLEASE WAIT..."; + } + + int baseline=0; + cv::Size textSize = cv::getTextSize(s.str(),cv::FONT_HERSHEY_PLAIN,1,1,&baseline); + + imText = cv::Mat(im.rows+textSize.height+10,im.cols,im.type()); + im.copyTo(imText.rowRange(0,im.rows).colRange(0,im.cols)); + imText.rowRange(im.rows,imText.rows) = cv::Mat::zeros(textSize.height+10,im.cols,im.type()); + cv::putText(imText,s.str(),cv::Point(5,imText.rows-5),cv::FONT_HERSHEY_PLAIN,1,cv::Scalar(255,255,255),1,8); + +} + +void FrameDrawer::Update(Tracking *pTracker) +{ + unique_lock lock(mMutex); + pTracker->mImGray.copyTo(mIm); + mvCurrentKeys=pTracker->mCurrentFrame.mvKeys; + mThDepth = pTracker->mCurrentFrame.mThDepth; + mvCurrentDepth = pTracker->mCurrentFrame.mvDepth; + + if(both){ + mvCurrentKeysRight = pTracker->mCurrentFrame.mvKeysRight; + pTracker->mImRight.copyTo(mImRight); + N = mvCurrentKeys.size() + mvCurrentKeysRight.size(); + } + else{ + N = mvCurrentKeys.size(); + } + + mvbVO = vector(N,false); + mvbMap = vector(N,false); + mbOnlyTracking = pTracker->mbOnlyTracking; + + //Variables for the new visualization + mCurrentFrame = pTracker->mCurrentFrame; + mmProjectPoints = mCurrentFrame.mmProjectPoints; + mmMatchedInImage.clear(); + + mvpLocalMap = pTracker->GetLocalMapMPS(); + mvMatchedKeys.clear(); + mvMatchedKeys.reserve(N); + mvpMatchedMPs.clear(); + mvpMatchedMPs.reserve(N); + mvOutlierKeys.clear(); + mvOutlierKeys.reserve(N); + mvpOutlierMPs.clear(); + mvpOutlierMPs.reserve(N); + + if(pTracker->mLastProcessedState==Tracking::NOT_INITIALIZED) + { + mvIniKeys=pTracker->mInitialFrame.mvKeys; + mvIniMatches=pTracker->mvIniMatches; + } + else if(pTracker->mLastProcessedState==Tracking::OK) + { + for(int i=0;imCurrentFrame.mvpMapPoints[i]; + if(pMP) + { + if(!pTracker->mCurrentFrame.mvbOutlier[i]) + { + if(pMP->Observations()>0) + mvbMap[i]=true; + else + mvbVO[i]=true; + + mmMatchedInImage[pMP->mnId] = mvCurrentKeys[i].pt; + } + else + { + mvpOutlierMPs.push_back(pMP); + mvOutlierKeys.push_back(mvCurrentKeys[i]); + } + } + } + + } + mState=static_cast(pTracker->mLastProcessedState); +} + +} //namespace ORB_SLAM diff --git a/ORB-SLAM3-UESTC/Workspace/src/G2oTypes.cc b/ORB-SLAM3-UESTC/Workspace/src/G2oTypes.cc new file mode 100644 index 0000000..0404424 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/G2oTypes.cc @@ -0,0 +1,994 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#include "G2oTypes.h" +#include "ImuTypes.h" +#include "Converter.h" +namespace ORB_SLAM3 +{ + +/** + * @brief 储存关键帧位姿相关的信息，用于优化 + * @param pKF 关键帧 + */ +ImuCamPose::ImuCamPose(KeyFrame *pKF):its(0) +{ + // Load IMU pose + twb = pKF->GetImuPosition().cast(); + Rwb = pKF->GetImuRotation().cast(); + + // Load camera poses + int num_cams; + if(pKF->mpCamera2) + num_cams=2; + else + num_cams=1; + + tcw.resize(num_cams); + Rcw.resize(num_cams); + tcb.resize(num_cams); + Rcb.resize(num_cams); + Rbc.resize(num_cams); + tbc.resize(num_cams); + pCamera.resize(num_cams); + + // Left camera + tcw[0] = pKF->GetTranslation().cast(); + Rcw[0] = pKF->GetRotation().cast(); + tcb[0] = pKF->mImuCalib.mTcb.translation().cast(); + Rcb[0] = pKF->mImuCalib.mTcb.rotationMatrix().cast(); + Rbc[0] = Rcb[0].transpose(); + tbc[0] = pKF->mImuCalib.mTbc.translation().cast(); + pCamera[0] = pKF->mpCamera; + bf = pKF->mbf; + + if(num_cams>1) + { + Eigen::Matrix4d Trl = pKF->GetRelativePoseTrl().matrix().cast(); + Rcw[1] = Trl.block<3,3>(0,0) * Rcw[0]; + tcw[1] = Trl.block<3,3>(0,0) * tcw[0] + Trl.block<3,1>(0,3); + tcb[1] = Trl.block<3,3>(0,0) * tcb[0] + Trl.block<3,1>(0,3); + Rcb[1] = Trl.block<3,3>(0,0) * Rcb[0]; + Rbc[1] = Rcb[1].transpose(); + tbc[1] = -Rbc[1] * tcb[1]; + pCamera[1] = pKF->mpCamera2; + } + + // For posegraph 4DoF + Rwb0 = Rwb; + DR.setIdentity(); +} + +/** + * @brief 储存普通帧位姿相关的信息，用于优化 + * @param pF 普通帧 + */ +ImuCamPose::ImuCamPose(Frame *pF):its(0) +{ + // Load IMU pose + twb = pF->GetImuPosition().cast(); + Rwb = pF->GetImuRotation().cast(); + + // Load camera poses + int num_cams; + if(pF->mpCamera2) + num_cams=2; + else + num_cams=1; + + tcw.resize(num_cams); + Rcw.resize(num_cams); + tcb.resize(num_cams); + Rcb.resize(num_cams); + Rbc.resize(num_cams); + tbc.resize(num_cams); + pCamera.resize(num_cams); + + // Left camera + tcw[0] = pF->GetPose().translation().cast(); + Rcw[0] = pF->GetPose().rotationMatrix().cast(); + tcb[0] = pF->mImuCalib.mTcb.translation().cast(); + Rcb[0] = pF->mImuCalib.mTcb.rotationMatrix().cast(); + Rbc[0] = Rcb[0].transpose(); + tbc[0] = pF->mImuCalib.mTbc.translation().cast(); + pCamera[0] = pF->mpCamera; + bf = pF->mbf; + + if(num_cams>1) + { + Eigen::Matrix4d Trl = pF->GetRelativePoseTrl().matrix().cast(); + Rcw[1] = Trl.block<3,3>(0,0) * Rcw[0]; + tcw[1] = Trl.block<3,3>(0,0) * tcw[0] + Trl.block<3,1>(0,3); + tcb[1] = Trl.block<3,3>(0,0) * tcb[0] + Trl.block<3,1>(0,3); + Rcb[1] = Trl.block<3,3>(0,0) * Rcb[0]; + Rbc[1] = Rcb[1].transpose(); + tbc[1] = -Rbc[1] * tcb[1]; + pCamera[1] = pF->mpCamera2; + } + + // For posegraph 4DoF + Rwb0 = Rwb; + DR.setIdentity(); +} + +/** + * @brief 储存位姿相关的信息，用于优化 + */ +ImuCamPose::ImuCamPose(Eigen::Matrix3d &_Rwc, Eigen::Vector3d &_twc, KeyFrame* pKF): its(0) +{ + // This is only for posegrpah, we do not care about multicamera + tcw.resize(1); + Rcw.resize(1); + tcb.resize(1); + Rcb.resize(1); + Rbc.resize(1); + tbc.resize(1); + pCamera.resize(1); + + tcb[0] = pKF->mImuCalib.mTcb.translation().cast(); + Rcb[0] = pKF->mImuCalib.mTcb.rotationMatrix().cast(); + Rbc[0] = Rcb[0].transpose(); + tbc[0] = pKF->mImuCalib.mTbc.translation().cast(); + twb = _Rwc * tcb[0] + _twc; + Rwb = _Rwc * Rcb[0]; + Rcw[0] = _Rwc.transpose(); + tcw[0] = -Rcw[0] * _twc; + pCamera[0] = pKF->mpCamera; + bf = pKF->mbf; + + // For posegraph 4DoF + Rwb0 = Rwb; + DR.setIdentity(); +} + +/** + * @brief 设置相关数据 + */ +void ImuCamPose::SetParam( + const std::vector &_Rcw, const std::vector &_tcw, + const std::vector &_Rbc, const std::vector &_tbc, const double &_bf) +{ + Rbc = _Rbc; + tbc = _tbc; + Rcw = _Rcw; + tcw = _tcw; + const int num_cams = Rbc.size(); + Rcb.resize(num_cams); + tcb.resize(num_cams); + + for(int i=0; iproject(Xc); +} + +/** + * @brief 双目投影，都是0 + * @return u v u` + */ +Eigen::Vector3d ImuCamPose::ProjectStereo(const Eigen::Vector3d &Xw, int cam_idx) const +{ + Eigen::Vector3d Pc = Rcw[cam_idx] * Xw + tcw[cam_idx]; + Eigen::Vector3d pc; + double invZ = 1/Pc(2); + pc.head(2) = pCamera[cam_idx]->project(Pc); + pc(2) = pc(0) - bf*invZ; + return pc; +} + +/** + * @brief 判断深度值是否有效 + */ +bool ImuCamPose::isDepthPositive(const Eigen::Vector3d &Xw, int cam_idx) const +{ + return (Rcw[cam_idx].row(2) * Xw + tcw[cam_idx](2)) > 0.0; +} + +/** + * @brief 优化算出更新值，更新到状态中 + * @param pu 更新值 + */ +void ImuCamPose::Update(const double *pu) +{ + Eigen::Vector3d ur, ut; + ur << pu[0], pu[1], pu[2]; + ut << pu[3], pu[4], pu[5]; + + // Update body pose + // 更新的是imu位姿 + twb += Rwb * ut; + Rwb = Rwb * ExpSO3(ur); + + // Normalize rotation after 5 updates + its++; + if(its>=3) + { + NormalizeRotation(Rwb); + its=0; + } + + // Update camera poses + const Eigen::Matrix3d Rbw = Rwb.transpose(); + const Eigen::Vector3d tbw = -Rbw * twb; + + for(int i=0; i=5) + { + DR(0,2) = 0.0; + DR(1,2) = 0.0; + DR(2,0) = 0.0; + DR(2,1) = 0.0; + NormalizeRotation(DR); + its = 0; + } + + // Update camera pose + const Eigen::Matrix3d Rbw = Rwb.transpose(); + const Eigen::Vector3d tbw = -Rbw * twb; + + for(int i=0; ifx), fy(pHostKF->fy), cx(pHostKF->cx), cy(pHostKF->cy), bf(pHostKF->mbf) +{ +} + +void InvDepthPoint::Update(const double *pu) +{ + rho += *pu; +} + +/** + * @brief 写入状态量 + */ +bool VertexPose::read(std::istream& is) +{ + std::vector > Rcw; + std::vector > tcw; + std::vector > Rbc; + std::vector > tbc; + + const int num_cams = _estimate.Rbc.size(); + for(int idx = 0; idx> Rcw[idx](i,j); + } + for (int i=0; i<3; i++){ + is >> tcw[idx](i); + } + + for (int i=0; i<3; i++){ + for (int j=0; j<3; j++) + is >> Rbc[idx](i,j); + } + for (int i=0; i<3; i++){ + is >> tbc[idx](i); + } + + float nextParam; + for(size_t i = 0; i < _estimate.pCamera[idx]->size(); i++){ + is >> nextParam; + _estimate.pCamera[idx]->setParameter(nextParam,i); + } + } + + double bf; + is >> bf; + _estimate.SetParam(Rcw,tcw,Rbc,tbc,bf); + updateCache(); + + return true; +} + +/** + * @brief 读出状态量 + */ +bool VertexPose::write(std::ostream& os) const +{ + std::vector > Rcw = _estimate.Rcw; + std::vector > tcw = _estimate.tcw; + + std::vector > Rbc = _estimate.Rbc; + std::vector > tbc = _estimate.tbc; + + const int num_cams = tcw.size(); + + for(int idx = 0; idxsize(); i++){ + os << _estimate.pCamera[idx]->getParameter(i) << " "; + } + } + + os << _estimate.bf << " "; + + return os.good(); +} + +/** + * @brief 单目视觉边计算雅克比 + * _jacobianOplusXi对应着_vertices[0] 也就是误差对于三维点的雅克比 + * _jacobianOplusXj就对应着位姿 + */ +void EdgeMono::linearizeOplus() +{ + const VertexPose* VPose = static_cast(_vertices[1]); + const g2o::VertexSBAPointXYZ* VPoint = static_cast(_vertices[0]); + + const Eigen::Matrix3d &Rcw = VPose->estimate().Rcw[cam_idx]; + const Eigen::Vector3d &tcw = VPose->estimate().tcw[cam_idx]; + const Eigen::Vector3d Xc = Rcw*VPoint->estimate() + tcw; + const Eigen::Vector3d Xb = VPose->estimate().Rbc[cam_idx]*Xc+VPose->estimate().tbc[cam_idx]; + const Eigen::Matrix3d &Rcb = VPose->estimate().Rcb[cam_idx]; + + const Eigen::Matrix proj_jac = VPose->estimate().pCamera[cam_idx]->projectJac(Xc); + + // 误差 = 观测-预测 + // -1 误差相对于预测的 + // proj_jac 像素相对于相机坐标系下的三维点的雅克比 + // Rcw 相机坐标系下的三维点 相对于世界坐标系下的三维点雅克比 + _jacobianOplusXi = -proj_jac * Rcw; + + Eigen::Matrix SE3deriv; + double x = Xb(0); + double y = Xb(1); + double z = Xb(2); + + SE3deriv << 0.0, z, -y, 1.0, 0.0, 0.0, + -z , 0.0, x, 0.0, 1.0, 0.0, + y , -x, 0.0, 0.0, 0.0, 1.0; + + // proj_jac 像素相对于相机坐标系下的三维点的雅克比 + // Rcb 相机坐标系下的三维点 相对于imu坐标系下的三维点雅克比 + // SE3deriv imu坐标系下的三维点 相对于 imu rt的雅克比 + // Rwb.t() * (Pw - twb) = Pb + // 求Pb对于Rwb与twb的雅克比，使用扰动的方式 Rwb -> Rwb * EXP(φ) twb -> twb + Rwb * δt + // 先带入Rwb (Rwb * EXP(φ)).t() * (Pw - twb) - Rwb.t() * (Pw - twb) + // 打开后算得imu坐标系下的三维点 相对于 imu r的雅克比为Pb^ + // 同理带入t可得imu坐标系下的三维点 相对于 imu t的雅克比为 -I + // 差了个负号，因为与proj_jac负号相抵，因此是正确的 + _jacobianOplusXj = proj_jac * Rcb * SE3deriv; // symbol different becasue of update mode +} + +/** + * @brief 单目视觉纯位姿边计算雅克比 + * _jacobianOplusXi对应着_vertices[0] 也就是误差对于位姿的雅克比 + */ +void EdgeMonoOnlyPose::linearizeOplus() +{ + const VertexPose* VPose = static_cast(_vertices[0]); + + const Eigen::Matrix3d &Rcw = VPose->estimate().Rcw[cam_idx]; + const Eigen::Vector3d &tcw = VPose->estimate().tcw[cam_idx]; + const Eigen::Vector3d Xc = Rcw*Xw + tcw; + const Eigen::Vector3d Xb = VPose->estimate().Rbc[cam_idx]*Xc+VPose->estimate().tbc[cam_idx]; + const Eigen::Matrix3d &Rcb = VPose->estimate().Rcb[cam_idx]; + + Eigen::Matrix proj_jac = VPose->estimate().pCamera[cam_idx]->projectJac(Xc); + + Eigen::Matrix SE3deriv; + double x = Xb(0); + double y = Xb(1); + double z = Xb(2); + SE3deriv << 0.0, z, -y, 1.0, 0.0, 0.0, + -z , 0.0, x, 0.0, 1.0, 0.0, + y , - x, 0.0, 0.0, 0.0, 1.0; + _jacobianOplusXi = proj_jac * Rcb * SE3deriv; // symbol different becasue of update mode +} + +/** + * @brief 双目视觉边计算雅克比，多了一维误差 + * _jacobianOplusXi对应着_vertices[0] 也就是误差对于三维点的雅克比 + * _jacobianOplusXj就对应着位姿 + */ +void EdgeStereo::linearizeOplus() +{ + const VertexPose* VPose = static_cast(_vertices[1]); + const g2o::VertexSBAPointXYZ* VPoint = static_cast(_vertices[0]); + + const Eigen::Matrix3d &Rcw = VPose->estimate().Rcw[cam_idx]; + const Eigen::Vector3d &tcw = VPose->estimate().tcw[cam_idx]; + const Eigen::Vector3d Xc = Rcw*VPoint->estimate() + tcw; + const Eigen::Vector3d Xb = VPose->estimate().Rbc[cam_idx]*Xc+VPose->estimate().tbc[cam_idx]; + const Eigen::Matrix3d &Rcb = VPose->estimate().Rcb[cam_idx]; + const double bf = VPose->estimate().bf; + const double inv_z2 = 1.0/(Xc(2)*Xc(2)); + + Eigen::Matrix proj_jac; + proj_jac.block<2,3>(0,0) = VPose->estimate().pCamera[cam_idx]->projectJac(Xc); + proj_jac.block<1,3>(2,0) = proj_jac.block<1,3>(0,0); + proj_jac(2,2) += bf*inv_z2; + + _jacobianOplusXi = -proj_jac * Rcw; + + Eigen::Matrix SE3deriv; + double x = Xb(0); + double y = Xb(1); + double z = Xb(2); + + SE3deriv << 0.0, z, -y, 1.0, 0.0, 0.0, + -z , 0.0, x, 0.0, 1.0, 0.0, + y , -x , 0.0, 0.0, 0.0, 1.0; + + _jacobianOplusXj = proj_jac * Rcb * SE3deriv; +} + +/** + * @brief 双目视觉纯位姿边计算雅克比，多了一维误差 + * _jacobianOplusXi对应着_vertices[0] 也就是误差对于位姿的雅克比 + */ +void EdgeStereoOnlyPose::linearizeOplus() +{ + const VertexPose* VPose = static_cast(_vertices[0]); + + const Eigen::Matrix3d &Rcw = VPose->estimate().Rcw[cam_idx]; + const Eigen::Vector3d &tcw = VPose->estimate().tcw[cam_idx]; + const Eigen::Vector3d Xc = Rcw*Xw + tcw; + const Eigen::Vector3d Xb = VPose->estimate().Rbc[cam_idx]*Xc+VPose->estimate().tbc[cam_idx]; + const Eigen::Matrix3d &Rcb = VPose->estimate().Rcb[cam_idx]; + const double bf = VPose->estimate().bf; + const double inv_z2 = 1.0/(Xc(2)*Xc(2)); + + Eigen::Matrix proj_jac; + proj_jac.block<2,3>(0,0) = VPose->estimate().pCamera[cam_idx]->projectJac(Xc); + proj_jac.block<1,3>(2,0) = proj_jac.block<1,3>(0,0); + proj_jac(2,2) += bf*inv_z2; + + Eigen::Matrix SE3deriv; + double x = Xb(0); + double y = Xb(1); + double z = Xb(2); + SE3deriv << 0.0, z, -y, 1.0, 0.0, 0.0, + -z , 0.0, x, 0.0, 1.0, 0.0, + y , -x , 0.0, 0.0, 0.0, 1.0; + _jacobianOplusXi = proj_jac * Rcb * SE3deriv; +} + +VertexVelocity::VertexVelocity(KeyFrame* pKF) +{ + setEstimate(pKF->GetVelocity().cast()); +} + +VertexVelocity::VertexVelocity(Frame* pF) +{ + setEstimate(pF->GetVelocity().cast()); +} + +VertexGyroBias::VertexGyroBias(KeyFrame *pKF) +{ + setEstimate(pKF->GetGyroBias().cast()); +} + +VertexGyroBias::VertexGyroBias(Frame *pF) +{ + Eigen::Vector3d bg; + bg << pF->mImuBias.bwx, pF->mImuBias.bwy,pF->mImuBias.bwz; + setEstimate(bg); +} + +VertexAccBias::VertexAccBias(KeyFrame *pKF) +{ + setEstimate(pKF->GetAccBias().cast()); +} + +VertexAccBias::VertexAccBias(Frame *pF) +{ + Eigen::Vector3d ba; + ba << pF->mImuBias.bax, pF->mImuBias.bay,pF->mImuBias.baz; + setEstimate(ba); +} + + +/** + * @brief 局部地图中imu的局部地图优化（此时已经初始化完毕不需要再优化重力方向与尺度） + * @param pInt 预积分相关内容 + */ +EdgeInertial::EdgeInertial(IMU::Preintegrated *pInt):JRg(pInt->JRg.cast()), + JVg(pInt->JVg.cast()), JPg(pInt->JPg.cast()), JVa(pInt->JVa.cast()), + JPa(pInt->JPa.cast()), mpInt(pInt), dt(pInt->dT) +{ + // 准备工作，把预积分类里面的值先取出来，包含信息的是两帧之间n多个imu信息预积分来的 + // This edge links 6 vertices + // 6元边 + resize(6); + // 1. 定义重力 + g << 0, 0, -IMU::GRAVITY_VALUE; + + // 2. 读取协方差矩阵的前9*9部分的逆矩阵，该部分表示的是预积分测量噪声的协方差矩阵 + Matrix9d Info = pInt->C.block<9,9>(0,0).cast().inverse(); + // 3. 强制让其成为对角矩阵 + Info = (Info+Info.transpose())/2; + // 4. 让特征值很小的时候置为0，再重新计算信息矩阵（暂不知这么操作的目的是什么，先搞清楚操作流程吧） + Eigen::SelfAdjointEigenSolver > es(Info); + Eigen::Matrix eigs = es.eigenvalues(); + for(int i=0;i<9;i++) + if(eigs[i]<1e-12) + eigs[i]=0; + // asDiagonal 生成对角矩阵 + Info = es.eigenvectors()*eigs.asDiagonal()*es.eigenvectors().transpose(); + setInformation(Info); +} + + + +/** + * @brief 计算误差 + */ +void EdgeInertial::computeError() +{ + // TODO Maybe Reintegrate inertial measurments when difference between linearization point and current estimate is too big + const VertexPose* VP1 = static_cast(_vertices[0]); //位姿Ti + const VertexVelocity* VV1= static_cast(_vertices[1]); //速度vi + const VertexGyroBias* VG1= static_cast(_vertices[2]); //零偏Bgi + const VertexAccBias* VA1= static_cast(_vertices[3]); //零偏Bai + const VertexPose* VP2 = static_cast(_vertices[4]); //位姿Tj + const VertexVelocity* VV2 = static_cast(_vertices[5]); //速度vj + const IMU::Bias b1(VA1->estimate()[0],VA1->estimate()[1],VA1->estimate()[2],VG1->estimate()[0],VG1->estimate()[1],VG1->estimate()[2]); + const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b1).cast(); + const Eigen::Vector3d dV = mpInt->GetDeltaVelocity(b1).cast(); + const Eigen::Vector3d dP = mpInt->GetDeltaPosition(b1).cast(); + + const Eigen::Vector3d er = LogSO3(dR.transpose()*VP1->estimate().Rwb.transpose()*VP2->estimate().Rwb); + const Eigen::Vector3d ev = VP1->estimate().Rwb.transpose()*(VV2->estimate() - VV1->estimate() - g*dt) - dV; + const Eigen::Vector3d ep = VP1->estimate().Rwb.transpose()*(VP2->estimate().twb - VP1->estimate().twb + - VV1->estimate()*dt - g*dt*dt/2) - dP; + + _error << er, ev, ep; +} + +// 计算雅克比矩阵 +void EdgeInertial::linearizeOplus() +{ + const VertexPose* VP1 = static_cast(_vertices[0]); + const VertexVelocity* VV1= static_cast(_vertices[1]); + const VertexGyroBias* VG1= static_cast(_vertices[2]); + const VertexAccBias* VA1= static_cast(_vertices[3]); + const VertexPose* VP2 = static_cast(_vertices[4]); + const VertexVelocity* VV2= static_cast(_vertices[5]); + const IMU::Bias b1(VA1->estimate()[0],VA1->estimate()[1],VA1->estimate()[2],VG1->estimate()[0],VG1->estimate()[1],VG1->estimate()[2]); + const IMU::Bias db = mpInt->GetDeltaBias(b1); + Eigen::Vector3d dbg; + dbg << db.bwx, db.bwy, db.bwz; + + const Eigen::Matrix3d Rwb1 = VP1->estimate().Rwb; // Ri + const Eigen::Matrix3d Rbw1 = Rwb1.transpose(); // Ri.t() + const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb; // Rj + + const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b1).cast(); + const Eigen::Matrix3d eR = dR.transpose()*Rbw1*Rwb2; // r△Rij + const Eigen::Vector3d er = LogSO3(eR); // r△φij + const Eigen::Matrix3d invJr = InverseRightJacobianSO3(er); // Jr^-1(log(△Rij)) + + // 就很神奇，_jacobianOplus个数等于边的个数，里面的大小等于观测值维度（也就是残差）× 每个节点待优化值的维度 + // Jacobians wrt Pose 1 + // _jacobianOplus[0] 9*6矩阵 总体来说就是三个残差分别对pose1的旋转与平移（p）求导 + _jacobianOplus[0].setZero(); + // rotation + // (0,0)起点的3*3块表示旋转残差对pose1的旋转求导 + _jacobianOplus[0].block<3,3>(0,0) = -invJr*Rwb2.transpose()*Rwb1; // OK + // (3,0)起点的3*3块表示速度残差对pose1的旋转求导 + _jacobianOplus[0].block<3,3>(3,0) = Sophus::SO3d::hat(Rbw1*(VV2->estimate() - VV1->estimate() - g*dt)); // OK + // (6,0)起点的3*3块表示位置残差对pose1的旋转求导 + _jacobianOplus[0].block<3,3>(6,0) = Sophus::SO3d::hat(Rbw1*(VP2->estimate().twb - VP1->estimate().twb + - VV1->estimate()*dt - 0.5*g*dt*dt)); // OK + // translation + // (6,3)起点的3*3块表示位置残差对pose1的位置求导 + _jacobianOplus[0].block<3,3>(6,3) = -Eigen::Matrix3d::Identity(); // OK + + // Jacobians wrt Velocity 1 + // _jacobianOplus[1] 9*3矩阵 总体来说就是三个残差分别对pose1的速度求导 + _jacobianOplus[1].setZero(); + _jacobianOplus[1].block<3,3>(3,0) = -Rbw1; // OK + _jacobianOplus[1].block<3,3>(6,0) = -Rbw1*dt; // OK + + // Jacobians wrt Gyro 1 + // _jacobianOplus[2] 9*3矩阵 总体来说就是三个残差分别对陀螺仪偏置的速度求导 + _jacobianOplus[2].setZero(); + _jacobianOplus[2].block<3,3>(0,0) = -invJr*eR.transpose()*RightJacobianSO3(JRg*dbg)*JRg; // OK + _jacobianOplus[2].block<3,3>(3,0) = -JVg; // OK + _jacobianOplus[2].block<3,3>(6,0) = -JPg; // OK + + // Jacobians wrt Accelerometer 1 + // _jacobianOplus[3] 9*3矩阵 总体来说就是三个残差分别对加速度计偏置的速度求导 + _jacobianOplus[3].setZero(); + _jacobianOplus[3].block<3,3>(3,0) = -JVa; // OK + _jacobianOplus[3].block<3,3>(6,0) = -JPa; // OK + + // Jacobians wrt Pose 2 + // _jacobianOplus[4] 9*6矩阵 总体来说就是三个残差分别对pose2的旋转与平移（p）求导 + _jacobianOplus[4].setZero(); + // rotation + _jacobianOplus[4].block<3,3>(0,0) = invJr; // OK + // translation + _jacobianOplus[4].block<3,3>(6,3) = Rbw1*Rwb2; // OK + + // Jacobians wrt Velocity 2 + // _jacobianOplus[5] 9*3矩阵 总体来说就是三个残差分别对pose2的速度求导 + _jacobianOplus[5].setZero(); + _jacobianOplus[5].block<3,3>(3,0) = Rbw1; // OK +} + +// localmapping中imu初始化所用的边，除了正常的几个优化变量外还优化了重力方向与尺度 +EdgeInertialGS::EdgeInertialGS(IMU::Preintegrated *pInt):JRg(pInt->JRg.cast()), + JVg(pInt->JVg.cast()), JPg(pInt->JPg.cast()), JVa(pInt->JVa.cast()), + JPa(pInt->JPa.cast()), mpInt(pInt), dt(pInt->dT) +{ + // 准备工作，把预积分类里面的值先取出来，包含信息的是两帧之间n多个imu信息预积分来的 + // This edge links 8 vertices + // 8元边 + resize(8); + // 1. 定义重力 + gI << 0, 0, -IMU::GRAVITY_VALUE; + + // 2. 读取协方差矩阵的前9*9部分的逆矩阵，该部分表示的是预积分测量噪声的协方差矩阵 + Matrix9d Info = pInt->C.block<9,9>(0,0).cast().inverse(); + // 3. 强制让其成为对角矩阵 + Info = (Info+Info.transpose())/2; + Eigen::SelfAdjointEigenSolver > es(Info); + // 4. 让特征值很小的时候置为0，再重新计算信息矩阵（暂不知这么操作的目的是什么，先搞清楚操作流程吧） + Eigen::Matrix eigs = es.eigenvalues(); + for(int i=0;i<9;i++) + if(eigs[i]<1e-12) + eigs[i]=0; + // asDiagonal 生成对角矩阵 + Info = es.eigenvectors()*eigs.asDiagonal()*es.eigenvectors().transpose(); + setInformation(Info); +} + + +// 计算误差 +void EdgeInertialGS::computeError() +{ + // TODO Maybe Reintegrate inertial measurments when difference between linearization point and current estimate is too big + const VertexPose* VP1 = static_cast(_vertices[0]); + const VertexVelocity* VV1= static_cast(_vertices[1]); + const VertexGyroBias* VG= static_cast(_vertices[2]); + const VertexAccBias* VA= static_cast(_vertices[3]); + const VertexPose* VP2 = static_cast(_vertices[4]); + const VertexVelocity* VV2 = static_cast(_vertices[5]); + const VertexGDir* VGDir = static_cast(_vertices[6]); + const VertexScale* VS = static_cast(_vertices[7]); + const IMU::Bias b(VA->estimate()[0],VA->estimate()[1],VA->estimate()[2],VG->estimate()[0],VG->estimate()[1],VG->estimate()[2]); + g = VGDir->estimate().Rwg*gI; + const double s = VS->estimate(); + const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b).cast(); + const Eigen::Vector3d dV = mpInt->GetDeltaVelocity(b).cast(); + const Eigen::Vector3d dP = mpInt->GetDeltaPosition(b).cast(); + + // 计算残差。广义上讲都是真实值 = 残差 + imu，旋转为imu*残差=真实值 + // dR.transpose() 为imu预积分的值，VP1->estimate().Rwb.transpose() * VP2->estimate().Rwb 为相机的Rwc在乘上相机与imu的标定外参矩阵 + const Eigen::Vector3d er = LogSO3(dR.transpose()*VP1->estimate().Rwb.transpose()*VP2->estimate().Rwb); + const Eigen::Vector3d ev = VP1->estimate().Rwb.transpose()*(s*(VV2->estimate() - VV1->estimate()) - g*dt) - dV; + const Eigen::Vector3d ep = VP1->estimate().Rwb.transpose()*(s*(VP2->estimate().twb - VP1->estimate().twb - VV1->estimate()*dt) - g*dt*dt/2) - dP; + + _error << er, ev, ep; +} + +// 计算雅克比矩阵 +void EdgeInertialGS::linearizeOplus() +{ + const VertexPose* VP1 = static_cast(_vertices[0]); + const VertexVelocity* VV1= static_cast(_vertices[1]); + const VertexGyroBias* VG= static_cast(_vertices[2]); + const VertexAccBias* VA= static_cast(_vertices[3]); + const VertexPose* VP2 = static_cast(_vertices[4]); + const VertexVelocity* VV2 = static_cast(_vertices[5]); + const VertexGDir* VGDir = static_cast(_vertices[6]); + const VertexScale* VS = static_cast(_vertices[7]); + // 1. 获取偏置的该变量，因为要对这个东西求导 + const IMU::Bias b(VA->estimate()[0],VA->estimate()[1],VA->estimate()[2],VG->estimate()[0],VG->estimate()[1],VG->estimate()[2]); + const IMU::Bias db = mpInt->GetDeltaBias(b); + + // 陀螺仪的偏置改变量 + Eigen::Vector3d dbg; + dbg << db.bwx, db.bwy, db.bwz; + + const Eigen::Matrix3d Rwb1 = VP1->estimate().Rwb; // Ri + const Eigen::Matrix3d Rbw1 = Rwb1.transpose(); // Ri.t() + const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb; // Rj + const Eigen::Matrix3d Rwg = VGDir->estimate().Rwg; // Rwg + Eigen::MatrixXd Gm = Eigen::MatrixXd::Zero(3,2); + Gm(0,1) = -IMU::GRAVITY_VALUE; + Gm(1,0) = IMU::GRAVITY_VALUE; + const double s = VS->estimate(); + const Eigen::MatrixXd dGdTheta = Rwg*Gm; + const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b).cast(); + const Eigen::Matrix3d eR = dR.transpose()*Rbw1*Rwb2; // r△Rij + const Eigen::Vector3d er = LogSO3(eR); // r△φij + const Eigen::Matrix3d invJr = InverseRightJacobianSO3(er); // Jr^-1(log(△Rij)) + + // 就很神奇，_jacobianOplus个数等于边的个数，里面的大小等于观测值维度（也就是残差）× 每个节点待优化值的维度 + // Jacobians wrt Pose 1 + // _jacobianOplus[0] 9*6矩阵 总体来说就是三个残差分别对pose1的旋转与平移（p）求导 + _jacobianOplus[0].setZero(); + // rotation + // (0,0)起点的3*3块表示旋转残差对pose1的旋转求导 + _jacobianOplus[0].block<3,3>(0,0) = -invJr*Rwb2.transpose()*Rwb1; + // (3,0)起点的3*3块表示速度残差对pose1的旋转求导 + _jacobianOplus[0].block<3,3>(3,0) = Sophus::SO3d::hat(Rbw1*(s*(VV2->estimate() - VV1->estimate()) - g*dt)); + // (6,0)起点的3*3块表示位置残差对pose1的旋转求导 + _jacobianOplus[0].block<3,3>(6,0) = Sophus::SO3d::hat(Rbw1*(s*(VP2->estimate().twb - VP1->estimate().twb + - VV1->estimate()*dt) - 0.5*g*dt*dt)); + // translation + // (6,3)起点的3*3块表示位置残差对pose1的位置求导 + _jacobianOplus[0].block<3,3>(6,3) = Eigen::DiagonalMatrix(-s,-s,-s); + + // Jacobians wrt Velocity 1 + // _jacobianOplus[1] 9*3矩阵 总体来说就是三个残差分别对pose1的速度求导 + _jacobianOplus[1].setZero(); + _jacobianOplus[1].block<3,3>(3,0) = -s*Rbw1; + _jacobianOplus[1].block<3,3>(6,0) = -s*Rbw1*dt; + + // Jacobians wrt Gyro bias + // _jacobianOplus[2] 9*3矩阵 总体来说就是三个残差分别对陀螺仪偏置的速度求导 + _jacobianOplus[2].setZero(); + _jacobianOplus[2].block<3,3>(0,0) = -invJr*eR.transpose()*RightJacobianSO3(JRg*dbg)*JRg; + _jacobianOplus[2].block<3,3>(3,0) = -JVg; + _jacobianOplus[2].block<3,3>(6,0) = -JPg; + + // Jacobians wrt Accelerometer bias + // _jacobianOplus[3] 9*3矩阵 总体来说就是三个残差分别对加速度计偏置的速度求导 + _jacobianOplus[3].setZero(); + _jacobianOplus[3].block<3,3>(3,0) = -JVa; + _jacobianOplus[3].block<3,3>(6,0) = -JPa; + + // Jacobians wrt Pose 2 + // _jacobianOplus[3] 9*6矩阵 总体来说就是三个残差分别对pose2的旋转与平移（p）求导 + _jacobianOplus[4].setZero(); + // rotation + _jacobianOplus[4].block<3,3>(0,0) = invJr; + // translation + _jacobianOplus[4].block<3,3>(6,3) = s*Rbw1*Rwb2; + + // Jacobians wrt Velocity 2 + // _jacobianOplus[3] 9*3矩阵 总体来说就是三个残差分别对pose2的速度求导 + _jacobianOplus[5].setZero(); + _jacobianOplus[5].block<3,3>(3,0) = s*Rbw1; + + // Jacobians wrt Gravity direction + // _jacobianOplus[3] 9*2矩阵 总体来说就是三个残差分别对重力方向求导 + _jacobianOplus[6].setZero(); + _jacobianOplus[6].block<3,2>(3,0) = -Rbw1*dGdTheta*dt; + _jacobianOplus[6].block<3,2>(6,0) = -0.5*Rbw1*dGdTheta*dt*dt; + + // Jacobians wrt scale factor + // _jacobianOplus[3] 9*1矩阵 总体来说就是三个残差分别对尺度求导 + _jacobianOplus[7].setZero(); + _jacobianOplus[7].block<3,1>(3,0) = Rbw1*(VV2->estimate()-VV1->estimate()) * s; + _jacobianOplus[7].block<3,1>(6,0) = Rbw1*(VP2->estimate().twb-VP1->estimate().twb-VV1->estimate()*dt) * s; +} + +/** + * @brief 滑窗边缘化时用的先验边 + */ +EdgePriorPoseImu::EdgePriorPoseImu(ConstraintPoseImu *c) +{ + resize(4); + Rwb = c->Rwb; + twb = c->twb; + vwb = c->vwb; + bg = c->bg; + ba = c->ba; + setInformation(c->H); +} + +/** + * @brief 先验边计算误差 + */ +void EdgePriorPoseImu::computeError() +{ + const VertexPose* VP = static_cast(_vertices[0]); + const VertexVelocity* VV = static_cast(_vertices[1]); + const VertexGyroBias* VG = static_cast(_vertices[2]); + const VertexAccBias* VA = static_cast(_vertices[3]); + + const Eigen::Vector3d er = LogSO3(Rwb.transpose()*VP->estimate().Rwb); + const Eigen::Vector3d et = Rwb.transpose()*(VP->estimate().twb-twb); + const Eigen::Vector3d ev = VV->estimate() - vwb; + const Eigen::Vector3d ebg = VG->estimate() - bg; + const Eigen::Vector3d eba = VA->estimate() - ba; + + _error << er, et, ev, ebg, eba; +} + +/** + * @brief 先验边计算雅克比 + */ +void EdgePriorPoseImu::linearizeOplus() +{ + const VertexPose* VP = static_cast(_vertices[0]); + const Eigen::Vector3d er = LogSO3(Rwb.transpose()*VP->estimate().Rwb); + // 就很神奇，_jacobianOplus个数等于边的个数，里面的大小等于观测值维度（也就是3旋转3平移3速度6偏置）× 每个节点待优化值的维度 + // 源码可读性太差了。。。里面会自动分配矩阵大小，计算改变量时按照对应位置来 + _jacobianOplus[0].setZero(); + // LOG(Rbw*R*EXP(φ)) = LOG(EXP(LOG(Rbw*R) + Jr(-1)*φ)) = LOG(Rbw*R) + Jr(-1)*φ + _jacobianOplus[0].block<3,3>(0,0) = InverseRightJacobianSO3(er); // Jr(-1) + // Rbw*(t + R*δt - twb) = Rbw*(t - twb) + Rbw*R*δt + _jacobianOplus[0].block<3,3>(3,3) = Rwb.transpose()*VP->estimate().Rwb; // Rbw*R + _jacobianOplus[1].setZero(); + _jacobianOplus[1].block<3,3>(6,0) = Eigen::Matrix3d::Identity(); + _jacobianOplus[2].setZero(); + _jacobianOplus[2].block<3,3>(9,0) = Eigen::Matrix3d::Identity(); + _jacobianOplus[3].setZero(); + _jacobianOplus[3].block<3,3>(12,0) = Eigen::Matrix3d::Identity(); +} + +void EdgePriorAcc::linearizeOplus() +{ + // Jacobian wrt bias + _jacobianOplusXi.block<3,3>(0,0) = Eigen::Matrix3d::Identity(); + +} + +void EdgePriorGyro::linearizeOplus() +{ + // Jacobian wrt bias + _jacobianOplusXi.block<3,3>(0,0) = Eigen::Matrix3d::Identity(); + +} + +// SO3 FUNCTIONS +Eigen::Matrix3d ExpSO3(const Eigen::Vector3d &w) +{ + return ExpSO3(w[0],w[1],w[2]); +} + +Eigen::Matrix3d ExpSO3(const double x, const double y, const double z) +{ + const double d2 = x*x+y*y+z*z; + const double d = sqrt(d2); + Eigen::Matrix3d W; + W << 0.0, -z, y,z, 0.0, -x,-y, x, 0.0; + if(d<1e-5) + { + Eigen::Matrix3d res = Eigen::Matrix3d::Identity() + W +0.5*W*W; + return NormalizeRotation(res); + } + else + { + Eigen::Matrix3d res =Eigen::Matrix3d::Identity() + W*sin(d)/d + W*W*(1.0-cos(d))/d2; + return NormalizeRotation(res); + } +} + +Eigen::Vector3d LogSO3(const Eigen::Matrix3d &R) +{ + const double tr = R(0,0)+R(1,1)+R(2,2); + Eigen::Vector3d w; + w << (R(2,1)-R(1,2))/2, (R(0,2)-R(2,0))/2, (R(1,0)-R(0,1))/2; + const double costheta = (tr-1.0)*0.5f; + if(costheta>1 || costheta<-1) + return w; + const double theta = acos(costheta); + const double s = sin(theta); + if(fabs(s)<1e-5) + return w; + else + return theta*w/s; +} + +Eigen::Matrix3d InverseRightJacobianSO3(const Eigen::Vector3d &v) +{ + return InverseRightJacobianSO3(v[0],v[1],v[2]); +} + +Eigen::Matrix3d InverseRightJacobianSO3(const double x, const double y, const double z) +{ + const double d2 = x*x+y*y+z*z; + const double d = sqrt(d2); + + Eigen::Matrix3d W; + W << 0.0, -z, y,z, 0.0, -x,-y, x, 0.0; + if(d<1e-5) + return Eigen::Matrix3d::Identity(); + else + return Eigen::Matrix3d::Identity() + W/2 + W*W*(1.0/d2 - (1.0+cos(d))/(2.0*d*sin(d))); +} + +Eigen::Matrix3d RightJacobianSO3(const Eigen::Vector3d &v) +{ + return RightJacobianSO3(v[0],v[1],v[2]); +} + +Eigen::Matrix3d RightJacobianSO3(const double x, const double y, const double z) +{ + const double d2 = x*x+y*y+z*z; + const double d = sqrt(d2); + + Eigen::Matrix3d W; + W << 0.0, -z, y,z, 0.0, -x,-y, x, 0.0; + if(d<1e-5) + { + return Eigen::Matrix3d::Identity(); + } + else + { + return Eigen::Matrix3d::Identity() - W*(1.0-cos(d))/d2 + W*W*(d-sin(d))/(d2*d); + } +} + +Eigen::Matrix3d Skew(const Eigen::Vector3d &w) +{ + Eigen::Matrix3d W; + W << 0.0, -w[2], w[1],w[2], 0.0, -w[0],-w[1], w[0], 0.0; + return W; +} + +} diff --git a/ORB-SLAM3-UESTC/Workspace/src/GeometricTools.cc b/ORB-SLAM3-UESTC/Workspace/src/GeometricTools.cc new file mode 100644 index 0000000..4df6b86 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/GeometricTools.cc @@ -0,0 +1,92 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#include "GeometricTools.h" + +#include "KeyFrame.h" + +namespace ORB_SLAM3 +{ + +/** + * @brief 计算两个关键帧间的基础矩阵 + * @param pKF1 关键帧1 + * @param pKF2 关键帧2 + */ +Eigen::Matrix3f GeometricTools::ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2) +{ + // 获取关键帧1的旋转平移 + Sophus::SE3 Tc1w = pKF1->GetPose(); + Sophus::Matrix3 Rc1w = Tc1w.rotationMatrix(); + Sophus::SE3::TranslationMember tc1w = Tc1w.translation(); + + // 获取关键帧2的旋转平移 + Sophus::SE3 Tc2w = pKF2->GetPose(); + Sophus::Matrix3 Rc2w = Tc2w.rotationMatrix(); + Sophus::SE3::TranslationMember tc2w = Tc2w.translation(); + + // 计算2->1的旋转平移 + Sophus::Matrix3 Rc1c2 = Rc1w * Rc2w.transpose(); + Eigen::Vector3f tc1c2 = -Rc1c2 * tc2w + tc1w; + + Eigen::Matrix3f tc1c2x = Sophus::SO3f::hat(tc1c2); + + const Eigen::Matrix3f K1 = pKF1->mpCamera->toK_(); + const Eigen::Matrix3f K2 = pKF2->mpCamera->toK_(); + + return K1.transpose().inverse() * tc1c2x * Rc1c2 * K2.inverse(); +} + +/** + * @brief 三角化获得三维点 + * @param x_c1 点在关键帧1下的归一化坐标 + * @param x_c2 点在关键帧2下的归一化坐标 + * @param Tc1w 关键帧1投影矩阵 [K*R | K*t] + * @param Tc2w 关键帧2投影矩阵 [K*R | K*t] + * @param x3D 三维点坐标，作为结果输出 + */ +bool GeometricTools::Triangulate( + Eigen::Vector3f &x_c1, Eigen::Vector3f &x_c2, Eigen::Matrix &Tc1w, Eigen::Matrix &Tc2w, + Eigen::Vector3f &x3D) +{ + Eigen::Matrix4f A; + // x = a*P*X， 左右两面乘Pc的反对称矩阵 a*[x]^ * P *X = 0 构成了A矩阵，中间涉及一个尺度a，因为都是归一化平面，但右面是0所以直接可以约掉不影响最后的尺度 + // 0 -1 v P(0) -P.row(1) + v*P.row(2) + // 1 0 -u * P(1) = P.row(0) - u*P.row(2) + // -v u 0 P(2) u*P.row(1) - v*P.row(0) + // 发现上述矩阵线性相关，所以取前两维，两个点构成了4行的矩阵，就是如下的操作，求出的是4维的结果[X,Y,Z,A]，所以需要除以最后一维使之为1，就成了[X,Y,Z,1]这种齐次形式 + A.block<1,4>(0,0) = x_c1(0) * Tc1w.block<1,4>(2,0) - Tc1w.block<1,4>(0,0); + A.block<1,4>(1,0) = x_c1(1) * Tc1w.block<1,4>(2,0) - Tc1w.block<1,4>(1,0); + A.block<1,4>(2,0) = x_c2(0) * Tc2w.block<1,4>(2,0) - Tc2w.block<1,4>(0,0); + A.block<1,4>(3,0) = x_c2(1) * Tc2w.block<1,4>(2,0) - Tc2w.block<1,4>(1,0); + + // 解方程 AX=0 + Eigen::JacobiSVD svd(A, Eigen::ComputeFullV); + + Eigen::Vector4f x3Dh = svd.matrixV().col(3); + + if(x3Dh(3)==0) + return false; + + // Euclidean coordinates + x3D = x3Dh.head(3)/x3Dh(3); + + return true; +} + +} //namespace ORB_SLAM diff --git a/ORB-SLAM3-UESTC/Workspace/src/ImuTypes.cc b/ORB-SLAM3-UESTC/Workspace/src/ImuTypes.cc new file mode 100644 index 0000000..0656edf --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/ImuTypes.cc @@ -0,0 +1,598 @@ +/** + * This file is part of ORB-SLAM3 + * + * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * + * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public + * License as published by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even + * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with ORB-SLAM3. + * If not, see . + */ + +#include "ImuTypes.h" +#include "Converter.h" + +#include "GeometricTools.h" + +#include + +namespace ORB_SLAM3 +{ + +namespace IMU +{ + +const float eps = 1e-4; + +/** + * @brief 强制让R变成一个正交矩阵 + * @param R 待优化的旋转矩阵 + * @return 优化后的矩阵 + */ +Eigen::Matrix3f NormalizeRotation(const Eigen::Matrix3f &R) +{ + // 这里关注一下 + // 1. 对于行列数一样的矩阵，Eigen::ComputeThinU | Eigen::ComputeThinV 与 Eigen::ComputeFullU | Eigen::ComputeFullV 一样 + // 2. 对于行列数不同的矩阵，例如3*4 或者 4*3 矩阵只有3个奇异向量，计算的时候如果是Thin 那么得出的UV矩阵列数只能是3，如果是full那么就是4 + // 3. thin会损失一部分数据，但是会加快计算，对于大型矩阵解算方程时，可以用thin加速得到结果 + Eigen::JacobiSVD svd(R, Eigen::ComputeFullU | Eigen::ComputeFullV); + return svd.matrixU() * svd.matrixV().transpose(); +} + +/** + * @brief 计算右雅可比 + * @param xyz 李代数 + * @return Jr + */ +Eigen::Matrix3f RightJacobianSO3(const float &x, const float &y, const float &z) +{ + Eigen::Matrix3f I; + I.setIdentity(); + const float d2 = x * x + y * y + z * z; + const float d = sqrt(d2); + Eigen::Vector3f v; + v << x, y, z; + Eigen::Matrix3f W = Sophus::SO3f::hat(v); + if (d < eps) + { + return I; + } + else + { + return I - W * (1.0f - cos(d)) / d2 + W * W * (d - sin(d)) / (d2 * d); + } +} + +/** + * @brief 计算右雅可比 + * @param v so3 + * @return Jr + */ +Eigen::Matrix3f RightJacobianSO3(const Eigen::Vector3f &v) +{ + return RightJacobianSO3(v(0), v(1), v(2)); +} + +/** + * @brief 计算右雅可比的逆 + * @param xyz so3 + * @return Jr^-1 + */ +Eigen::Matrix3f InverseRightJacobianSO3(const float &x, const float &y, const float &z) +{ + Eigen::Matrix3f I; + I.setIdentity(); + const float d2 = x * x + y * y + z * z; + const float d = sqrt(d2); + Eigen::Vector3f v; + v << x, y, z; + Eigen::Matrix3f W = Sophus::SO3f::hat(v); + + if (d < eps) + { + return I; + } + else + { + return I + W / 2 + W * W * (1.0f / d2 - (1.0f + cos(d)) / (2.0f * d * sin(d))); + } +} + +/** + * @brief 计算右雅可比的逆 + * @param v so3 + * @return Jr^-1 + */ +Eigen::Matrix3f InverseRightJacobianSO3(const Eigen::Vector3f &v) +{ + return InverseRightJacobianSO3(v(0), v(1), v(2)); +} + +/** + * @brief 计算旋转角度积分量 + * + * @param[in] angVel 陀螺仪数据 + * @param[in] imuBias 陀螺仪偏置 + * @param[in] time 两帧间的时间差 + */ +IntegratedRotation::IntegratedRotation(const Eigen::Vector3f &angVel, const Bias &imuBias, const float &time) +{ + // 得到考虑偏置后的角度旋转 + const float x = (angVel(0) - imuBias.bwx) * time; + const float y = (angVel(1) - imuBias.bwy) * time; + const float z = (angVel(2) - imuBias.bwz) * time; + + // 计算旋转矩阵的模值，后面用罗德里格公式计算旋转矩阵时会用到 + const float d2 = x * x + y * y + z * z; + const float d = sqrt(d2); + + Eigen::Vector3f v; + v << x, y, z; + + // 角度转成叉积的矩阵形式 + Eigen::Matrix3f W = Sophus::SO3f::hat(v); + // eps = 1e-4 是一个小量，根据罗德里格斯公式求极限，后面的高阶小量忽略掉得到此式 + if (d < eps) + { + deltaR = Eigen::Matrix3f::Identity() + W; + rightJ = Eigen::Matrix3f::Identity(); + } + else + { + deltaR = Eigen::Matrix3f::Identity() + W * sin(d) / d + W * W * (1.0f - cos(d)) / d2; + rightJ = Eigen::Matrix3f::Identity() - W * (1.0f - cos(d)) / d2 + W * W * (d - sin(d)) / (d2 * d); + } +} + +/** + * @brief 预积分类构造函数，根据输入的偏置初始化预积分参数 + * @param b_ 偏置 + * @param calib imu标定参数的类 + */ +Preintegrated::Preintegrated(const Bias &b_, const Calib &calib) +{ + Nga = calib.Cov; + NgaWalk = calib.CovWalk; + Initialize(b_); +} + +// Copy constructor +Preintegrated::Preintegrated(Preintegrated *pImuPre) + : dT(pImuPre->dT), C(pImuPre->C), Info(pImuPre->Info), + Nga(pImuPre->Nga), NgaWalk(pImuPre->NgaWalk), b(pImuPre->b), dR(pImuPre->dR), dV(pImuPre->dV), + dP(pImuPre->dP), JRg(pImuPre->JRg), JVg(pImuPre->JVg), JVa(pImuPre->JVa), JPg(pImuPre->JPg), JPa(pImuPre->JPa), + avgA(pImuPre->avgA), avgW(pImuPre->avgW), bu(pImuPre->bu), db(pImuPre->db), mvMeasurements(pImuPre->mvMeasurements) +{ +} + +/** + * @brief 复制上一帧的预积分 + * @param pImuPre 上一帧的预积分 + */ +void Preintegrated::CopyFrom(Preintegrated *pImuPre) +{ + dT = pImuPre->dT; + C = pImuPre->C; + Info = pImuPre->Info; + Nga = pImuPre->Nga; + NgaWalk = pImuPre->NgaWalk; + b.CopyFrom(pImuPre->b); + dR = pImuPre->dR; + dV = pImuPre->dV; + dP = pImuPre->dP; + // 旋转关于陀螺仪偏置变化的雅克比，以此类推 + JRg = pImuPre->JRg; + JVg = pImuPre->JVg; + JVa = pImuPre->JVa; + JPg = pImuPre->JPg; + JPa = pImuPre->JPa; + avgA = pImuPre->avgA; + avgW = pImuPre->avgW; + bu.CopyFrom(pImuPre->bu); + db = pImuPre->db; + mvMeasurements = pImuPre->mvMeasurements; +} + +/** + * @brief 初始化预积分 + * @param b_ 偏置 + */ +void Preintegrated::Initialize(const Bias &b_) +{ + dR.setIdentity(); + dV.setZero(); + dP.setZero(); + JRg.setZero(); + JVg.setZero(); + JVa.setZero(); + JPg.setZero(); + JPa.setZero(); + C.setZero(); + Info.setZero(); + db.setZero(); + b = b_; + bu = b_; // 更新后的偏置 + avgA.setZero(); // 平均加速度 + avgW.setZero(); // 平均角速度 + dT = 0.0f; + mvMeasurements.clear(); // 存放imu数据及dt +} + +/** + * @brief 根据新的偏置重新积分mvMeasurements里的数据 Optimizer::InertialOptimization调用 + */ +void Preintegrated::Reintegrate() +{ + std::unique_lock lock(mMutex); + const std::vector aux = mvMeasurements; + Initialize(bu); + for (size_t i = 0; i < aux.size(); i++) + IntegrateNewMeasurement(aux[i].a, aux[i].w, aux[i].t); +} + +/** + * @brief 预积分计算，更新noise + * + * @param[in] acceleration 加速度计数据 + * @param[in] angVel 陀螺仪数据 + * @param[in] dt 两图像 帧之间时间差 + */ +void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt) +{ + // 保存imu数据，利用中值积分的结果构造一个预积分类保存在mvMeasurements中 + mvMeasurements.push_back(integrable(acceleration, angVel, dt)); + + // Position is updated firstly, as it depends on previously computed velocity and rotation. + // Velocity is updated secondly, as it depends on previously computed rotation. + // Rotation is the last to be updated. + + // Matrices to compute covariance + // Step 1.构造协方差矩阵 + // 噪声矩阵的传递矩阵，这部分用于计算i到j-1历史噪声或者协方差 + Eigen::Matrix A; + A.setIdentity(); + // 噪声矩阵的传递矩阵，这部分用于计算j-1新的噪声或协方差，这两个矩阵里面的数都是当前时刻的，计算主要是为了下一时刻使用 + Eigen::Matrix B; + B.setZero(); + + // 考虑偏置后的加速度、角速度 + Eigen::Vector3f acc, accW; + acc << acceleration(0) - b.bax, acceleration(1) - b.bay, acceleration(2) - b.baz; + accW << angVel(0) - b.bwx, angVel(1) - b.bwy, angVel(2) - b.bwz; + + // 记录平均加速度和角速度 + avgA = (dT * avgA + dR * acc * dt) / (dT + dt); + avgW = (dT * avgW + accW * dt) / (dT + dt); + + // Update delta position dP and velocity dV (rely on no-updated delta rotation) + // 根据没有更新的dR来更新dP与dV eq.(38) + dP = dP + dV * dt + 0.5f * dR * acc * dt * dt; + dV = dV + dR * acc * dt; + + // Compute velocity and position parts of matrices A and B (rely on non-updated delta rotation) + // 根据η_ij = A * η_i,j-1 + B_j-1 * η_j-1中的Ａ矩阵和Ｂ矩阵对速度和位移进行更新 + Eigen::Matrix Wacc = Sophus::SO3f::hat(acc); + + A.block<3, 3>(3, 0) = -dR * dt * Wacc; + A.block<3, 3>(6, 0) = -0.5f * dR * dt * dt * Wacc; + A.block<3, 3>(6, 3) = Eigen::DiagonalMatrix(dt, dt, dt); + B.block<3, 3>(3, 3) = dR * dt; + B.block<3, 3>(6, 3) = 0.5f * dR * dt * dt; + + // Update position and velocity jacobians wrt bias correction + // 因为随着时间推移，不可能每次都重新计算雅克比矩阵，所以需要做J(k+1) = j(k) + (~)这类事，分解方式与AB矩阵相同 + // 论文作者对forster论文公式的基础上做了变形，然后递归更新，参见 https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/212 + JPa = JPa + JVa * dt - 0.5f * dR * dt * dt; + JPg = JPg + JVg * dt - 0.5f * dR * dt * dt * Wacc * JRg; + JVa = JVa - dR * dt; + JVg = JVg - dR * dt * Wacc * JRg; + + // Update delta rotation + // Step 2. 构造函数，会根据更新后的bias进行角度积分 + IntegratedRotation dRi(angVel, b, dt); + // 强行归一化使其符合旋转矩阵的格式 + dR = NormalizeRotation(dR * dRi.deltaR); + + // Compute rotation parts of matrices A and B + // 补充AB矩阵 + A.block<3, 3>(0, 0) = dRi.deltaR.transpose(); + B.block<3, 3>(0, 0) = dRi.rightJ * dt; + + // 小量delta初始为0，更新后通常也为0，故省略了小量的更新 + // Update covariance + // Step 3.更新协方差，frost经典预积分论文的第63个公式，推导了噪声（ηa, ηg）对dR dV dP 的影响 + C.block<9, 9>(0, 0) = A * C.block<9, 9>(0, 0) * A.transpose() + B * Nga * B.transpose(); // B矩阵为9*6矩阵 Nga 6*6对角矩阵，3个陀螺仪噪声的平方，3个加速度计噪声的平方 + // 这一部分最开始是0矩阵，随着积分次数增加，每次都加上随机游走，偏置的信息矩阵 + C.block<6, 6>(9, 9) += NgaWalk; + + // Update rotation jacobian wrt bias correction + // 计算偏置的雅克比矩阵，r对bg的导数，∂ΔRij/∂bg = (ΔRjj-1) * ∂ΔRij-1/∂bg - Jr(j-1)*t + // 论文作者对forster论文公式的基础上做了变形，然后递归更新，参见 https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/212 + // ? 为什么先更新JPa、JPg、JVa、JVg最后更新JRg? 答：这里必须先更新dRi才能更新到这个值，但是为什么JPg和JVg依赖的上一个JRg值进行更新的？ + JRg = dRi.deltaR.transpose() * JRg - dRi.rightJ * dt; + + // Total integrated time + // 更新总时间 + dT += dt; +} + +/** + * @brief 融合两个预积分，发生在删除关键帧的时候，3帧变2帧，需要把两段预积分融合 + * @param pPrev 前面的预积分 + */ +void Preintegrated::MergePrevious(Preintegrated *pPrev) +{ + if (pPrev == this) + return; + + std::unique_lock lock1(mMutex); + std::unique_lock lock2(pPrev->mMutex); + Bias bav; + bav.bwx = bu.bwx; + bav.bwy = bu.bwy; + bav.bwz = bu.bwz; + bav.bax = bu.bax; + bav.bay = bu.bay; + bav.baz = bu.baz; + + const std::vector aux1 = pPrev->mvMeasurements; + const std::vector aux2 = mvMeasurements; + + Initialize(bav); + for (size_t i = 0; i < aux1.size(); i++) + IntegrateNewMeasurement(aux1[i].a, aux1[i].w, aux1[i].t); + for (size_t i = 0; i < aux2.size(); i++) + IntegrateNewMeasurement(aux2[i].a, aux2[i].w, aux2[i].t); +} + +/** + * @brief 更新偏置 + * @param bu_ 偏置 + */ +void Preintegrated::SetNewBias(const Bias &bu_) +{ + std::unique_lock lock(mMutex); + bu = bu_; + + db(0) = bu_.bwx - b.bwx; + db(1) = bu_.bwy - b.bwy; + db(2) = bu_.bwz - b.bwz; + db(3) = bu_.bax - b.bax; + db(4) = bu_.bay - b.bay; + db(5) = bu_.baz - b.baz; +} + +/** + * @brief 获得当前偏置与输入偏置的改变量 + * @param b_ 偏置 + * @return 改变量 + */ +IMU::Bias Preintegrated::GetDeltaBias(const Bias &b_) +{ + std::unique_lock lock(mMutex); + return IMU::Bias(b_.bax - b.bax, b_.bay - b.bay, b_.baz - b.baz, b_.bwx - b.bwx, b_.bwy - b.bwy, b_.bwz - b.bwz); +} + +/** + * @brief 根据新的偏置计算新的dR + * @param b_ 新的偏置 + * @return dR + */ +Eigen::Matrix3f Preintegrated::GetDeltaRotation(const Bias &b_) +{ + std::unique_lock lock(mMutex); + // 计算偏置的变化量 + Eigen::Vector3f dbg; + dbg << b_.bwx - b.bwx, b_.bwy - b.bwy, b_.bwz - b.bwz; + // 考虑偏置后，dR对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13～P14 + // Forster论文公式（44）yP17也有结果（但没有推导），后面两个函数GetDeltaPosition和GetDeltaPosition也是基于此推导的 + return NormalizeRotation(dR * Sophus::SO3f::exp(JRg * dbg).matrix()); +} + +/** + * @brief 根据新的偏置计算新的dV + * @param b_ 新的偏置 + * @return dV + */ +Eigen::Vector3f Preintegrated::GetDeltaVelocity(const Bias &b_) +{ + std::unique_lock lock(mMutex); + Eigen::Vector3f dbg, dba; + dbg << b_.bwx - b.bwx, b_.bwy - b.bwy, b_.bwz - b.bwz; + dba << b_.bax - b.bax, b_.bay - b.bay, b_.baz - b.baz; + // 考虑偏置后，dV对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13，JPg和JPa在预积分处理中更新 + return dV + JVg * dbg + JVa * dba; +} + +/** + * @brief 根据新的偏置计算新的dP + * @param b_ 新的偏置 + * @return dP + */ +Eigen::Vector3f Preintegrated::GetDeltaPosition(const Bias &b_) +{ + std::unique_lock lock(mMutex); + Eigen::Vector3f dbg, dba; + dbg << b_.bwx - b.bwx, b_.bwy - b.bwy, b_.bwz - b.bwz; + dba << b_.bax - b.bax, b_.bay - b.bay, b_.baz - b.baz; + // 考虑偏置后，dP对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13，JPg和JPa在预积分处理中更新 + return dP + JPg * dbg + JPa * dba; +} + +/** + * @brief 返回经过db(δba, δbg)更新后的dR,与上面是一个意思 + * @return dR + */ +Eigen::Matrix3f Preintegrated::GetUpdatedDeltaRotation() +{ + std::unique_lock lock(mMutex); + return NormalizeRotation(dR * Sophus::SO3f::exp(JRg * db.head(3)).matrix()); +} + +/** + * @brief 返回经过db(δba, δbg)更新后的dV,与上面是一个意思 + * @return dV + */ +Eigen::Vector3f Preintegrated::GetUpdatedDeltaVelocity() +{ + std::unique_lock lock(mMutex); + return dV + JVg * db.head(3) + JVa * db.tail(3); +} + +/** + * @brief 返回经过db(δba, δbg)更新后的dP,与上面是一个意思 + * @return dP + */ +Eigen::Vector3f Preintegrated::GetUpdatedDeltaPosition() +{ + std::unique_lock lock(mMutex); + return dP + JPg * db.head(3) + JPa * db.tail(3); +} + +/** + * @brief 获取dR + * @return dR + */ +Eigen::Matrix3f Preintegrated::GetOriginalDeltaRotation() +{ + std::unique_lock lock(mMutex); + return dR; +} + +/** + * @brief 获取dV + * @return dV + */ +Eigen::Vector3f Preintegrated::GetOriginalDeltaVelocity() +{ + std::unique_lock lock(mMutex); + return dV; +} + +/** + * @brief 获取dP + * @return dP + */ +Eigen::Vector3f Preintegrated::GetOriginalDeltaPosition() +{ + std::unique_lock lock(mMutex); + return dP; +} + +/** + * @brief 获取b,更新前的偏置 + * @return b + */ +Bias Preintegrated::GetOriginalBias() +{ + std::unique_lock lock(mMutex); + return b; +} + +/** + * @brief 获取bu,更新后的偏置 + * @return bu + */ +Bias Preintegrated::GetUpdatedBias() +{ + std::unique_lock lock(mMutex); + return bu; +} + +/** + * @brief 获取db,更新前后的偏置差 + * @return db + */ +Eigen::Matrix Preintegrated::GetDeltaBias() +{ + std::unique_lock lock(mMutex); + return db; +} + +/** + * @brief 赋值新的偏置 + * @param b 偏置 + */ +void Bias::CopyFrom(Bias &b) +{ + bax = b.bax; + bay = b.bay; + baz = b.baz; + bwx = b.bwx; + bwy = b.bwy; + bwz = b.bwz; +} + +std::ostream &operator<<(std::ostream &out, const Bias &b) +{ + if (b.bwx > 0) + out << " "; + out << b.bwx << ","; + if (b.bwy > 0) + out << " "; + out << b.bwy << ","; + if (b.bwz > 0) + out << " "; + out << b.bwz << ","; + if (b.bax > 0) + out << " "; + out << b.bax << ","; + if (b.bay > 0) + out << " "; + out << b.bay << ","; + if (b.baz > 0) + out << " "; + out << b.baz; + + return out; +} + +/** + * @brief 设置参数 + * @param Tbc_ 位姿变换 + * @param ng 噪声 + * @param na 噪声 + * @param ngw 随机游走 + * @param naw 随机游走 + */ +void Calib::Set(const Sophus::SE3 &sophTbc, const float &ng, const float &na, const float &ngw, const float &naw) +{ + mbIsSet = true; + const float ng2 = ng * ng; + const float na2 = na * na; + const float ngw2 = ngw * ngw; + const float naw2 = naw * naw; + + // Sophus/Eigen + mTbc = sophTbc; + mTcb = mTbc.inverse(); + // 噪声协方差 + Cov.diagonal() << ng2, ng2, ng2, na2, na2, na2; + // 随机游走协方差 + CovWalk.diagonal() << ngw2, ngw2, ngw2, naw2, naw2, naw2; +} + +/** + * @brief imu标定参数的构造函数 + * @param calib imu标定参数 + */ +Calib::Calib(const Calib &calib) +{ + mbIsSet = calib.mbIsSet; + // Sophus/Eigen parameters + mTbc = calib.mTbc; + mTcb = calib.mTcb; + Cov = calib.Cov; + CovWalk = calib.CovWalk; +} + +} // namespace IMU + +} // namespace ORB_SLAM2 diff --git a/ORB-SLAM3-UESTC/Workspace/src/Initializer.cc b/ORB-SLAM3-UESTC/Workspace/src/Initializer.cc new file mode 100644 index 0000000..37c3fd9 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/Initializer.cc @@ -0,0 +1,1713 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#include "Initializer.h" + +#include "Thirdparty/DBoW2/DUtils/Random.h" + +#include "Optimizer.h" +#include "ORBmatcher.h" + +#include +#include + +namespace ORB_SLAM3 +{ + +/** + * @brief 根据参考帧构造初始化器 + * + * @param[in] ReferenceFrame 参考帧 + * @param[in] sigma 测量误差 + * @param[in] iterations RANSAC迭代次数 + */ +Initializer::Initializer(const Frame &ReferenceFrame, float sigma, int iterations) +{ + mpCamera = ReferenceFrame.mpCamera; + //从参考帧中获取相机的内参数矩阵 + mK = ReferenceFrame.mK.clone(); + + // 从参考帧中获取去畸变后的特征点 + mvKeys1 = ReferenceFrame.mvKeysUn; + + mSigma = sigma; + mSigma2 = sigma*sigma; + //最大迭代次数 + mMaxIterations = iterations; +} + +/** + * @brief 计算基础矩阵和单应性矩阵，选取最佳的来恢复出最开始两帧之间的相对姿态，并进行三角化得到初始地图点 + * Step 1 重新记录特征点对的匹配关系 + * Step 2 在所有匹配特征点对中随机选择8对匹配特征点为一组，用于估计H矩阵和F矩阵 + * Step 3 计算fundamental 矩阵 和homography 矩阵，为了加速分别开了线程计算 + * Step 4 计算得分比例来判断选取哪个模型来求位姿R,t + * + * @param[in] CurrentFrame 当前帧，也就是SLAM意义上的第二帧 + * @param[in] vMatches12 当前帧（2）和参考帧（1）图像中特征点的匹配关系 + * vMatches12[i]解释：i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值 + * 没有匹配关系的话，vMatches12[i]值为 -1 + * @param[in & out] R21 相机从参考帧到当前帧的旋转 + * @param[in & out] t21 相机从参考帧到当前帧的平移 + * @param[in & out] vP3D 三角化测量之后的三维地图点 + * @param[in & out] vbTriangulated 标记三角化点是否有效，有效为true + * @return true 该帧可以成功初始化，返回true + * @return false 该帧不满足初始化条件，返回false + */ +bool Initializer::Initialize(const Frame &CurrentFrame, const vector &vMatches12, cv::Mat &R21, cv::Mat &t21, + vector &vP3D, vector &vbTriangulated) +{ + // Reference Frame: 1, Current Frame: 2 + + //获取当前帧的去畸变之后的特征点 + mvKeys2 = CurrentFrame.mvKeysUn; + + // mvMatches12记录匹配上的特征点对，记录的是帧2在帧1的匹配索引 + mvMatches12.clear(); + // 预分配空间，大小和关键点数目一致mvKeys2.size() + mvMatches12.reserve(mvKeys2.size()); + // 记录参考帧1中的每个特征点是否有匹配的特征点 + // 这个成员变量后面没有用到，后面只关心匹配上的特征点 + mvbMatched1.resize(mvKeys1.size()); + // Step 1 重新记录特征点对的匹配关系存储在mvMatches12，是否有匹配存储在mvbMatched1 + // 将vMatches12（有冗余） 转化为 mvMatches12（只记录了匹配关系） + for(size_t i=0, iend=vMatches12.size();i=0) + { + //mvMatches12 中只记录有匹配关系的特征点对的索引值 + //i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值 + mvMatches12.push_back(make_pair(i,vMatches12[i])); + //标记参考帧1中的这个特征点有匹配关系 + mvbMatched1[i]=true; + } + else + //标记参考帧1中的这个特征点没有匹配关系 + mvbMatched1[i]=false; + } + + // 有匹配的特征点的对数 + const int N = mvMatches12.size(); + // Indices for minimum set selection + // 新建一个容器vAllIndices存储特征点索引，并预分配空间 + vector vAllIndices; + vAllIndices.reserve(N); + + //在RANSAC的某次迭代中，还可以被抽取来作为数据样本的特征点对的索引，所以这里起的名字叫做可用的索引 + vector vAvailableIndices; + //初始化所有特征点对的索引，索引值0到N-1 + for(int i=0; i >(mMaxIterations,vector(8,0)); + + //用于进行随机数据样本采样，设置随机数种子 + DUtils::Random::SeedRandOnce(0); + + //开始每一次的迭代 + for(int it=0; it vbMatchesInliersH, vbMatchesInliersF; + //计算出来的单应矩阵和基础矩阵的RANSAC评分，这里其实是采用重投影误差来计算的 + float SH, SF; + //这两个是经过RANSAC算法后计算出来的单应矩阵和基础矩阵 + cv::Mat H, F; + + // 构造线程来计算H矩阵及其得分 + // thread方法比较特殊，在传递引用的时候，外层需要用ref来进行引用传递，否则就是浅拷贝 + thread threadH(&Initializer::FindHomography, //该线程的主函数 + this, //由于主函数为类的成员函数，所以第一个参数就应该是当前对象的this指针 + ref(vbMatchesInliersH), //输出，特征点对的Inlier标记 + ref(SH), //输出，计算的单应矩阵的RANSAC评分 + ref(H)); //输出，计算的单应矩阵结果 + // 计算fundamental matrix并打分，参数定义和H是一样的，这里不再赘述 + thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F)); + + //cout << "5" << endl; + + // Wait until both threads have finished + //等待两个计算线程结束 + threadH.join(); + threadF.join(); + + // Compute ratio of scores + // Step 4 计算得分比例来判断选取哪个模型来求位姿R,t + //通过这个规则来判断谁的评分占比更多一些，注意不是简单的比较绝对评分大小，而是看评分的占比 + float RH = SH/(SH+SF); + + //cout << "6" << endl; + + float minParallax = 1.0; // 1.0 originally + + cv::Mat K = static_cast(mpCamera)->toK(); + // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45) + // 注意这里更倾向于用H矩阵恢复位姿。如果单应矩阵的评分占比达到了0.4以上,则从单应矩阵恢复运动,否则从基础矩阵恢复运动 + if(RH>0.40) // if(RH>0.40) + { + //更偏向于平面，此时从单应矩阵恢复，函数ReconstructH返回bool型结果 + return ReconstructH(vbMatchesInliersH, //输入，匹配成功的特征点对Inliers标记 + H, //输入，前面RANSAC计算后的单应矩阵 + K, //输入，相机的内参数矩阵 + R21,t21, //输出，计算出来的相机从参考帧1到当前帧2所发生的旋转和位移变换 + vP3D, //特征点对经过三角测量之后的空间坐标，也就是地图点 + vbTriangulated, //特征点对是否成功三角化的标记 + minParallax, //这个对应的形参为minParallax，即认为某对特征点的三角化测量中，认为其测量有效时 + //需要满足的最小视差角（如果视差角过小则会引起非常大的观测误差）,单位是角度 + 50); //为了进行运动恢复，所需要的最少的三角化测量成功的点个数 + } + else //if(pF_HF>0.6) + { + // 更偏向于非平面，从基础矩阵恢复 + //cout << "Initialization from Fundamental" << endl; + return ReconstructF(vbMatchesInliersF,F,K,R21,t21,vP3D,vbTriangulated,minParallax,50); + } + //一般地程序不应该执行到这里，如果执行到这里说明程序跑飞了 + return false; +} + +/** + * @brief 计算单应矩阵，假设场景为平面情况下通过前两帧求取Homography矩阵，并得到该模型的评分 + * 原理参考Multiple view geometry in computer vision P109 算法4.4 + * Step 1 将当前帧和参考帧中的特征点坐标进行归一化 + * Step 2 选择8个归一化之后的点对进行迭代 + * Step 3 八点法计算单应矩阵矩阵 + * Step 4 利用重投影误差为当次RANSAC的结果评分 + * Step 5 更新具有最优评分的单应矩阵计算结果,并且保存所对应的特征点对的内点标记 + * + * @param[in & out] vbMatchesInliers 标记是否是外点 + * @param[in & out] score 计算单应矩阵的得分 + * @param[in & out] H21 单应矩阵结果 + */ +void Initializer::FindHomography(vector &vbMatchesInliers, float &score, cv::Mat &H21) +{ + // Number of putative matches + //匹配的特征点对总数 + const int N = mvMatches12.size(); + + // Normalize coordinates + // Step 1 将当前帧和参考帧中的特征点坐标进行归一化，主要是平移和尺度变换 + // 具体来说,就是将mvKeys1和mvKey2归一化到均值为0，一阶绝对矩为1，归一化矩阵分别为T1、T2 + // 这里所谓的一阶绝对矩其实就是随机变量到取值的中心的绝对值的平均值 + // 归一化矩阵就是把上述归一化的操作用矩阵来表示。这样特征点坐标乘归一化矩阵可以得到归一化后的坐标 + + + //归一化后的参考帧1和当前帧2中的特征点坐标 + vector vPn1, vPn2; + // 记录各自的归一化矩阵 + cv::Mat T1, T2; + Normalize(mvKeys1,vPn1, T1); + Normalize(mvKeys2,vPn2, T2); + //这里求的逆在后面的代码中要用到，辅助进行原始尺度的恢复 + cv::Mat T2inv = T2.inv(); + + // Best Results variables + // 记录最佳评分 + score = 0.0; + // 取得历史最佳评分时,特征点对的inliers标记 + vbMatchesInliers = vector(N,false); + + // Iteration variables + //某次迭代中，参考帧的特征点坐标 + vector vPn1i(8); + //某次迭代中，当前帧的特征点坐标 + vector vPn2i(8); + //以及计算出来的单应矩阵、及其逆矩阵 + cv::Mat H21i, H12i; + + // 每次RANSAC记录Inliers与得分 + vector vbCurrentInliers(N,false); + float currentScore; + + // Perform all RANSAC iterations and save the solution with highest score + //下面进行每次的RANSAC迭代 + for(int it=0; itscore) + { + //如果当前的结果得分更高，那么就更新最优计算结果 + H21 = H21i.clone(); + //保存匹配好的特征点对的Inliers标记 + vbMatchesInliers = vbCurrentInliers; + //更新历史最优评分 + score = currentScore; + } + } +} + +/** + * @brief 计算基础矩阵，假设场景为非平面情况下通过前两帧求取Fundamental矩阵，得到该模型的评分 + * Step 1 将当前帧和参考帧中的特征点坐标进行归一化 + * Step 2 选择8个归一化之后的点对进行迭代 + * Step 3 八点法计算基础矩阵矩阵 + * Step 4 利用重投影误差为当次RANSAC的结果评分 + * Step 5 更新具有最优评分的基础矩阵计算结果,并且保存所对应的特征点对的内点标记 + * + * @param[in & out] vbMatchesInliers 标记是否是外点 + * @param[in & out] score 计算基础矩阵得分 + * @param[in & out] F21 从特征点1到2的基础矩阵 + */ +void Initializer::FindFundamental(vector &vbMatchesInliers, float &score, cv::Mat &F21) +{ + // 计算基础矩阵,其过程和上面的计算单应矩阵的过程十分相似. + + // Number of putative matches + // 匹配的特征点对总数 + const int N = vbMatchesInliers.size(); + + // Normalize coordinates + // Step 1 将当前帧和参考帧中的特征点坐标进行归一化，主要是平移和尺度变换 + // 具体来说,就是将mvKeys1和mvKey2归一化到均值为0，一阶绝对矩为1，归一化矩阵分别为T1、T2 + // 这里所谓的一阶绝对矩其实就是随机变量到取值的中心的绝对值的平均值 + // 归一化矩阵就是把上述归一化的操作用矩阵来表示。这样特征点坐标乘归一化矩阵可以得到归一化后的坐标 + + vector vPn1, vPn2; + cv::Mat T1, T2; + Normalize(mvKeys1,vPn1, T1); + Normalize(mvKeys2,vPn2, T2); + // ! 注意这里取的是归一化矩阵T2的转置,因为基础矩阵的定义和单应矩阵不同，两者去归一化的计算也不相同 + cv::Mat T2t = T2.t(); + + // Best Results variables + //最优结果 + score = 0.0; + vbMatchesInliers = vector(N,false); + + // Iteration variables + // 某次迭代中，参考帧的特征点坐标 + vector vPn1i(8); + // 某次迭代中，当前帧的特征点坐标 + vector vPn2i(8); + // 某次迭代中，计算的基础矩阵 + cv::Mat F21i; + // 每次RANSAC记录的Inliers与得分 + vector vbCurrentInliers(N,false); + float currentScore; + + // Perform all RANSAC iterations and save the solution with highest score + // 下面进行每次的RANSAC迭代 + for(int it=0; itscore) + { + //如果当前的结果得分更高，那么就更新最优计算结果 + F21 = F21i.clone(); + vbMatchesInliers = vbCurrentInliers; + score = currentScore; + } + } +} + + +/** + * @brief 用DLT方法求解单应矩阵H + * 这里最少用4对点就能够求出来，不过这里为了统一还是使用了8对点求最小二乘解 + * + * @param[in] vP1 参考帧中归一化后的特征点 + * @param[in] vP2 当前帧中归一化后的特征点 + * @return cv::Mat 计算的单应矩阵H + */ +cv::Mat Initializer::ComputeH21( + const vector &vP1, //归一化后的点, in reference frame + const vector &vP2) //归一化后的点, in current frame +{ + // 基本原理：见附件推导过程： + // |x'| | h1 h2 h3 ||x| + // |y'| = a | h4 h5 h6 ||y| 简写: x' = a H x, a为一个尺度因子 + // |1 | | h7 h8 h9 ||1| + // 使用DLT(direct linear tranform)求解该模型 + // x' = a H x + // ---> (x') 叉乘 (H x) = 0 (因为方向相同) (取前两行就可以推导出下面的了) + // ---> Ah = 0 + // A = | 0 0 0 -x -y -1 xy' yy' y'| h = | h1 h2 h3 h4 h5 h6 h7 h8 h9 | + // |-x -y -1 0 0 0 xx' yx' x'| + // 通过SVD求解Ah = 0，A^T*A最小特征值对应的特征向量即为解 + // 其实也就是右奇异值矩阵的最后一列 + + //获取参与计算的特征点的数目 + const int N = vP1.size(); + + // 构造用于计算的矩阵 A + cv::Mat A(2*N, //行，注意每一个点的数据对应两行 + 9, //列 + CV_32F); //float数据类型 + + // 构造矩阵A，将每个特征点添加到矩阵A中的元素 + for(int i=0; i(2*i,0) = 0.0; + A.at(2*i,1) = 0.0; + A.at(2*i,2) = 0.0; + A.at(2*i,3) = -u1; + A.at(2*i,4) = -v1; + A.at(2*i,5) = -1; + A.at(2*i,6) = v2*u1; + A.at(2*i,7) = v2*v1; + A.at(2*i,8) = v2; + + //生成这个点的第二行 + A.at(2*i+1,0) = u1; + A.at(2*i+1,1) = v1; + A.at(2*i+1,2) = 1; + A.at(2*i+1,3) = 0.0; + A.at(2*i+1,4) = 0.0; + A.at(2*i+1,5) = 0.0; + A.at(2*i+1,6) = -u2*u1; + A.at(2*i+1,7) = -u2*v1; + A.at(2*i+1,8) = -u2; + + } + + // 定义输出变量，u是左边的正交矩阵U， w为奇异矩阵，vt中的t表示是右正交矩阵V的转置 + cv::Mat u,w,vt; + + //使用opencv提供的进行奇异值分解的函数 + cv::SVDecomp(A, //输入，待进行奇异值分解的矩阵 + w, //输出，奇异值矩阵 + u, //输出，矩阵U + vt, //输出，矩阵V^T + cv::SVD::MODIFY_A | //输入，MODIFY_A是指允许计算函数可以修改待分解的矩阵，官方文档上说这样可以加快计算速度、节省内存 + cv::SVD::FULL_UV); //FULL_UV=把U和VT补充成单位正交方阵 + + // 返回最小奇异值所对应的右奇异向量 + // 注意前面说的是右奇异值矩阵的最后一列，但是在这里因为是vt，转置后了，所以是行；由于A有9列数据，故最后一列的下标为8 + return vt.row(8).reshape(0, //转换后的通道数，这里设置为0表示是与前面相同 + 3); //转换后的行数,对应V的最后一列 +} + + +/** + * @brief 根据特征点匹配求fundamental matrix（normalized 8点法） + * 注意F矩阵有秩为2的约束，所以需要两次SVD分解 + * + * @param[in] vP1 参考帧中归一化后的特征点 + * @param[in] vP2 当前帧中归一化后的特征点 + * @return cv::Mat 最后计算得到的基础矩阵F + */ +cv::Mat Initializer::ComputeF21( + const vector &vP1, //归一化后的点, in reference frame + const vector &vP2) //归一化后的点, in current frame +{ + // 原理详见附件推导 + // x'Fx = 0 整理可得：Af = 0 + // A = | x'x x'y x' y'x y'y y' x y 1 |, f = | f1 f2 f3 f4 f5 f6 f7 f8 f9 | + // 通过SVD求解Af = 0，A'A最小特征值对应的特征向量即为解 + + //获取参与计算的特征点对数 + const int N = vP1.size(); + + //初始化A矩阵 + cv::Mat A(N,9,CV_32F); // N*9维 + + // 构造矩阵A，将每个特征点添加到矩阵A中的元素 + for(int i=0; i(i,0) = u2*u1; + A.at(i,1) = u2*v1; + A.at(i,2) = u2; + A.at(i,3) = v2*u1; + A.at(i,4) = v2*v1; + A.at(i,5) = v2; + A.at(i,6) = u1; + A.at(i,7) = v1; + A.at(i,8) = 1; + } + + //存储奇异值分解结果的变量 + cv::Mat u,w,vt; + + // 定义输出变量，u是左边的正交矩阵U， w为奇异矩阵，vt中的t表示是右正交矩阵V的转置 + cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV); + // 转换成基础矩阵的形式 + cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最后一列 + + //基础矩阵的秩为2,而我们不敢保证计算得到的这个结果的秩为2,所以需要通过第二次奇异值分解,来强制使其秩为2 + // 对初步得来的基础矩阵进行第2次奇异值分解 + cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV); + + // 秩2约束，强制将第3个奇异值设置为0 + w.at(2)=0; + + // 重新组合好满足秩约束的基础矩阵，作为最终计算结果返回 + return u*cv::Mat::diag(w)*vt; +} + +/** + * @brief 对给定的homography matrix打分,需要使用到卡方检验的知识 + * + * @param[in] H21 从参考帧到当前帧的单应矩阵 + * @param[in] H12 从当前帧到参考帧的单应矩阵 + * @param[in] vbMatchesInliers 匹配好的特征点对的Inliers标记 + * @param[in] sigma 方差，默认为1 + * @return float 返回得分 + */ +float Initializer::CheckHomography( + const cv::Mat &H21, //从参考帧到当前帧的单应矩阵 + const cv::Mat &H12, //从当前帧到参考帧的单应矩阵 + vector &vbMatchesInliers, //匹配好的特征点对的Inliers标记 + float sigma) //估计误差 +{ + // 说明：在已值n维观测数据误差服从N(0，sigma）的高斯分布时 + // 其误差加权最小二乘结果为 sum_error = SUM(e(i)^T * Q^(-1) * e(i)) + // 其中：e(i) = [e_x,e_y,...]^T, Q维观测数据协方差矩阵，即sigma * sigma组成的协方差矩阵 + // 误差加权最小二次结果越小，说明观测数据精度越高 + // 那么，score = SUM((th - e(i)^T * Q^(-1) * e(i)))的分数就越高 + // 算法目标： 检查单应变换矩阵 + // 检查方式：通过H矩阵，进行参考帧和当前帧之间的双向投影，并计算起加权最小二乘投影误差 + + // 算法流程 + // input: 单应性矩阵 H21, H12, 匹配点集 mvKeys1 + // do: + // for p1(i), p2(i) in mvKeys: + // error_i1 = ||p2(i) - H21 * p1(i)||2 + // error_i2 = ||p1(i) - H12 * p2(i)||2 + // + // w1 = 1 / sigma / sigma + // w2 = 1 / sigma / sigma + // + // if error1 < th + // score += th - error_i1 * w1 + // if error2 < th + // score += th - error_i2 * w2 + // + // if error_1i > th or error_2i > th + // p1(i), p2(i) are inner points + // vbMatchesInliers(i) = true + // else + // p1(i), p2(i) are outliers + // vbMatchesInliers(i) = false + // end + // end + // output: score, inliers + + // 特点匹配个数 + const int N = mvMatches12.size(); + + const float h11 = H21.at(0,0); + const float h12 = H21.at(0,1); + const float h13 = H21.at(0,2); + const float h21 = H21.at(1,0); + const float h22 = H21.at(1,1); + const float h23 = H21.at(1,2); + const float h31 = H21.at(2,0); + const float h32 = H21.at(2,1); + const float h33 = H21.at(2,2); + + // 获取从当前帧到参考帧的单应矩阵的各个元素 + const float h11inv = H12.at(0,0); + const float h12inv = H12.at(0,1); + const float h13inv = H12.at(0,2); + const float h21inv = H12.at(1,0); + const float h22inv = H12.at(1,1); + const float h23inv = H12.at(1,2); + const float h31inv = H12.at(2,0); + const float h32inv = H12.at(2,1); + const float h33inv = H12.at(2,2); + + // 给特征点对的Inliers标记预分配空间 + vbMatchesInliers.resize(N); + + // 初始化score值 + float score = 0; + + // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差） + // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值 + const float th = 5.991; + + //信息矩阵，方差平方的倒数 + const float invSigmaSquare = 1.0/(sigma*sigma); + + // Step 2 通过H矩阵，进行参考帧和当前帧之间的双向投影，并计算起加权最小二乘投影误差 + // H21 表示从img1 到 img2的变换矩阵 + // H12 表示从img2 到 img1的变换矩阵 + for(int i=0; ith) + bIn = false; + else + // 误差越大，得分越低 + score += th - chiSquare1; + + // 计算从img1 到 img2 的投影变换误差 + // x1in2 = H21*x1 + // 将图像2中的特征点单应到图像1中 + // |u2| |h11 h12 h13||u1| |u1in2| + // |v2| = |h21 h22 h23||v1| = |v1in2| * w1in2inv + // |1 | |h31 h32 h33||1 | | 1 | + // 计算投影归一化坐标 + const float w1in2inv = 1.0/(h31*u1+h32*v1+h33); + const float u1in2 = (h11*u1+h12*v1+h13)*w1in2inv; + const float v1in2 = (h21*u1+h22*v1+h23)*w1in2inv; + + // 计算重投影误差 + const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2); + const float chiSquare2 = squareDist2*invSigmaSquare; + + // 用阈值标记离群点，内点的话累加得分 + if(chiSquare2>th) + bIn = false; + else + score += th - chiSquare2; + + // Step 2.4 如果从img2 到 img1 和 从img1 到img2的重投影误差均满足要求，则说明是Inlier point + if(bIn) + vbMatchesInliers[i]=true; + else + vbMatchesInliers[i]=false; + } + + return score; +} + +/** + * @brief 对给定的Fundamental matrix打分 + * + * @param[in] F21 当前帧和参考帧之间的基础矩阵 + * @param[in] vbMatchesInliers 匹配的特征点对属于inliers的标记 + * @param[in] sigma 方差，默认为1 + * @return float 返回得分 + */ +float Initializer::CheckFundamental( + const cv::Mat &F21, //当前帧和参考帧之间的基础矩阵 + vector &vbMatchesInliers, //匹配的特征点对属于inliers的标记 + float sigma) //方差 +{ + + // 说明：在已值n维观测数据误差服从N(0，sigma）的高斯分布时 + // 其误差加权最小二乘结果为 sum_error = SUM(e(i)^T * Q^(-1) * e(i)) + // 其中：e(i) = [e_x,e_y,...]^T, Q维观测数据协方差矩阵，即sigma * sigma组成的协方差矩阵 + // 误差加权最小二次结果越小，说明观测数据精度越高 + // 那么，score = SUM((th - e(i)^T * Q^(-1) * e(i)))的分数就越高 + // 算法目标：检查基础矩阵 + // 检查方式：利用对极几何原理 p2^T * F * p1 = 0 + // 假设：三维空间中的点 P 在 img1 和 img2 两图像上的投影分别为 p1 和 p2（两个为同名点） + // 则：p2 一定存在于极线 l2 上，即 p2*l2 = 0. 而l2 = F*p1 = (a, b, c)^T + // 所以，这里的误差项 e 为 p2 到 极线 l2 的距离，如果在直线上，则 e = 0 + // 根据点到直线的距离公式：d = (ax + by + c) / sqrt(a * a + b * b) + // 所以，e = (a * p2.x + b * p2.y + c) / sqrt(a * a + b * b) + + // 算法流程 + // input: 基础矩阵 F 左右视图匹配点集 mvKeys1 + // do: + // for p1(i), p2(i) in mvKeys: + // l2 = F * p1(i) + // l1 = p2(i) * F + // error_i1 = dist_point_to_line(x2,l2) + // error_i2 = dist_point_to_line(x1,l1) + // + // w1 = 1 / sigma / sigma + // w2 = 1 / sigma / sigma + // + // if error1 < th + // score += thScore - error_i1 * w1 + // if error2 < th + // score += thScore - error_i2 * w2 + // + // if error_1i > th or error_2i > th + // p1(i), p2(i) are inner points + // vbMatchesInliers(i) = true + // else + // p1(i), p2(i) are outliers + // vbMatchesInliers(i) = false + // end + // end + // output: score, inliers + + // 获取匹配的特征点对的总对数 + const int N = mvMatches12.size(); + + // Step 1 提取基础矩阵中的元素数据 + const float f11 = F21.at(0,0); + const float f12 = F21.at(0,1); + const float f13 = F21.at(0,2); + const float f21 = F21.at(1,0); + const float f22 = F21.at(1,1); + const float f23 = F21.at(1,2); + const float f31 = F21.at(2,0); + const float f32 = F21.at(2,1); + const float f33 = F21.at(2,2); + + // 预分配空间 + vbMatchesInliers.resize(N); + + // 设置评分初始值（因为后面需要进行这个数值的累计） + float score = 0; + + // 基于卡方检验计算出的阈值 + // 自由度为1的卡方分布，显著性水平为0.05，对应的临界阈值 + // ?是因为点到直线距离是一个自由度吗？ + const float th = 3.841; + + // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值 + const float thScore = 5.991; + + // 信息矩阵，或 协方差矩阵的逆矩阵 + const float invSigmaSquare = 1.0/(sigma*sigma); + + + // Step 2 计算img1 和 img2 在估计 F 时的score值 + for(int i=0; ith) + bIn = false; + else + // 误差越大，得分越低 + score += thScore - chiSquare1; + + // 计算img2上的点在 img1 上投影得到的极线 l1= p2 * F21 = (a1,b1,c1) + const float a1 = f11*u2+f21*v2+f31; + const float b1 = f12*u2+f22*v2+f32; + const float c1 = f13*u2+f23*v2+f33; + + // 计算误差 e = (a * p2.x + b * p2.y + c) / sqrt(a * a + b * b) + const float num1 = a1*u1+b1*v1+c1; + + const float squareDist2 = num1*num1/(a1*a1+b1*b1); + + // 带权重误差 + const float chiSquare2 = squareDist2*invSigmaSquare; + + // 误差大于阈值就说明这个点是Outlier + if(chiSquare2>th) + bIn = false; + else + score += thScore - chiSquare2; + + // Step 2.5 保存结果 + if(bIn) + vbMatchesInliers[i]=true; + else + vbMatchesInliers[i]=false; + } + // 返回评分 + return score; +} + +/** + * @brief 从基础矩阵F中求解位姿R，t及三维点 + * F分解出E，E有四组解，选择计算的有效三维点（在摄像头前方、投影误差小于阈值、视差角大于阈值）最多的作为最优的解 + * @param[in] vbMatchesInliers 匹配好的特征点对的Inliers标记 + * @param[in] F21 从参考帧到当前帧的基础矩阵 + * @param[in] K 相机的内参数矩阵 + * @param[in & out] R21 计算好的相机从参考帧到当前帧的旋转 + * @param[in & out] t21 计算好的相机从参考帧到当前帧的平移 + * @param[in & out] vP3D 三角化测量之后的特征点的空间坐标 + * @param[in & out] vbTriangulated 特征点三角化成功的标志 + * @param[in] minParallax 认为三角化有效的最小视差角 + * @param[in] minTriangulated 最小三角化点数量 + * @return true 成功初始化 + * @return false 初始化失败 + */ +bool Initializer::ReconstructF(vector &vbMatchesInliers, cv::Mat &F21, cv::Mat &K, + cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated) +{ + // Step 1 统计有效匹配点个数，并用 N 表示 + // vbMatchesInliers 中存储匹配点对是否是有效 + int N=0; + for(size_t i=0, iend = vbMatchesInliers.size() ; i vP3D1, vP3D2, vP3D3, vP3D4; + + // 定义四组解分别对同一匹配点集的有效三角化结果，True or False + vector vbTriangulated1,vbTriangulated2,vbTriangulated3, vbTriangulated4; + + // 定义四种解对应的比较大的特征点对视差角 + float parallax1,parallax2, parallax3, parallax4; + + // Step 4.1 使用同样的匹配点分别检查四组解，记录当前计算的3D点在摄像头前方且投影误差小于阈值的个数，记为有效3D点个数 + int nGood1 = CheckRT(R1,t1, //当前组解 + mvKeys1,mvKeys2, //参考帧和当前帧中的特征点 + mvMatches12, vbMatchesInliers, //特征点的匹配关系和Inliers标记 + K, //相机的内参数矩阵 + vP3D1, //存储三角化以后特征点的空间坐标 + 4.0*mSigma2, //三角化测量过程中允许的最大重投影误差 + vbTriangulated1, //参考帧中被成功进行三角化测量的特征点的标记 + parallax1); //认为某对特征点三角化测量有效的比较大的视差角 + int nGood2 = CheckRT(R2,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D2, 4.0*mSigma2, vbTriangulated2, parallax2); + int nGood3 = CheckRT(R1,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D3, 4.0*mSigma2, vbTriangulated3, parallax3); + int nGood4 = CheckRT(R2,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D4, 4.0*mSigma2, vbTriangulated4, parallax4); + + // Step 4.2 选取最大可三角化测量的点的数目 + int maxGood = max(nGood1,max(nGood2,max(nGood3,nGood4))); + + // 重置变量，并在后面赋值为最佳R和T + R21 = cv::Mat(); + t21 = cv::Mat(); + + // Step 4.3 确定最小的可以三角化的点数 + // 在0.9倍的内点数 和 指定值minTriangulated =50 中取最大的，也就是说至少50个 + int nMinGood = max(static_cast(0.9*N), minTriangulated); + + // 统计四组解中重建的有效3D点个数 > 0.7 * maxGood 的解的数目 + // 如果有多个解同时满足该条件，认为结果太接近，nsimilar++，nsimilar>1就认为有问题了，后面返回false + int nsimilar = 0; + if(nGood1>0.7*maxGood) + nsimilar++; + if(nGood2>0.7*maxGood) + nsimilar++; + if(nGood3>0.7*maxGood) + nsimilar++; + if(nGood4>0.7*maxGood) + nsimilar++; + + // Step 4.4 四个结果中如果没有明显的最优结果，或者没有足够数量的三角化点，则返回失败 + // 条件1: 如果四组解能够重建的最多3D点个数小于所要求的最少3D点个数（mMinGood），失败 + // 条件2: 如果存在两组及以上的解能三角化出 >0.7*maxGood的点，说明没有明显最优结果，失败 + if(maxGood1) + { + return false; + } + + + // Step 4.5 选择最佳解记录结果 + // 条件1: 有效重建最多的3D点，即maxGood == nGoodx，也即是位于相机前方的3D点个数最多 + // 条件2: 三角化视差角 parallax 必须大于最小视差角 minParallax，角度越大3D点越稳定 + + //看看最好的good点是在哪种解的条件下发生的 + if(maxGood==nGood1) + { + //如果该种解下的parallax大于函数参数中给定的最小值 + if(parallax1>minParallax) + { + // 存储3D坐标 + vP3D = vP3D1; + + // 获取特征点向量的三角化测量标记 + vbTriangulated = vbTriangulated1; + + // 存储相机姿态 + R1.copyTo(R21); + t1.copyTo(t21); + + // 结束 + return true; + } + }else if(maxGood==nGood2) + { + if(parallax2>minParallax) + { + vP3D = vP3D2; + vbTriangulated = vbTriangulated2; + + R2.copyTo(R21); + t1.copyTo(t21); + return true; + } + }else if(maxGood==nGood3) + { + if(parallax3>minParallax) + { + vP3D = vP3D3; + vbTriangulated = vbTriangulated3; + + R1.copyTo(R21); + t2.copyTo(t21); + return true; + } + }else if(maxGood==nGood4) + { + if(parallax4>minParallax) + { + vP3D = vP3D4; + vbTriangulated = vbTriangulated4; + + R2.copyTo(R21); + t2.copyTo(t21); + return true; + } + } + + // 如果有最优解但是不满足对应的parallax>minParallax，那么返回false表示求解失败 + return false; +} + +/** + * @brief 用H矩阵恢复R, t和三维点 + * H矩阵分解常见有两种方法：Faugeras SVD-based decomposition 和 Zhang SVD-based decomposition + * 代码使用了Faugeras SVD-based decomposition算法，参考文献 + * Motion and structure from motion in a piecewise planar environment. International Journal of Pattern Recognition and Artificial Intelligence, 1988 + * + * @param[in] vbMatchesInliers 匹配点对的内点标记 + * @param[in] H21 从参考帧到当前帧的单应矩阵 + * @param[in] K 相机的内参数矩阵 + * @param[in & out] R21 计算出来的相机旋转 + * @param[in & out] t21 计算出来的相机平移 + * @param[in & out] vP3D 世界坐标系下，三角化测量特征点对之后得到的特征点的空间坐标 + * @param[in & out] vbTriangulated 特征点是否成功三角化的标记 + * @param[in] minParallax 对特征点的三角化测量中，认为其测量有效时需要满足的最小视差角（如果视差角过小则会引起非常大的观测误差）,单位是角度 + * @param[in] minTriangulated 为了进行运动恢复，所需要的最少的三角化测量成功的点个数 + * @return true 单应矩阵成功计算出位姿和三维点 + * @return false 初始化失败 + */ +bool Initializer::ReconstructH(vector &vbMatchesInliers, cv::Mat &H21, cv::Mat &K, + cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated) +{ + + // 目的 ：通过单应矩阵H恢复两帧图像之间的旋转矩阵R和平移向量T + // 参考 ：Motion and structure from motion in a piecewise plannar environment. + // International Journal of Pattern Recognition and Artificial Intelligence, 1988 + // https://www.researchgate.net/publication/243764888_Motion_and_Structure_from_Motion_in_a_Piecewise_Planar_Environment + + // 流程: + // 1. 根据H矩阵的奇异值d'= d2 或者 d' = -d2 分别计算 H 矩阵分解的 8 组解 + // 1.1 讨论 d' > 0 时的 4 组解 + // 1.2 讨论 d' < 0 时的 4 组解 + // 2. 对 8 组解进行验证，并选择产生相机前方最多3D点的解为最优解 + + // 统计匹配的特征点对中属于内点(Inlier)或有效点个数 + int N=0; + for(size_t i=0, iend = vbMatchesInliers.size() ; i(0); + float d2 = w.at(1); + float d3 = w.at(2); + + // SVD分解正常情况下特征值di应该是正的，且满足d1>=d2>=d3 + if(d1/d2<1.00001 || d2/d3<1.00001) { + return false; + } + + + // 在ORBSLAM中没有对奇异值 d1 d2 d3按照论文中描述的关系进行分类讨论, 而是直接进行了计算 + // 定义8中情况下的旋转矩阵、平移向量和空间向量 + vector vR, vt, vn; + vR.reserve(8); + vt.reserve(8); + vn.reserve(8); + + // Step 1.1 讨论 d' > 0 时的 4 组解 + // 根据论文eq.(12)有 + // x1 = e1 * sqrt((d1 * d1 - d2 * d2) / (d1 * d1 - d3 * d3)) + // x2 = 0 + // x3 = e3 * sqrt((d2 * d2 - d2 * d2) / (d1 * d1 - d3 * d3)) + // 令 aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3)) + // aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3)) + // 则 + // x1 = e1 * aux1 + // x3 = e3 * aux2 + + // 因为 e1,e2,e3 = 1 or -1 + // 所以有x1和x3有四种组合 + // x1 = {aux1,aux1,-aux1,-aux1} + // x3 = {aux3,-aux3,aux3,-aux3} + + float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3)); + float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3)); + float x1[] = {aux1,aux1,-aux1,-aux1}; + float x3[] = {aux3,-aux3,aux3,-aux3}; + + + // 根据论文eq.(13)有 + // sin(theta) = e1 * e3 * sqrt(( d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)) /(d1 + d3)/d2 + // cos(theta) = (d2* d2 + d1 * d3) / (d1 + d3) / d2 + // 令 aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2) + // 则 sin(theta) = e1 * e3 * aux_stheta + // cos(theta) = (d2*d2+d1*d3)/((d1+d3)*d2) + // 因为 e1 e2 e3 = 1 or -1 + // 所以 sin(theta) = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta} + float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2); + + float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2); + float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta}; + + // 计算旋转矩阵 R' + //根据不同的e1 e3组合所得出来的四种R t的解 + // | ctheta 0 -aux_stheta| | aux1| + // Rp = | 0 1 0 | tp = | 0 | + // | aux_stheta 0 ctheta | |-aux3| + + // | ctheta 0 aux_stheta| | aux1| + // Rp = | 0 1 0 | tp = | 0 | + // |-aux_stheta 0 ctheta | | aux3| + + // | ctheta 0 aux_stheta| |-aux1| + // Rp = | 0 1 0 | tp = | 0 | + // |-aux_stheta 0 ctheta | |-aux3| + + // | ctheta 0 -aux_stheta| |-aux1| + // Rp = | 0 1 0 | tp = | 0 | + // | aux_stheta 0 ctheta | | aux3| + // 开始遍历这四种情况中的每一种 + for(int i=0; i<4; i++) + { + //生成Rp，就是eq.(8) 的 R' + cv::Mat Rp=cv::Mat::eye(3,3,CV_32F); + Rp.at(0,0)=ctheta; + Rp.at(0,2)=-stheta[i]; + Rp.at(2,0)=stheta[i]; + Rp.at(2,2)=ctheta; + + // eq.(8) 计算R + cv::Mat R = s*U*Rp*Vt; + + // 保存 + vR.push_back(R); + + // eq. (14) 生成tp + cv::Mat tp(3,1,CV_32F); + tp.at(0)=x1[i]; + tp.at(1)=0; + tp.at(2)=-x3[i]; + tp*=d1-d3; + + // 这里虽然对t有归一化，并没有决定单目整个SLAM过程的尺度 + // 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变 + // eq.(8)恢复原始的t + cv::Mat t = U*tp; + vt.push_back(t/cv::norm(t)); + + // 构造法向量np + cv::Mat np(3,1,CV_32F); + np.at(0)=x1[i]; + np.at(1)=0; + np.at(2)=x3[i]; + + // eq.(8) 恢复原始的法向量 + cv::Mat n = V*np; + // 保持平面法向量向上 + if(n.at(2)<0) + n=-n; + vn.push_back(n); + } + + // Step 1.2 讨论 d' < 0 时的 4 组解 + float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2); + // cos_theta项 + float cphi = (d1*d3-d2*d2)/((d1-d3)*d2); + // 考虑到e1,e2的取值，这里的sin_theta有两种可能的解 + float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi}; + + // 对于每种由e1 e3取值的组合而形成的四种解的情况 + for(int i=0; i<4; i++) + { + // 计算旋转矩阵 R' + cv::Mat Rp=cv::Mat::eye(3,3,CV_32F); + Rp.at(0,0)=cphi; + Rp.at(0,2)=sphi[i]; + Rp.at(1,1)=-1; + Rp.at(2,0)=sphi[i]; + Rp.at(2,2)=-cphi; + + // 恢复出原来的R + cv::Mat R = s*U*Rp*Vt; + // 然后添加到vector中 + vR.push_back(R); + + // 构造tp + cv::Mat tp(3,1,CV_32F); + tp.at(0)=x1[i]; + tp.at(1)=0; + tp.at(2)=x3[i]; + tp*=d1+d3; + + // 恢复出原来的t + cv::Mat t = U*tp; + // 归一化之后加入到vector中,要提供给上面的平移矩阵都是要进行过归一化的 + vt.push_back(t/cv::norm(t)); + + // 构造法向量np + cv::Mat np(3,1,CV_32F); + np.at(0)=x1[i]; + np.at(1)=0; + np.at(2)=x3[i]; + + // 恢复出原来的法向量 + cv::Mat n = V*np; + // 保证法向量指向上方 + if(n.at(2)<0) + n=-n; + // 添加到vector中 + vn.push_back(n); + } + + // 最好的good点 + int bestGood = 0; + // 其次最好的good点 + int secondBestGood = 0; + // 最好的解的索引，初始值为-1 + int bestSolutionIdx = -1; + // 最大的视差角 + float bestParallax = -1; + // 存储最好解对应的，对特征点对进行三角化测量的结果 + vector bestP3D; + // 最佳解所对应的，那些可以被三角化测量的点的标记 + vector bestTriangulated; + + // Instead of applying the visibility constraints proposed in the Faugeras' paper (which could fail for points seen with low parallax) + // We reconstruct all hypotheses and check in terms of triangulated points and parallax + + // Step 2. 对 8 组解进行验证，并选择产生相机前方最多3D点的解为最优解 + for(size_t i=0; i<8; i++) + { + // 第i组解对应的比较大的视差角 + float parallaxi; + // 三角化测量之后的特征点的空间坐标 + vector vP3Di; + // 特征点对是否被三角化的标记 + vector vbTriangulatedi; + + // 调用 Initializer::CheckRT(), 计算good点的数目 + int nGood = CheckRT(vR[i],vt[i], //当前组解的旋转矩阵和平移向量 + mvKeys1,mvKeys2, //特征点 + mvMatches12,vbMatchesInliers, //特征匹配关系以及Inlier标记 + K, //相机的内参数矩阵 + vP3Di, //存储三角化测量之后的特征点空间坐标的 + 4.0*mSigma2, //三角化过程中允许的最大重投影误差 + vbTriangulatedi, //特征点是否被成功进行三角测量的标记 + parallaxi); // 这组解在三角化测量的时候的比较大的视差角 + + // 更新历史最优和次优的解 + // 保留最优的和次优的解.保存次优解的目的是看看最优解是否突出 + if(nGood>bestGood) + { + // 如果当前组解的good点数是历史最优，那么之前的历史最优就变成了历史次优 + secondBestGood = bestGood; + // 更新历史最优点 + bestGood = nGood; + // 最优解的组索引为i（就是当前次遍历） + bestSolutionIdx = i; + // 更新变量 + bestParallax = parallaxi; + bestP3D = vP3Di; + bestTriangulated = vbTriangulatedi; + } + // 如果当前组的good计数小于历史最优但却大于历史次优 + else if(nGood>secondBestGood) + { + // 说明当前组解是历史次优点，更新之 + secondBestGood = nGood; + } + } + + + + // Step 3 选择最优解。要满足下面的四个条件 + // 1. good点数最优解明显大于次优解，这里取0.75经验值 + // 2. 视角差大于规定的阈值 + // 3. good点数要大于规定的最小的被三角化的点数量 + // 4. good数要足够多，达到总数的90%以上 + if(secondBestGood<0.75*bestGood && + bestParallax>=minParallax && + bestGood>minTriangulated && + bestGood>0.9*N) + { + // 从最佳的解的索引访问到R，t + vR[bestSolutionIdx].copyTo(R21); + vt[bestSolutionIdx].copyTo(t21); + // 获得最佳解时，成功三角化的三维点，以后作为初始地图点使用 + vP3D = bestP3D; + // 获取特征点的被成功进行三角化的标记 + vbTriangulated = bestTriangulated; + + //返回真，找到了最好的解 + return true; + } + + return false; +} + +/** 给定投影矩阵P1,P2和图像上的匹配特征点点kp1,kp2，从而计算三维点坐标 + * @brief + * + * @param[in] kp1 特征点, in reference frame + * @param[in] kp2 特征点, in current frame + * @param[in] P1 投影矩阵P1 + * @param[in] P2 投影矩阵P2 + * @param[in & out] x3D 计算的三维点 + */ +void Initializer::Triangulate( + const cv::KeyPoint &kp1, //特征点, in reference frame + const cv::KeyPoint &kp2, //特征点, in current frame + const cv::Mat &P1, //投影矩阵P1 + const cv::Mat &P2, //投影矩阵P2 + cv::Mat &x3D) //三维点 +{ + // 原理 + // Trianularization: 已知匹配特征点对{x x'} 和 各自相机矩阵{P P'}, 估计三维点 X + // x' = P'X x = PX + // 它们都属于 x = aPX模型 + // |X| + // |x| |p1 p2 p3 p4 ||Y| |x| |--p0--||.| + // |y| = a |p5 p6 p7 p8 ||Z| ===>|y| = a|--p1--||X| + // |z| |p9 p10 p11 p12||1| |z| |--p2--||.| + // 采用DLT的方法：x叉乘PX = 0 + // |yp2 - p1| |0| + // |p0 - xp2| X = |0| + // |xp1 - yp0| |0| + // 两个点: + // |yp2 - p1 | |0| + // |p0 - xp2 | X = |0| ===> AX = 0 + // |y'p2' - p1' | |0| + // |p0' - x'p2'| |0| + // 变成程序中的形式： + // |xp2 - p0 | |0| + // |yp2 - p1 | X = |0| ===> AX = 0 + // |x'p2'- p0'| |0| + // |y'p2'- p1'| |0| + // 然后就组成了一个四元一次正定方程组，SVD求解，右奇异矩阵的最后一行就是最终的解. + + //这个就是上面注释中的矩阵A + cv::Mat A(4,4,CV_32F); + + //构造参数矩阵A + A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0); + A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1); + A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0); + A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1); + + //奇异值分解的结果 + cv::Mat u,w,vt; + //对系数矩阵A进行奇异值分解 + cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV); + //根据前面的结论，奇异值分解右矩阵的最后一行其实就是解，原理类似于前面的求最小二乘解，四个未知数四个方程正好正定 + //别忘了我们更习惯用列向量来表示一个点的空间坐标 + x3D = vt.row(3).t(); + //为了符合其次坐标的形式，使最后一维为1 + x3D = x3D.rowRange(0,3)/x3D.at(3); +} + + +/** + * @brief 归一化特征点到同一尺度，作为后续normalize DLT的输入 + * [x' y' 1]' = T * [x y 1]' + * 归一化后x', y'的均值为0，sum(abs(x_i'-0))=1，sum(abs((y_i'-0))=1 + * + * 为什么要归一化？ + * 在相似变换之后(点在不同的坐标系下),他们的单应性矩阵是不相同的 + * 如果图像存在噪声,使得点的坐标发生了变化,那么它的单应性矩阵也会发生变化 + * 我们采取的方法是将点的坐标放到同一坐标系下,并将缩放尺度也进行统一 + * 对同一幅图像的坐标进行相同的变换,不同图像进行不同变换 + * 缩放尺度是为了让噪声对于图像的影响在一个数量级上 + * + * Step 1 计算特征点X,Y坐标的均值 + * Step 2 计算特征点X,Y坐标离均值的平均偏离程度 + * Step 3 将x坐标和y坐标分别进行尺度归一化，使得x坐标和y坐标的一阶绝对矩分别为1 + * Step 4 计算归一化矩阵：其实就是前面做的操作用矩阵变换来表示而已 + * + * @param[in] vKeys 待归一化的特征点 + * @param[in & out] vNormalizedPoints 特征点归一化后的坐标 + * @param[in & out] T 归一化特征点的变换矩阵 + */ +void Initializer::Normalize(const vector &vKeys, vector &vNormalizedPoints, cv::Mat &T) //将特征点归一化的矩阵 +{ + // 归一化的是这些点在x方向和在y方向上的一阶绝对矩（随机变量的期望）。 + + // Step 1 计算特征点X,Y坐标的均值 meanX, meanY + float meanX = 0; + float meanY = 0; + + //获取特征点的数量 + const int N = vKeys.size(); + + //设置用来存储归一后特征点的向量大小，和归一化前保持一致 + vNormalizedPoints.resize(N); + + //开始遍历所有的特征点 + for(int i=0; i(0,0) = sX; + T.at(1,1) = sY; + T.at(0,2) = -meanX*sX; + T.at(1,2) = -meanY*sY; +} + +/** + * @brief 用R，t来对特征匹配点三角化，并根据三角化结果判断R,t的合法性 + * + * @param[in] R 旋转矩阵R + * @param[in] t 平移矩阵t + * @param[in] vKeys1 参考帧特征点 + * @param[in] vKeys2 当前帧特征点 + * @param[in] vMatches12 两帧特征点的匹配关系 + * @param[in] vbMatchesInliers 特征点对内点标记 + * @param[in] K 相机内参矩阵 + * @param[in & out] vP3D 三角化测量之后的特征点的空间坐标 + * @param[in] th2 重投影误差的阈值 + * @param[in & out] vbGood 标记成功三角化点？ + * @param[in & out] parallax 计算出来的比较大的视差角（注意不是最大，具体看后面代码） + * @return int + */ +int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector &vKeys1, const vector &vKeys2, + const vector &vMatches12, vector &vbMatchesInliers, + const cv::Mat &K, vector &vP3D, float th2, vector &vbGood, float ¶llax) +{ + // 对给出的特征点对及其R t , 通过三角化检查解的有效性，也称为 cheirality check + //特征点是否是good点的标记，这里的特征点指的是参考帧中的特征点 + vbGood = vector(vKeys1.size(),false); + //重设存储空间坐标的点的大小 + vP3D.resize(vKeys1.size()); + + //存储计算出来的每对特征点的视差 + vector vCosParallax; + vCosParallax.reserve(vKeys1.size()); + + // Camera 1 Projection Matrix K[I|0] + // Step 1：计算相机的投影矩阵 + // 投影矩阵P是一个 3x4 的矩阵，可以将空间中的一个点投影到平面上，获得其平面坐标，这里均指的是齐次坐标。 + // 对于第一个相机是 P1=K*[I|0] + + // 以第一个相机的光心作为世界坐标系, 定义相机的投影矩阵 + cv::Mat P1(3,4, //矩阵的大小是3x4 + CV_32F, //数据类型是浮点数 + cv::Scalar(0)); //初始的数值是0 + //将整个K矩阵拷贝到P1矩阵的左侧3x3矩阵，因为 K*I = K + K.copyTo(P1.rowRange(0,3).colRange(0,3)); + // 第一个相机的光心设置为世界坐标系下的原点 + cv::Mat O1 = cv::Mat::zeros(3,1,CV_32F); + + // Camera 2 Projection Matrix K[R|t] + // 计算第二个相机的投影矩阵 P2=K*[R|t] + cv::Mat P2(3,4,CV_32F); + R.copyTo(P2.rowRange(0,3).colRange(0,3)); + t.copyTo(P2.rowRange(0,3).col(3)); + //最终结果是K*[R|t] + P2 = K*P2; + // 第二个相机的光心在世界坐标系下的坐标 + cv::Mat O2 = -R.t()*t; + + //在遍历开始前，先将good点计数设置为0 + int nGood=0; + + // 开始遍历所有的特征点对 + for(size_t i=0, iend=vMatches12.size();i(0)) || !isfinite(p3dC1.at(1)) || !isfinite(p3dC1.at(2))) + { + //其实这里就算是不这样写也没问题，因为默认的匹配点对就不是good点 + vbGood[vMatches12[i].first]=false; + //继续对下一对匹配点的处理 + continue; + } + + // Check parallax + // Step 4 第二关：通过三维点深度值正负、两相机光心视差角大小来检查是否合法 + + //得到向量PO1 + cv::Mat normal1 = p3dC1 - O1; + //求取模长，其实就是距离 + float dist1 = cv::norm(normal1); + + //同理构造向量PO2 + cv::Mat normal2 = p3dC1 - O2; + //求模长 + float dist2 = cv::norm(normal2); + + //根据公式：a.*b=|a||b|cos_theta 可以推导出来下面的式子 + float cosParallax = normal1.dot(normal2)/(dist1*dist2); + + // Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth) + // 如果深度值为负值，为非法三维点跳过该匹配点对 + // ?视差比较小时，重投影误差比较大。这里0.99998 对应的角度为0.36°,这里不应该是 cosParallax>0.99998 吗？ + // ?因为后面判断vbGood 点时的条件也是 cosParallax<0.99998 + // !可能导致初始化不稳定 + if(p3dC1.at(2)<=0 && cosParallax<0.99998) + continue; + + // Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth) + // 讲空间点p3dC1变换到第2个相机坐标系下变为p3dC2 + cv::Mat p3dC2 = R*p3dC1+t; + //判断过程和上面的相同 + if(p3dC2.at(2)<=0 && cosParallax<0.99998) + continue; + + // Step 5 第三关：计算空间点在参考帧和当前帧上的重投影误差，如果大于阈值则舍弃 + // Check reprojection error in first image + cv::Point2f uv1 = mpCamera->project(p3dC1); + //参考帧上的重投影误差，这个的确就是按照定义来的 + float squareError1 = (uv1.x-kp1.pt.x)*(uv1.x-kp1.pt.x)+(uv1.y-kp1.pt.y)*(uv1.y-kp1.pt.y); + + // 重投影误差太大，跳过淘汰 + if(squareError1>th2) + continue; + + // Check reprojection error in second image + cv::Point2f uv2 = mpCamera->project(p3dC2); + float squareError2 = (uv2.x-kp2.pt.x)*(uv2.x-kp2.pt.x)+(uv2.y-kp2.pt.y)*(uv2.y-kp2.pt.y); + + // 重投影误差太大，跳过淘汰 + if(squareError2>th2) + continue; + + // Step 6 统计经过检验的3D点个数，记录3D点视差角 + // 如果运行到这里就说明当前遍历的这个特征点对靠谱，经过了重重检验，说明是一个合格的点，称之为good点 + vCosParallax.push_back(cosParallax); + //存储这个三角化测量后的3D点在世界坐标系下的坐标 + vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at(0),p3dC1.at(1),p3dC1.at(2)); + //good点计数++ + nGood++; + + //判断视差角，只有视差角稍稍大一丢丢的才会给打good点标记 + //? bug 我觉得这个写的位置不太对。你的good点计数都++了然后才判断，不是会让good点标志和good点计数不一样吗 + if(cosParallax<0.99998) + vbGood[vMatches12[i].first]=true; + } + // Step 7 得到3D点中较小的视差角，并且转换成为角度制表示 + if(nGood>0) + { + // 从小到大排序，注意vCosParallax值越大，视差越小 + sort(vCosParallax.begin(),vCosParallax.end()); + + // !排序后并没有取最小的视差角，而是取一个较小的视差角 + // 作者的做法：如果经过检验过后的有效3D点小于50个，那么就取最后那个最小的视差角(cos值最大) + // 如果大于50个，就取排名第50个的较小的视差角即可，为了避免3D点太多时出现太小的视差角 + size_t idx = min(50,int(vCosParallax.size()-1)); + //将这个选中的角弧度制转换为角度制 + parallax = acos(vCosParallax[idx])*180/CV_PI; + } + else + //如果没有good点那么这个就直接设置为0了 + parallax=0; + + //返回good点计数 + return nGood; +} + +/** + * @brief 分解Essential矩阵得到R,t + * 分解E矩阵将得到4组解，这4组解分别为[R1,t],[R1,-t],[R2,t],[R2,-t] + * 参考：Multiple View Geometry in Computer Vision - Result 9.19 p259 + * @param[in] E 本质矩阵 + * @param[in & out] R1 旋转矩阵1 + * @param[in & out] R2 旋转矩阵2 + * @param[in & out] t 平移向量，另外一个取相反数 + */ +void Initializer::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t) +{ + + // 对本质矩阵进行奇异值分解 + //准备存储对本质矩阵进行奇异值分解的结果 + cv::Mat u,w,vt; + //对本质矩阵进行奇异值分解 + cv::SVD::compute(E,w,u,vt); + + u.col(2).copyTo(t); + t=t/cv::norm(t); + + // 构造一个绕Z轴旋转pi/2的旋转矩阵W，按照下式组合得到旋转矩阵 R1 = u*W*vt + //计算完成后要检查一下旋转矩阵行列式的数值，使其满足行列式为1的约束 + cv::Mat W(3,3,CV_32F,cv::Scalar(0)); + W.at(0,1)=-1; + W.at(1,0)=1; + W.at(2,2)=1; + + //计算 + R1 = u*W*vt; + //旋转矩阵有行列式为+1的约束，所以如果算出来为负值，需要取反 + if(cv::determinant(R1)<0) + R1=-R1; + + // 同理将矩阵W取转置来按照相同的公式计算旋转矩阵R2 = u*W.t()*vt + R2 = u*W.t()*vt; + //旋转矩阵有行列式为1的约束 + if(cv::determinant(R2)<0) + R2=-R2; +} + +} //namespace ORB_SLAM diff --git a/ORB-SLAM3-UESTC/Workspace/src/KeyFrame.cc b/ORB-SLAM3-UESTC/Workspace/src/KeyFrame.cc new file mode 100644 index 0000000..40f9c2d --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/KeyFrame.cc @@ -0,0 +1,1320 @@ +/** + * This file is part of ORB-SLAM3 + * + * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * + * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public + * License as published by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even + * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with ORB-SLAM3. + * If not, see . + */ + +#include "KeyFrame.h" +#include "Converter.h" +#include "ImuTypes.h" +#include + +namespace ORB_SLAM3 +{ + +long unsigned int KeyFrame::nNextId = 0; + +KeyFrame::KeyFrame() + : mnFrameId(0), mTimeStamp(0), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS), + mfGridElementWidthInv(0), mfGridElementHeightInv(0), + mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0), mnBALocalForMerge(0), + mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnMergeQuery(0), mnMergeWords(0), mnBAGlobalForKF(0), + fx(0), fy(0), cx(0), cy(0), invfx(0), invfy(0), mnPlaceRecognitionQuery(0), mnPlaceRecognitionWords(0), mPlaceRecognitionScore(0), + mbf(0), mb(0), mThDepth(0), N(0), mvKeys(static_cast>(NULL)), mvKeysUn(static_cast>(NULL)), + mvuRight(static_cast>(NULL)), mvDepth(static_cast>(NULL)), mnScaleLevels(0), mfScaleFactor(0), + mfLogScaleFactor(0), mvScaleFactors(0), mvLevelSigma2(0), mvInvLevelSigma2(0), mnMinX(0), mnMinY(0), mnMaxX(0), + mnMaxY(0), mPrevKF(static_cast(NULL)), mNextKF(static_cast(NULL)), mbFirstConnection(true), mpParent(NULL), mbNotErase(false), + mbToBeErased(false), mbBad(false), mHalfBaseline(0), mbCurrentPlaceRecognition(false), mnMergeCorrectedForKF(0), + NLeft(0), NRight(0), mnNumberOfOpt(0), mbHasVelocity(false) +{ +} + +KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB) + : bImu(pMap->isImuInitialized()), mnFrameId(F.mnId), mTimeStamp(F.mTimeStamp), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS), + mfGridElementWidthInv(F.mfGridElementWidthInv), mfGridElementHeightInv(F.mfGridElementHeightInv), + mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0), mnBALocalForMerge(0), + mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnBAGlobalForKF(0), mnPlaceRecognitionQuery(0), mnPlaceRecognitionWords(0), mPlaceRecognitionScore(0), + fx(F.fx), fy(F.fy), cx(F.cx), cy(F.cy), invfx(F.invfx), invfy(F.invfy), + mbf(F.mbf), mb(F.mb), mThDepth(F.mThDepth), N(F.N), mvKeys(F.mvKeys), mvKeysUn(F.mvKeysUn), + mvuRight(F.mvuRight), mvDepth(F.mvDepth), mDescriptors(F.mDescriptors.clone()), + mBowVec(F.mBowVec), mFeatVec(F.mFeatVec), mnScaleLevels(F.mnScaleLevels), mfScaleFactor(F.mfScaleFactor), + mfLogScaleFactor(F.mfLogScaleFactor), mvScaleFactors(F.mvScaleFactors), mvLevelSigma2(F.mvLevelSigma2), + mvInvLevelSigma2(F.mvInvLevelSigma2), mnMinX(F.mnMinX), mnMinY(F.mnMinY), mnMaxX(F.mnMaxX), + mnMaxY(F.mnMaxY), mK_(F.mK_), mPrevKF(NULL), mNextKF(NULL), mpImuPreintegrated(F.mpImuPreintegrated), + mImuCalib(F.mImuCalib), mvpMapPoints(F.mvpMapPoints), mpKeyFrameDB(pKFDB), + mpORBvocabulary(F.mpORBvocabulary), mbFirstConnection(true), mpParent(NULL), mDistCoef(F.mDistCoef), mbNotErase(false), mnDataset(F.mnDataset), + mbToBeErased(false), mbBad(false), mHalfBaseline(F.mb / 2), mpMap(pMap), mbCurrentPlaceRecognition(false), mNameFile(F.mNameFile), mnMergeCorrectedForKF(0), + mpCamera(F.mpCamera), mpCamera2(F.mpCamera2), + mvLeftToRightMatch(F.mvLeftToRightMatch), mvRightToLeftMatch(F.mvRightToLeftMatch), mTlr(F.GetRelativePoseTlr()), + mvKeysRight(F.mvKeysRight), NLeft(F.Nleft), NRight(F.Nright), mTrl(F.GetRelativePoseTrl()), mnNumberOfOpt(0), mbHasVelocity(false) +{ + mnId = nNextId++; + + // 根据指定的普通帧, 初始化用于加速匹配的网格对象信息; 其实就把每个网格中有的特征点的索引复制过来 + mGrid.resize(mnGridCols); + if (F.Nleft != -1) + mGridRight.resize(mnGridCols); + for (int i = 0; i < mnGridCols; i++) + { + mGrid[i].resize(mnGridRows); + if (F.Nleft != -1) + mGridRight[i].resize(mnGridRows); + for (int j = 0; j < mnGridRows; j++) + { + mGrid[i][j] = F.mGrid[i][j]; + if (F.Nleft != -1) + { + mGridRight[i][j] = F.mGridRight[i][j]; + } + } + } + + if (!F.HasVelocity()) + { + mVw.setZero(); + mbHasVelocity = false; + } + else + { + mVw = F.GetVelocity(); + mbHasVelocity = true; + } + + mImuBias = F.mImuBias; + SetPose(F.GetPose()); + + mnOriginMapId = pMap->GetId(); +} + +void KeyFrame::ComputeBoW() +{ + // 只有当词袋向量或者节点和特征序号的特征向量为空的时候执行 + if (mBowVec.empty() || mFeatVec.empty()) + { + vector vCurrentDesc = Converter::toDescriptorVector(mDescriptors); + // Feature vector associate features with nodes in the 4th level (from leaves up) + // We assume the vocabulary tree has 6 levels, change the 4 otherwise + mpORBvocabulary->transform(vCurrentDesc, mBowVec, mFeatVec, 4); + } +} + +// 设置当前关键帧的位姿 +void KeyFrame::SetPose(const Sophus::SE3f &Tcw) +{ + unique_lock lock(mMutexPose); + + mTcw = Tcw; + mRcw = mTcw.rotationMatrix(); + mTwc = mTcw.inverse(); + mRwc = mTwc.rotationMatrix(); + + if (mImuCalib.mbIsSet) // TODO Use a flag instead of the OpenCV matrix + { + mOwb = mRwc * mImuCalib.mTcb.translation() + mTwc.translation(); + } +} + +void KeyFrame::SetVelocity(const Eigen::Vector3f &Vw) +{ + unique_lock lock(mMutexPose); + mVw = Vw; + mbHasVelocity = true; +} + +// 获取位姿 +Sophus::SE3f KeyFrame::GetPose() +{ + unique_lock lock(mMutexPose); + return mTcw; +} + +// 获取位姿的逆 +Sophus::SE3f KeyFrame::GetPoseInverse() +{ + unique_lock lock(mMutexPose); + return mTwc; +} + +// 获取(左目)相机的中心在世界坐标系下的坐标 +Eigen::Vector3f KeyFrame::GetCameraCenter() +{ + unique_lock lock(mMutexPose); + return mTwc.translation(); +} + +Eigen::Vector3f KeyFrame::GetImuPosition() +{ + unique_lock lock(mMutexPose); + return mOwb; +} + +Eigen::Matrix3f KeyFrame::GetImuRotation() +{ + unique_lock lock(mMutexPose); + return (mTwc * mImuCalib.mTcb).rotationMatrix(); +} + +Sophus::SE3f KeyFrame::GetImuPose() +{ + unique_lock lock(mMutexPose); + return mTwc * mImuCalib.mTcb; +} + +Eigen::Matrix3f KeyFrame::GetRotation() +{ + unique_lock lock(mMutexPose); + return mRcw; +} + +Eigen::Vector3f KeyFrame::GetTranslation() +{ + unique_lock lock(mMutexPose); + return mTcw.translation(); +} + +Eigen::Vector3f KeyFrame::GetVelocity() +{ + unique_lock lock(mMutexPose); + return mVw; +} + +bool KeyFrame::isVelocitySet() +{ + unique_lock lock(mMutexPose); + return mbHasVelocity; +} + +// 为关键帧之间添加或更新连接 +void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight) +{ + { + // 如果被占用就一直等着,这个添加连接的操作不能够被放弃 + unique_lock lock(mMutexConnections); + + // 判断当前关键帧是否已经和其他的关键帧创建了联系 + // std::map::count函数只可能返回0或1两种情况 + + // count函数返回0，mConnectedKeyFrameWeights中没有pKF，之前没有连接 + if (!mConnectedKeyFrameWeights.count(pKF)) + mConnectedKeyFrameWeights[pKF] = weight; + else if (mConnectedKeyFrameWeights[pKF] != weight) // 之前连接的权重不一样，更新 + mConnectedKeyFrameWeights[pKF] = weight; + else + return; + } + + // 如果添加了更新的连接关系就要更新一下,主要是重新进行排序 + UpdateBestCovisibles(); +} + +/** + * @brief 按照权重对连接的关键帧进行排序 + * + * 更新后的变量存储在mvpOrderedConnectedKeyFrames和mvOrderedWeights中 + */ +void KeyFrame::UpdateBestCovisibles() +{ + unique_lock lock(mMutexConnections); + vector> vPairs; + vPairs.reserve(mConnectedKeyFrameWeights.size()); + // 取出所有连接的关键帧，mConnectedKeyFrameWeights的类型为std::map，而vPairs变量将共视的3D点数放在前面，利于排序 + for (map::iterator mit = mConnectedKeyFrameWeights.begin(), mend = mConnectedKeyFrameWeights.end(); mit != mend; mit++) + vPairs.push_back(make_pair(mit->second, mit->first)); + + // 按照权重进行排序（默认是从小到大） + sort(vPairs.begin(), vPairs.end()); + + // 为什么要用链表保存？因为插入和删除操作方便，只需要修改上一节点位置，不需要移动其他元素 + list lKFs; + list lWs; + for (size_t i = 0, iend = vPairs.size(); i < iend; i++) + { + if (!vPairs[i].second->isBad()) + { + // push_front 后变成从大到小 + lKFs.push_front(vPairs[i].second); + lWs.push_front(vPairs[i].first); + } + } + + // 权重从大到小 + mvpOrderedConnectedKeyFrames = vector(lKFs.begin(), lKFs.end()); + mvOrderedWeights = vector(lWs.begin(), lWs.end()); +} + +// 得到与该关键帧连接（>15个共视地图点）的关键帧(没有排序的) +set KeyFrame::GetConnectedKeyFrames() +{ + unique_lock lock(mMutexConnections); + set s; + for (map::iterator mit = mConnectedKeyFrameWeights.begin(); mit != mConnectedKeyFrameWeights.end(); mit++) + s.insert(mit->first); + return s; +} + +// 得到与该关键帧连接的关键帧(已按权值排序) +vector KeyFrame::GetVectorCovisibleKeyFrames() +{ + unique_lock lock(mMutexConnections); + return mvpOrderedConnectedKeyFrames; +} + +// 得到与该关键帧连接的前N个关键帧(已按权值排序) +vector KeyFrame::GetBestCovisibilityKeyFrames(const int &N) +{ + unique_lock lock(mMutexConnections); + // 如果不够达到的数目就直接吧现在所有的关键帧都返回了 + if ((int)mvpOrderedConnectedKeyFrames.size() < N) + return mvpOrderedConnectedKeyFrames; + else + return vector(mvpOrderedConnectedKeyFrames.begin(), mvpOrderedConnectedKeyFrames.begin() + N); +} + +// 得到与该关键帧连接的权重大于等于w的关键帧 +vector KeyFrame::GetCovisiblesByWeight(const int &w) +{ + unique_lock lock(mMutexConnections); + + // 如果没有和当前关键帧连接的关键帧 + if (mvpOrderedConnectedKeyFrames.empty()) + { + return vector(); + } + + // 从mvOrderedWeights找出第一个大于w的那个迭代器 + vector::iterator it = upper_bound(mvOrderedWeights.begin(), mvOrderedWeights.end(), w, KeyFrame::weightComp); + + // 如果没有找到(最大的权重也比给定的阈值小) + if (it == mvOrderedWeights.end() && mvOrderedWeights.back() < w) + { + return vector(); + } + else + { + int n = it - mvOrderedWeights.begin(); + return vector(mvpOrderedConnectedKeyFrames.begin(), mvpOrderedConnectedKeyFrames.begin() + n); + } +} + +// 得到该关键帧与pKF的权重 +int KeyFrame::GetWeight(KeyFrame *pKF) +{ + unique_lock lock(mMutexConnections); + + // 没有连接的话权重也就是共视点个数就是0 + if (mConnectedKeyFrameWeights.count(pKF)) + return mConnectedKeyFrameWeights[pKF]; + else + return 0; +} + +int KeyFrame::GetNumberMPs() +{ + unique_lock lock(mMutexFeatures); + int numberMPs = 0; + for (size_t i = 0, iend = mvpMapPoints.size(); i < iend; i++) + { + if (!mvpMapPoints[i]) + continue; + numberMPs++; + } + return numberMPs; +} + +void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx) +{ + unique_lock lock(mMutexFeatures); + mvpMapPoints[idx] = pMP; +} + +/** + * @brief 由于其他的原因,导致当前关键帧观测到的某个地图点被删除(bad==true)了,将该地图点置为NULL + * + * @param[in] idx 地图点在该关键帧中的id + */ +void KeyFrame::EraseMapPointMatch(const int &idx) +{ + unique_lock lock(mMutexFeatures); + mvpMapPoints[idx] = static_cast(NULL); +} + +void KeyFrame::EraseMapPointMatch(MapPoint *pMP) +{ + tuple indexes = pMP->GetIndexInKeyFrame(this); + size_t leftIndex = get<0>(indexes), rightIndex = get<1>(indexes); + if (leftIndex != -1) + mvpMapPoints[leftIndex] = static_cast(NULL); + if (rightIndex != -1) + mvpMapPoints[rightIndex] = static_cast(NULL); +} + +// 地图点的替换 +void KeyFrame::ReplaceMapPointMatch(const int &idx, MapPoint *pMP) +{ + mvpMapPoints[idx] = pMP; +} + +// 获取当前关键帧中的所有地图点 +set KeyFrame::GetMapPoints() +{ + unique_lock lock(mMutexFeatures); + set s; + for (size_t i = 0, iend = mvpMapPoints.size(); i < iend; i++) + { + // 判断是否被删除了 + if (!mvpMapPoints[i]) + continue; + MapPoint *pMP = mvpMapPoints[i]; + // 如果是没有来得及删除的坏点也要进行这一步 + if (!pMP->isBad()) + s.insert(pMP); + } + return s; +} + +// 关键帧中，大于等于最少观测数目minObs的MapPoints的数量.这些特征点被认为追踪到了 +int KeyFrame::TrackedMapPoints(const int &minObs) +{ + unique_lock lock(mMutexFeatures); + + int nPoints = 0; + // 是否检查数目 + const bool bCheckObs = minObs > 0; + // N是当前帧中特征点的个数 + for (int i = 0; i < N; i++) + { + MapPoint *pMP = mvpMapPoints[i]; + if (pMP) + { + if (!pMP->isBad()) + { + if (bCheckObs) + { + // 满足输入阈值要求的地图点计数加1 + if (mvpMapPoints[i]->Observations() >= minObs) + nPoints++; + } + else + nPoints++; + } + } + } + + return nPoints; +} + +// 获取当前关键帧的具体的地图点 +vector KeyFrame::GetMapPointMatches() +{ + unique_lock lock(mMutexFeatures); + return mvpMapPoints; +} + +// 获取当前关键帧的具体的某个地图点 +MapPoint *KeyFrame::GetMapPoint(const size_t &idx) +{ + unique_lock lock(mMutexFeatures); + return mvpMapPoints[idx]; +} + +/* + * 更新图的连接 + * + * 1. 首先获得该关键帧的所有MapPoint点，统计观测到这些3d点的每个关键帧与其它所有关键帧之间的共视程度 + * 对每一个找到的关键帧，建立一条边，边的权重是该关键帧与当前关键帧公共3d点的个数。 + * 2. 并且该权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高） + * 3. 对这些连接按照权重从大到小进行排序，以方便将来的处理 + * 更新完covisibility图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树 + */ +void KeyFrame::UpdateConnections(bool upParent) +{ + map KFcounter; + + vector vpMP; + + { + // 获得该关键帧的所有3D点 + unique_lock lockMPs(mMutexFeatures); + vpMP = mvpMapPoints; + } + + // For all map points in keyframe check in which other keyframes are they seen + // Increase counter for those keyframes + // 通过3D点间接统计可以观测到这些3D点的所有关键帧之间的共视程度 + // Step 1 统计每一个地图点都有多少关键帧与当前关键帧存在共视关系，统计结果放在KFcounter + for (vector::iterator vit = vpMP.begin(), vend = vpMP.end(); vit != vend; vit++) + { + MapPoint *pMP = *vit; + + if (!pMP) + continue; + + if (pMP->isBad()) + continue; + + // 对于每一个MapPoint点，observations记录了可以观测到该MapPoint的所有关键帧 + map> observations = pMP->GetObservations(); + + for (map>::iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++) + { + if (mit->first->mnId == mnId || mit->first->isBad() || mit->first->GetMap() != mpMap) + continue; + // 这里的操作非常精彩！ + // map[key] = value，当要插入的键存在时，会覆盖键对应的原来的值。如果键不存在，则添加一组键值对 + // mit->first 是地图点看到的关键帧，同一个关键帧看到的地图点会累加到该关键帧计数 + // 所以最后KFcounter 第一个参数表示某个关键帧，第2个参数表示该关键帧看到了多少当前帧的地图点，也就是共视程度 + KFcounter[mit->first]++; + } + } + + // This should not happen + if (KFcounter.empty()) + return; + + // If the counter is greater than threshold add connection + // In case no keyframe counter is over threshold add the one with maximum counter + int nmax = 0; + KeyFrame *pKFmax = NULL; + // 至少有15个共视地图点 + int th = 15; + + // vPairs记录与其它关键帧共视帧数大于th的关键帧 + // pair将关键帧的权重写在前面，关键帧写在后面方便后面排序 + vector> vPairs; + vPairs.reserve(KFcounter.size()); + if (!upParent) + cout << "UPDATE_CONN: current KF " << mnId << endl; + // Step 2 找到对应权重最大的关键帧（共视程度最高的关键帧） + for (map::iterator mit = KFcounter.begin(), mend = KFcounter.end(); mit != mend; mit++) + { + if (!upParent) + cout << " UPDATE_CONN: KF " << mit->first->mnId << " ; num matches: " << mit->second << endl; + if (mit->second > nmax) + { + nmax = mit->second; + pKFmax = mit->first; + } + if (mit->second >= th) + { + // 对应权重需要大于阈值，对这些关键帧建立连接 + vPairs.push_back(make_pair(mit->second, mit->first)); + // 对方关键帧也要添加这个信息 + // 更新KFcounter中该关键帧的mConnectedKeyFrameWeights + // 更新其它KeyFrame的mConnectedKeyFrameWeights，更新其它关键帧与当前帧的连接权重 + (mit->first)->AddConnection(this, mit->second); + } + } + + // Step 3 如果没有连接到关键（超过阈值的权重），则对权重最大的关键帧建立连接 + if (vPairs.empty()) + { + // 如果每个关键帧与它共视的关键帧的个数都少于th， + // 那就只更新与其它关键帧共视程度最高的关键帧的mConnectedKeyFrameWeights + // 这是对之前th这个阈值可能过高的一个补丁 + vPairs.push_back(make_pair(nmax, pKFmax)); + pKFmax->AddConnection(this, nmax); + } + + // Step 4 对共视程度比较高的关键帧对更新连接关系及权重（从大到小） + // vPairs里存的都是相互共视程度比较高的关键帧和共视权重，接下来由大到小进行排序 + sort(vPairs.begin(), vPairs.end()); // sort函数默认升序排列 + // 将排序后的结果分别组织成为两种数据类型 + list lKFs; + list lWs; + for (size_t i = 0; i < vPairs.size(); i++) + { + // push_front 后变成了从大到小顺序 + lKFs.push_front(vPairs[i].second); + lWs.push_front(vPairs[i].first); + } + + { + unique_lock lockCon(mMutexConnections); + + // 更新当前帧与其它关键帧的连接权重 + mConnectedKeyFrameWeights = KFcounter; + mvpOrderedConnectedKeyFrames = vector(lKFs.begin(), lKFs.end()); + mvOrderedWeights = vector(lWs.begin(), lWs.end()); + + // Step 5 更新生成树的连接 + if (mbFirstConnection && mnId != mpMap->GetInitKFid()) + { + // 初始化该关键帧的父关键帧为共视程度最高的那个关键帧 + mpParent = mvpOrderedConnectedKeyFrames.front(); + // 建立双向连接关系，将当前关键帧作为其子关键帧 + mpParent->AddChild(this); + mbFirstConnection = false; + } + } +} + +// 添加子关键帧（即和子关键帧具有最大共视关系的关键帧就是当前关键帧） +void KeyFrame::AddChild(KeyFrame *pKF) +{ + unique_lock lockCon(mMutexConnections); + mspChildrens.insert(pKF); +} + +// 删除某个子关键帧 +void KeyFrame::EraseChild(KeyFrame *pKF) +{ + unique_lock lockCon(mMutexConnections); + mspChildrens.erase(pKF); +} + +// 改变当前关键帧的父关键帧 +void KeyFrame::ChangeParent(KeyFrame *pKF) +{ + unique_lock lockCon(mMutexConnections); + if (pKF == this) + { + cout << "ERROR: Change parent KF, the parent and child are the same KF" << endl; + throw std::invalid_argument("The parent and child can not be the same"); + } + + mpParent = pKF; + pKF->AddChild(this); +} + +// 获取当前关键帧的子关键帧 +set KeyFrame::GetChilds() +{ + unique_lock lockCon(mMutexConnections); + return mspChildrens; +} + +// 获取当前关键帧的父关键帧 +KeyFrame *KeyFrame::GetParent() +{ + unique_lock lockCon(mMutexConnections); + return mpParent; +} + +// 判断某个关键帧是否是当前关键帧的子关键帧 +bool KeyFrame::hasChild(KeyFrame *pKF) +{ + unique_lock lockCon(mMutexConnections); + return mspChildrens.count(pKF); +} + +void KeyFrame::SetFirstConnection(bool bFirst) +{ + unique_lock lockCon(mMutexConnections); + mbFirstConnection = bFirst; +} + +// 给当前关键帧添加回环边，回环边连接了形成闭环关系的关键帧 +void KeyFrame::AddLoopEdge(KeyFrame *pKF) +{ + unique_lock lockCon(mMutexConnections); + mbNotErase = true; + mspLoopEdges.insert(pKF); +} + +// 获取和当前关键帧形成闭环关系的关键帧 +set KeyFrame::GetLoopEdges() +{ + unique_lock lockCon(mMutexConnections); + return mspLoopEdges; +} + +void KeyFrame::AddMergeEdge(KeyFrame *pKF) +{ + unique_lock lockCon(mMutexConnections); + mbNotErase = true; + mspMergeEdges.insert(pKF); +} + +set KeyFrame::GetMergeEdges() +{ + unique_lock lockCon(mMutexConnections); + return mspMergeEdges; +} + +// 设置当前关键帧不要在优化的过程中被删除. 由回环检测线程调用 +void KeyFrame::SetNotErase() +{ + unique_lock lock(mMutexConnections); + mbNotErase = true; +} + +/** + * @brief 删除当前的这个关键帧,表示不进行回环检测过程;由回环检测线程调用 + * + */ +void KeyFrame::SetErase() +{ + { + unique_lock lock(mMutexConnections); + // 如果当前关键帧和其他的关键帧没有形成回环关系,那么就删吧 + if (mspLoopEdges.empty()) + { + mbNotErase = false; + } + } + + // mbToBeErased：删除之前记录的想要删但时机不合适没有删除的帧 + if (mbToBeErased) + { + SetBadFlag(); + } +} + +/** + * @brief 真正地执行删除关键帧的操作 + * 需要删除的是该关键帧和其他所有帧、地图点之间的连接关系 + * + * mbNotErase作用：表示要删除该关键帧及其连接关系但是这个关键帧有可能正在回环检测或者计算sim3操作，这时候虽然这个关键帧冗余，但是却不能删除， + * 仅设置mbNotErase为true，这时候调用setbadflag函数时，不会将这个关键帧删除，只会把mbTobeErase变成true，代表这个关键帧可以删除但不到时候,先记下来以后处理。 + * 在闭环线程里调用 SetErase()会根据mbToBeErased 来删除之前可以删除还没删除的帧。 + */ +void KeyFrame::SetBadFlag() +{ + { + unique_lock lock(mMutexConnections); + // 初始关键帧不能删除 + if (mnId == mpMap->GetInitKFid()) + { + return; + } + else if (mbNotErase) + { + // mbNotErase表示不应该删除，于是把mbToBeErased置为true，假装已经删除，其实没有删除 + mbToBeErased = true; + return; + } + } + + // Step 2 遍历所有和当前关键帧共视的关键帧，删除他们与当前关键帧的联系 + for (map::iterator mit = mConnectedKeyFrameWeights.begin(), mend = mConnectedKeyFrameWeights.end(); mit != mend; mit++) + { + mit->first->EraseConnection(this); + } + + // Step 3 遍历每一个当前关键帧的地图点，删除每一个地图点和当前关键帧的联系 + for (size_t i = 0; i < mvpMapPoints.size(); i++) + { + if (mvpMapPoints[i]) + { + mvpMapPoints[i]->EraseObservation(this); // 让与自己有联系的MapPoint删除与自己的联系 + } + } + + { + unique_lock lock(mMutexConnections); + unique_lock lock1(mMutexFeatures); + + // 清空自己与其它关键帧之间的联系 + mConnectedKeyFrameWeights.clear(); + mvpOrderedConnectedKeyFrames.clear(); + + // Update Spanning Tree + // Step 4 更新生成树，主要是处理好父子关键帧，不然会造成整个关键帧维护的图断裂，或者混乱，不能够为后端提供较好的初值 + // 子关键帧候选父关键帧 + set sParentCandidates; + // 将当前帧的父关键帧放入候选父关键帧 + if (mpParent) + sParentCandidates.insert(mpParent); + + // Assign at each iteration one children with a parent (the pair with highest covisibility weight) + // Include that children as new parent candidate for the rest + // 如果这个关键帧有自己的子关键帧，告诉这些子关键帧，它们的父关键帧不行了，赶紧找新的父关键帧 + while (!mspChildrens.empty()) + { + bool bContinue = false; + + int max = -1; + KeyFrame *pC; + KeyFrame *pP; + + // Step 4.1 遍历每一个子关键帧，让它们更新它们指向的父关键帧 + for (set::iterator sit = mspChildrens.begin(), send = mspChildrens.end(); sit != send; sit++) + { + KeyFrame *pKF = *sit; + // 跳过无效的子关键帧 + if (pKF->isBad()) + continue; + + // Check if a parent candidate is connected to the keyframe + // Step 4.2 子关键帧遍历每一个与它共视的关键帧 + vector vpConnected = pKF->GetVectorCovisibleKeyFrames(); + for (size_t i = 0, iend = vpConnected.size(); i < iend; i++) + { + // sParentCandidates 中刚开始存的是“爷爷” + for (set::iterator spcit = sParentCandidates.begin(), spcend = sParentCandidates.end(); spcit != spcend; spcit++) + { + if (vpConnected[i]->mnId == (*spcit)->mnId) + { + int w = pKF->GetWeight(vpConnected[i]); + // 寻找并更新权值最大的那个共视关系 + if (w > max) + { + pC = pKF; // 子关键帧 + pP = vpConnected[i]; // 目前和子关键帧具有最大权值的关键帧（将来的父关键帧） + max = w; // 这个最大的权值 + bContinue = true; // 说明子节点找到了可以作为其新父关键帧的帧 + } + } + } + } + } + + // Step 4.4 如果在上面的过程中找到了新的父节点 + // 下面代码应该放到遍历子关键帧循环中? + // 回答：不需要！这里while循环还没退出，会使用更新的sParentCandidates + if (bContinue) + { + // 因为父节点死了，并且子节点找到了新的父节点，就把它更新为自己的父节点 + pC->ChangeParent(pP); + // 因为子节点找到了新的父节点并更新了父节点，那么该子节点升级，作为其它子节点的备选父节点 + sParentCandidates.insert(pC); + // 该子节点处理完毕，删掉 + mspChildrens.erase(pC); + } + else + break; + } + + // If a children has no covisibility links with any parent candidate, assign to the original parent of this KF + // Step 4.5 如果还有子节点没有找到新的父节点 + if (!mspChildrens.empty()) + { + for (set::iterator sit = mspChildrens.begin(); sit != mspChildrens.end(); sit++) + { + // 直接把父节点的父节点作为自己的父节点 即对于这些子节点来说,他们的新的父节点其实就是自己的爷爷节点 + (*sit)->ChangeParent(mpParent); + } + } + + if (mpParent) + { + mpParent->EraseChild(this); + // 如果当前的关键帧要被删除的话就要计算这个,表示原父关键帧到当前关键帧的位姿变换 + // 注意在这个删除的过程中,其实并没有将当前关键帧中存储的父关键帧的指针删除掉 + mTcp = mTcw * mpParent->GetPoseInverse(); + } + // 标记当前关键帧已经死了 + mbBad = true; + } + + mpMap->EraseKeyFrame(this); + mpKeyFrameDB->erase(this); +} + +// 返回当前关键帧是否已经完蛋了 +bool KeyFrame::isBad() +{ + unique_lock lock(mMutexConnections); + return mbBad; +} + +// 删除当前关键帧和指定关键帧之间的共视关系 +void KeyFrame::EraseConnection(KeyFrame *pKF) +{ + // 其实这个应该表示是否真的是有共视关系 + bool bUpdate = false; + { + unique_lock lock(mMutexConnections); + if (mConnectedKeyFrameWeights.count(pKF)) + { + mConnectedKeyFrameWeights.erase(pKF); + bUpdate = true; + } + } + + // 如果是真的有共视关系,那么删除之后就要更新共视关系 + if (bUpdate) + UpdateBestCovisibles(); +} + +// 获取某个特征点的邻域中的特征点id,其实这个和 Frame.cc 中的那个函数基本上都是一致的; r为边长（半径） +vector KeyFrame::GetFeaturesInArea(const float &x, const float &y, const float &r, const bool bRight) const +{ + vector vIndices; + vIndices.reserve(N); + + // 计算要搜索的cell的范围 + float factorX = r; + float factorY = r; + + // floor向下取整，mfGridElementWidthInv 为每个像素占多少个格子 + const int nMinCellX = max(0, (int)floor((x - mnMinX - factorX) * mfGridElementWidthInv)); + if (nMinCellX >= mnGridCols) + return vIndices; + + // ceil向上取整 + const int nMaxCellX = min((int)mnGridCols - 1, (int)ceil((x - mnMinX + factorX) * mfGridElementWidthInv)); + if (nMaxCellX < 0) + return vIndices; + + const int nMinCellY = max(0, (int)floor((y - mnMinY - factorY) * mfGridElementHeightInv)); + if (nMinCellY >= mnGridRows) + return vIndices; + + const int nMaxCellY = min((int)mnGridRows - 1, (int)ceil((y - mnMinY + factorY) * mfGridElementHeightInv)); + if (nMaxCellY < 0) + return vIndices; + + // 遍历每个cell,取出其中每个cell中的点,并且每个点都要计算是否在邻域内 + for (int ix = nMinCellX; ix <= nMaxCellX; ix++) + { + for (int iy = nMinCellY; iy <= nMaxCellY; iy++) + { + const vector vCell = (!bRight) ? mGrid[ix][iy] : mGridRight[ix][iy]; + for (size_t j = 0, jend = vCell.size(); j < jend; j++) + { + const cv::KeyPoint &kpUn = (NLeft == -1) ? mvKeysUn[vCell[j]] + : (!bRight) ? mvKeys[vCell[j]] + : mvKeysRight[vCell[j]]; + const float distx = kpUn.pt.x - x; + const float disty = kpUn.pt.y - y; + + if (fabs(distx) < r && fabs(disty) < r) + vIndices.push_back(vCell[j]); + } + } + } + + return vIndices; +} + +// 判断某个点是否在当前关键帧的图像中 +bool KeyFrame::IsInImage(const float &x, const float &y) const +{ + return (x >= mnMinX && x < mnMaxX && y >= mnMinY && y < mnMaxY); +} + +/** + * @brief 在双目和RGBD情况下将特征点反投影到空间中得到世界坐标系下三维点 + * + * @param[in] i 第i个特征点 + * @return Eigen::Vector3f 返回世界坐标系下三维点 + */ +bool KeyFrame::UnprojectStereo(int i, Eigen::Vector3f &x3D) +{ + const float z = mvDepth[i]; + if (z > 0) + { + // 由2维图像反投影到相机坐标系 + // 双目中mvDepth是在ComputeStereoMatches函数中求取的，rgbd中是直接测量的 + const float u = mvKeys[i].pt.x; + const float v = mvKeys[i].pt.y; + const float x = (u - cx) * z * invfx; + const float y = (v - cy) * z * invfy; + Eigen::Vector3f x3Dc(x, y, z); + + unique_lock lock(mMutexPose); + x3D = mRwc * x3Dc + mTwc.translation(); + return true; + } + else + return false; +} + +// Compute Scene Depth (q=2 median). Used in monocular. 评估当前关键帧场景深度，q=2表示中值. 只是在单目情况下才会使用 +// 其实过程就是对当前关键帧下所有地图点的深度进行从小到大排序,返回距离头部其中1/q处的深度值作为当前场景的平均深度 +float KeyFrame::ComputeSceneMedianDepth(const int q) +{ + if (N == 0) + return -1.0; + + vector vpMapPoints; + Eigen::Matrix3f Rcw; + Eigen::Vector3f tcw; + { + unique_lock lock(mMutexFeatures); + unique_lock lock2(mMutexPose); + vpMapPoints = mvpMapPoints; + tcw = mTcw.translation(); + Rcw = mRcw; + } + + vector vDepths; + vDepths.reserve(N); + Eigen::Matrix Rcw2 = Rcw.row(2); + float zcw = tcw(2); + // 遍历每一个地图点,计算并保存其在当前关键帧下的深度 + for (int i = 0; i < N; i++) + { + if (mvpMapPoints[i]) + { + MapPoint *pMP = mvpMapPoints[i]; + Eigen::Vector3f x3Dw = pMP->GetWorldPos(); + float z = Rcw2.dot(x3Dw) + zcw; // (R*x3Dw+t)的第三行，即z + vDepths.push_back(z); + } + } + + sort(vDepths.begin(), vDepths.end()); + + return vDepths[(vDepths.size() - 1) / q]; +} + +void KeyFrame::SetNewBias(const IMU::Bias &b) +{ + unique_lock lock(mMutexPose); + mImuBias = b; + if (mpImuPreintegrated) + mpImuPreintegrated->SetNewBias(b); +} + +Eigen::Vector3f KeyFrame::GetGyroBias() +{ + unique_lock lock(mMutexPose); + return Eigen::Vector3f(mImuBias.bwx, mImuBias.bwy, mImuBias.bwz); +} + +Eigen::Vector3f KeyFrame::GetAccBias() +{ + unique_lock lock(mMutexPose); + return Eigen::Vector3f(mImuBias.bax, mImuBias.bay, mImuBias.baz); +} + +IMU::Bias KeyFrame::GetImuBias() +{ + unique_lock lock(mMutexPose); + return mImuBias; +} + +Map *KeyFrame::GetMap() +{ + unique_lock lock(mMutexMap); + return mpMap; +} + +void KeyFrame::UpdateMap(Map *pMap) +{ + unique_lock lock(mMutexMap); + mpMap = pMap; +} + +void KeyFrame::PreSave(set &spKF, set &spMP, set &spCam) +{ + // Save the id of each MapPoint in this KF, there can be null pointer in the vector + mvBackupMapPointsId.clear(); + mvBackupMapPointsId.reserve(N); + for (int i = 0; i < N; ++i) + { + + if (mvpMapPoints[i] && spMP.find(mvpMapPoints[i]) != spMP.end()) // Checks if the element is not null + mvBackupMapPointsId.push_back(mvpMapPoints[i]->mnId); + else // If the element is null his value is -1 because all the id are positives + mvBackupMapPointsId.push_back(-1); + } + // Save the id of each connected KF with it weight + mBackupConnectedKeyFrameIdWeights.clear(); + for (std::map::const_iterator it = mConnectedKeyFrameWeights.begin(), end = mConnectedKeyFrameWeights.end(); it != end; ++it) + { + if (spKF.find(it->first) != spKF.end()) + mBackupConnectedKeyFrameIdWeights[it->first->mnId] = it->second; + } + + // Save the parent id + mBackupParentId = -1; + if (mpParent && spKF.find(mpParent) != spKF.end()) + mBackupParentId = mpParent->mnId; + + // Save the id of the childrens KF + mvBackupChildrensId.clear(); + mvBackupChildrensId.reserve(mspChildrens.size()); + for (KeyFrame *pKFi : mspChildrens) + { + if (spKF.find(pKFi) != spKF.end()) + mvBackupChildrensId.push_back(pKFi->mnId); + } + + // Save the id of the loop edge KF + mvBackupLoopEdgesId.clear(); + mvBackupLoopEdgesId.reserve(mspLoopEdges.size()); + for (KeyFrame *pKFi : mspLoopEdges) + { + if (spKF.find(pKFi) != spKF.end()) + mvBackupLoopEdgesId.push_back(pKFi->mnId); + } + + // Save the id of the merge edge KF + mvBackupMergeEdgesId.clear(); + mvBackupMergeEdgesId.reserve(mspMergeEdges.size()); + for (KeyFrame *pKFi : mspMergeEdges) + { + if (spKF.find(pKFi) != spKF.end()) + mvBackupMergeEdgesId.push_back(pKFi->mnId); + } + + // Camera data + mnBackupIdCamera = -1; + if (mpCamera && spCam.find(mpCamera) != spCam.end()) + mnBackupIdCamera = mpCamera->GetId(); + + mnBackupIdCamera2 = -1; + if (mpCamera2 && spCam.find(mpCamera2) != spCam.end()) + mnBackupIdCamera2 = mpCamera2->GetId(); + + // Inertial data + mBackupPrevKFId = -1; + if (mPrevKF && spKF.find(mPrevKF) != spKF.end()) + mBackupPrevKFId = mPrevKF->mnId; + + mBackupNextKFId = -1; + if (mNextKF && spKF.find(mNextKF) != spKF.end()) + mBackupNextKFId = mNextKF->mnId; + + if (mpImuPreintegrated) + mBackupImuPreintegrated.CopyFrom(mpImuPreintegrated); +} + +void KeyFrame::PostLoad(map &mpKFid, map &mpMPid, map &mpCamId) +{ + // Rebuild the empty variables + + // Pose + SetPose(mTcw); + + mTrl = mTlr.inverse(); + + // Reference reconstruction + // Each MapPoint sight from this KeyFrame + mvpMapPoints.clear(); + mvpMapPoints.resize(N); + for (int i = 0; i < N; ++i) + { + if (mvBackupMapPointsId[i] != -1) + mvpMapPoints[i] = mpMPid[mvBackupMapPointsId[i]]; + else + mvpMapPoints[i] = static_cast(NULL); + } + + // Conected KeyFrames with him weight + mConnectedKeyFrameWeights.clear(); + for (map::const_iterator it = mBackupConnectedKeyFrameIdWeights.begin(), end = mBackupConnectedKeyFrameIdWeights.end(); + it != end; ++it) + { + KeyFrame *pKFi = mpKFid[it->first]; + mConnectedKeyFrameWeights[pKFi] = it->second; + } + + // Restore parent KeyFrame + if (mBackupParentId >= 0) + mpParent = mpKFid[mBackupParentId]; + + // KeyFrame childrens + mspChildrens.clear(); + for (vector::const_iterator it = mvBackupChildrensId.begin(), end = mvBackupChildrensId.end(); it != end; ++it) + { + mspChildrens.insert(mpKFid[*it]); + } + + // Loop edge KeyFrame + mspLoopEdges.clear(); + for (vector::const_iterator it = mvBackupLoopEdgesId.begin(), end = mvBackupLoopEdgesId.end(); it != end; ++it) + { + mspLoopEdges.insert(mpKFid[*it]); + } + + // Merge edge KeyFrame + mspMergeEdges.clear(); + for (vector::const_iterator it = mvBackupMergeEdgesId.begin(), end = mvBackupMergeEdgesId.end(); it != end; ++it) + { + mspMergeEdges.insert(mpKFid[*it]); + } + + // Camera data + if (mnBackupIdCamera >= 0) + { + mpCamera = mpCamId[mnBackupIdCamera]; + } + else + { + cout << "ERROR: There is not a main camera in KF " << mnId << endl; + } + if (mnBackupIdCamera2 >= 0) + { + mpCamera2 = mpCamId[mnBackupIdCamera2]; + } + + // Inertial data + if (mBackupPrevKFId != -1) + { + mPrevKF = mpKFid[mBackupPrevKFId]; + } + if (mBackupNextKFId != -1) + { + mNextKF = mpKFid[mBackupNextKFId]; + } + mpImuPreintegrated = &mBackupImuPreintegrated; + + // Remove all backup container + mvBackupMapPointsId.clear(); + mBackupConnectedKeyFrameIdWeights.clear(); + mvBackupChildrensId.clear(); + mvBackupLoopEdgesId.clear(); + + UpdateBestCovisibles(); +} + +bool KeyFrame::ProjectPointDistort(MapPoint *pMP, cv::Point2f &kp, float &u, float &v) +{ + + // 3D in absolute coordinates + Eigen::Vector3f P = pMP->GetWorldPos(); + + // 3D in camera coordinates + Eigen::Vector3f Pc = mRcw * P + mTcw.translation(); + float &PcX = Pc(0); + float &PcY = Pc(1); + float &PcZ = Pc(2); + + // Check positive depth + if (PcZ < 0.0f) + { + cout << "Negative depth: " << PcZ << endl; + return false; + } + + // Project in image and check it is not outside + float invz = 1.0f / PcZ; + u = fx * PcX * invz + cx; + v = fy * PcY * invz + cy; + + // cout << "c"; + + if (u < mnMinX || u > mnMaxX) + return false; + if (v < mnMinY || v > mnMaxY) + return false; + + float x = (u - cx) * invfx; + float y = (v - cy) * invfy; + float r2 = x * x + y * y; + float k1 = mDistCoef.at(0); + float k2 = mDistCoef.at(1); + float p1 = mDistCoef.at(2); + float p2 = mDistCoef.at(3); + float k3 = 0; + if (mDistCoef.total() == 5) + { + k3 = mDistCoef.at(4); + } + + // Radial distorsion + float x_distort = x * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2); + float y_distort = y * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2); + + // Tangential distorsion + x_distort = x_distort + (2 * p1 * x * y + p2 * (r2 + 2 * x * x)); + y_distort = y_distort + (p1 * (r2 + 2 * y * y) + 2 * p2 * x * y); + + float u_distort = x_distort * fx + cx; + float v_distort = y_distort * fy + cy; + + u = u_distort; + v = v_distort; + + kp = cv::Point2f(u, v); + + return true; +} + +bool KeyFrame::ProjectPointUnDistort(MapPoint *pMP, cv::Point2f &kp, float &u, float &v) +{ + + // 3D in absolute coordinates + Eigen::Vector3f P = pMP->GetWorldPos(); + + // 3D in camera coordinates + Eigen::Vector3f Pc = mRcw * P + mTcw.translation(); + float &PcX = Pc(0); + float &PcY = Pc(1); + float &PcZ = Pc(2); + + // Check positive depth + if (PcZ < 0.0f) + { + cout << "Negative depth: " << PcZ << endl; + return false; + } + + // Project in image and check it is not outside + const float invz = 1.0f / PcZ; + u = fx * PcX * invz + cx; + v = fy * PcY * invz + cy; + + if (u < mnMinX || u > mnMaxX) + return false; + if (v < mnMinY || v > mnMaxY) + return false; + + kp = cv::Point2f(u, v); + + return true; +} + +Sophus::SE3f KeyFrame::GetRelativePoseTrl() +{ + unique_lock lock(mMutexPose); + return mTrl; +} + +Sophus::SE3f KeyFrame::GetRelativePoseTlr() +{ + unique_lock lock(mMutexPose); + return mTlr; +} + +Sophus::SE3 KeyFrame::GetRightPose() +{ + unique_lock lock(mMutexPose); + + return mTrl * mTcw; +} + +Sophus::SE3 KeyFrame::GetRightPoseInverse() +{ + unique_lock lock(mMutexPose); + + return mTwc * mTlr; +} + +Eigen::Vector3f KeyFrame::GetRightCameraCenter() +{ + unique_lock lock(mMutexPose); + + return (mTwc * mTlr).translation(); +} + +Eigen::Matrix KeyFrame::GetRightRotation() +{ + unique_lock lock(mMutexPose); + + return (mTrl.so3() * mTcw.so3()).matrix(); +} + +Eigen::Vector3f KeyFrame::GetRightTranslation() +{ + unique_lock lock(mMutexPose); + return (mTrl * mTcw).translation(); +} + +void KeyFrame::SetORBVocabulary(ORBVocabulary *pORBVoc) +{ + mpORBvocabulary = pORBVoc; +} + +void KeyFrame::SetKeyFrameDatabase(KeyFrameDatabase *pKFDB) +{ + mpKeyFrameDB = pKFDB; +} + +} // namespace ORB_SLAM diff --git a/ORB-SLAM3-UESTC/Workspace/src/KeyFrameDatabase.cc b/ORB-SLAM3-UESTC/Workspace/src/KeyFrameDatabase.cc new file mode 100644 index 0000000..1b93049 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/KeyFrameDatabase.cc @@ -0,0 +1,950 @@ +/** + * This file is part of ORB-SLAM3 + * + * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * + * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public + * License as published by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even + * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with ORB-SLAM3. + * If not, see . + */ + +#include "KeyFrameDatabase.h" + +#include "KeyFrame.h" +#include "Thirdparty/DBoW2/DBoW2/BowVector.h" + +#include + +using namespace std; + +namespace ORB_SLAM3 +{ + +// 构造函数 +KeyFrameDatabase::KeyFrameDatabase(const ORBVocabulary &voc) : mpVoc(&voc) +{ + mvInvertedFile.resize(voc.size()); +} + +// 根据关键帧的BoW，更新数据库的倒排索引 +void KeyFrameDatabase::add(KeyFrame *pKF) +{ + unique_lock lock(mMutex); + + // 为每一个word添加该KeyFrame + for (DBoW2::BowVector::const_iterator vit = pKF->mBowVec.begin(), vend = pKF->mBowVec.end(); vit != vend; vit++) + mvInvertedFile[vit->first].push_back(pKF); +} + +// 关键帧被删除后，更新数据库的倒排索引 +void KeyFrameDatabase::erase(KeyFrame *pKF) +{ + unique_lock lock(mMutex); + + // Erase elements in the Inverse File for the entry + // 每一个KeyFrame包含多个words，遍历mvInvertedFile中的这些words，然后在word中删除该KeyFrame + for (DBoW2::BowVector::const_iterator vit = pKF->mBowVec.begin(), vend = pKF->mBowVec.end(); vit != vend; vit++) + { + // List of keyframes that share the word + list &lKFs = mvInvertedFile[vit->first]; + + for (list::iterator lit = lKFs.begin(), lend = lKFs.end(); lit != lend; lit++) + { + if (pKF == *lit) + { + lKFs.erase(lit); + break; + } + } + } +} + +// 清空关键帧数据库 +void KeyFrameDatabase::clear() +{ + mvInvertedFile.clear(); + mvInvertedFile.resize(mpVoc->size()); +} + +void KeyFrameDatabase::clearMap(Map *pMap) +{ + unique_lock lock(mMutex); + + // Erase elements in the Inverse File for the entry + for (std::vector>::iterator vit = mvInvertedFile.begin(), vend = mvInvertedFile.end(); vit != vend; vit++) + { + // List of keyframes that share the word + list &lKFs = *vit; + + for (list::iterator lit = lKFs.begin(), lend = lKFs.end(); lit != lend;) + { + KeyFrame *pKFi = *lit; + if (pMap == pKFi->GetMap()) + { + lit = lKFs.erase(lit); + // Dont delete the KF because the class Map clean all the KF when it is destroyed + } + else + { + ++lit; + } + } + } +} + +/** + * @brief 在闭环检测中找到与该关键帧可能闭环的关键帧（注意不和当前帧连接） + * Step 1：找出和当前帧具有公共单词的所有关键帧，不包括与当前帧连接（也就是共视）的关键帧 + * Step 2：只和具有共同单词较多的（最大数目的80%以上）关键帧进行相似度计算 + * Step 3：计算上述候选帧对应的共视关键帧组的总得分，只取最高组得分75%以上的组 + * Step 4：得到上述组中分数最高的关键帧作为闭环候选关键帧 + * @param[in] pKF 需要闭环检测的关键帧 + * @param[in] minScore 候选闭环关键帧帧和当前关键帧的BoW相似度至少要大于minScore + * @return vector 闭环候选关键帧 + */ +vector KeyFrameDatabase::DetectLoopCandidates(KeyFrame *pKF, float minScore) +{ + // 取出与当前关键帧相连（>15个共视地图点）的所有关键帧，这些相连关键帧都是局部相连，在闭环检测的时候将被剔除 + // 相连关键帧定义见 KeyFrame::UpdateConnections() + set spConnectedKeyFrames = pKF->GetConnectedKeyFrames(); + // 用于保存可能与当前关键帧形成闭环的候选帧（只要有相同的word，且不属于局部相连（共视）帧） + list lKFsSharingWords; + + // Search all keyframes that share a word with current keyframes + // Discard keyframes connected to the query keyframe + // Step 1：找出和当前帧具有公共单词的所有关键帧，不包括与当前帧连接（也就是共视）的关键帧 + { + unique_lock lock(mMutex); + + // words是检测图像是否匹配的枢纽，遍历该pKF的每一个word + // mBowVec 内部实际存储的是std::map + // WordId 和 WordValue 表示Word在叶子中的id 和权重 + for (DBoW2::BowVector::const_iterator vit = pKF->mBowVec.begin(), vend = pKF->mBowVec.end(); vit != vend; vit++) + { + // 提取所有包含该word的KeyFrame + list &lKFs = mvInvertedFile[vit->first]; + // 然后对这些关键帧展开遍历 + for (list::iterator lit = lKFs.begin(), lend = lKFs.end(); lit != lend; lit++) + { + KeyFrame *pKFi = *lit; + if (pKFi->GetMap() == pKF->GetMap()) // For consider a loop candidate it a candidate it must be in the same map + { + if (pKFi->mnLoopQuery != pKF->mnId) + { + // 还没有标记为pKF的闭环候选帧 + pKFi->mnLoopWords = 0; + // 和当前关键帧共视的话不作为闭环候选帧 + if (!spConnectedKeyFrames.count(pKFi)) + { + // 没有共视就标记作为闭环候选关键帧，放到lKFsSharingWords里 + pKFi->mnLoopQuery = pKF->mnId; + lKFsSharingWords.push_back(pKFi); + } + } + pKFi->mnLoopWords++; // 记录pKFi与pKF具有相同word的个数 + } + } + } + } + // 如果没有关键帧和这个关键帧具有相同的单词,那么就返回空 + if (lKFsSharingWords.empty()) + return vector(); + + list> lScoreAndMatch; + + // Only compare against those keyframes that share enough words + // Step 2：统计上述所有闭环候选帧中与当前帧具有共同单词最多的单词数，用来决定相对阈值 + int maxCommonWords = 0; + for (list::iterator lit = lKFsSharingWords.begin(), lend = lKFsSharingWords.end(); lit != lend; lit++) + { + if ((*lit)->mnLoopWords > maxCommonWords) + maxCommonWords = (*lit)->mnLoopWords; + } + + // 确定最小公共单词数为最大公共单词数目的0.8倍 + int minCommonWords = maxCommonWords * 0.8f; + + int nscores = 0; + + // Compute similarity score. Retain the matches whose score is higher than minScore + // Step 3：遍历上述所有闭环候选帧，挑选出共有单词数大于minCommonWords且单词匹配度大于minScore存入lScoreAndMatch + for (list::iterator lit = lKFsSharingWords.begin(), lend = lKFsSharingWords.end(); lit != lend; lit++) + { + KeyFrame *pKFi = *lit; + + // pKF只和具有共同单词较多（大于minCommonWords）的关键帧进行比较 + if (pKFi->mnLoopWords > minCommonWords) + { + nscores++; + + // 用mBowVec来计算两者的相似度得分 + float si = mpVoc->score(pKF->mBowVec, pKFi->mBowVec); + + pKFi->mLoopScore = si; + if (si >= minScore) + lScoreAndMatch.push_back(make_pair(si, pKFi)); + } + } + + // 如果没有超过指定相似度阈值的，那么也就直接跳过去 + if (lScoreAndMatch.empty()) + return vector(); + + list> lAccScoreAndMatch; + float bestAccScore = minScore; + + // Lets now accumulate score by covisibility + // 单单计算当前帧和某一关键帧的相似性是不够的，这里将与关键帧相连（权值最高，共视程度最高）的前十个关键帧归为一组，计算累计得分 + // Step 4：计算上述候选帧对应的共视关键帧组的总得分，得到最高组得分bestAccScore，并以此决定阈值minScoreToRetain + for (list>::iterator it = lScoreAndMatch.begin(), itend = lScoreAndMatch.end(); it != itend; it++) + { + KeyFrame *pKFi = it->second; + vector vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10); + + float bestScore = it->first; // 该组最高分数 + float accScore = it->first; // 该组累计得分 + KeyFrame *pBestKF = pKFi; // 该组最高分数对应的关键帧 + for (vector::iterator vit = vpNeighs.begin(), vend = vpNeighs.end(); vit != vend; vit++) + { + KeyFrame *pKF2 = *vit; + // 只有pKF2也在闭环候选帧中，且公共单词数超过最小要求，才能贡献分数 + if (pKF2->mnLoopQuery == pKF->mnId && pKF2->mnLoopWords > minCommonWords) + { + accScore += pKF2->mLoopScore; + // 统计得到组里分数最高的关键帧 + if (pKF2->mLoopScore > bestScore) + { + pBestKF = pKF2; + bestScore = pKF2->mLoopScore; + } + } + } + + lAccScoreAndMatch.push_back(make_pair(accScore, pBestKF)); + // 记录所有组中组得分最高的组，用于确定相对阈值 + if (accScore > bestAccScore) + bestAccScore = accScore; + } + + // Return all those keyframes with a score higher than 0.75*bestScore + // 所有组中最高得分的0.75倍，作为最低阈值 + float minScoreToRetain = 0.75f * bestAccScore; + + set spAlreadyAddedKF; + vector vpLoopCandidates; + vpLoopCandidates.reserve(lAccScoreAndMatch.size()); + + // Step 5：只取组得分大于阈值的组，得到组中分数最高的关键帧作为闭环候选关键帧 + for (list>::iterator it = lAccScoreAndMatch.begin(), itend = lAccScoreAndMatch.end(); it != itend; it++) + { + if (it->first > minScoreToRetain) + { + KeyFrame *pKFi = it->second; + // spAlreadyAddedKF 是为了防止重复添加 + if (!spAlreadyAddedKF.count(pKFi)) + { + vpLoopCandidates.push_back(pKFi); + spAlreadyAddedKF.insert(pKFi); + } + } + } + + return vpLoopCandidates; +} + +void KeyFrameDatabase::DetectCandidates(KeyFrame *pKF, float minScore, vector &vpLoopCand, vector &vpMergeCand) +{ + set spConnectedKeyFrames = pKF->GetConnectedKeyFrames(); + list lKFsSharingWordsLoop, lKFsSharingWordsMerge; + + // Search all keyframes that share a word with current keyframes + // Discard keyframes connected to the query keyframe + { + unique_lock lock(mMutex); + + for (DBoW2::BowVector::const_iterator vit = pKF->mBowVec.begin(), vend = pKF->mBowVec.end(); vit != vend; vit++) + { + list &lKFs = mvInvertedFile[vit->first]; + + for (list::iterator lit = lKFs.begin(), lend = lKFs.end(); lit != lend; lit++) + { + KeyFrame *pKFi = *lit; + if (pKFi->GetMap() == pKF->GetMap()) // For consider a loop candidate it a candidate it must be in the same map + { + if (pKFi->mnLoopQuery != pKF->mnId) + { + pKFi->mnLoopWords = 0; + if (!spConnectedKeyFrames.count(pKFi)) + { + pKFi->mnLoopQuery = pKF->mnId; + lKFsSharingWordsLoop.push_back(pKFi); + } + } + pKFi->mnLoopWords++; + } + else if (!pKFi->GetMap()->IsBad()) + { + if (pKFi->mnMergeQuery != pKF->mnId) + { + pKFi->mnMergeWords = 0; + if (!spConnectedKeyFrames.count(pKFi)) + { + pKFi->mnMergeQuery = pKF->mnId; + lKFsSharingWordsMerge.push_back(pKFi); + } + } + pKFi->mnMergeWords++; + } + } + } + } + + if (lKFsSharingWordsLoop.empty() && lKFsSharingWordsMerge.empty()) + return; + + if (!lKFsSharingWordsLoop.empty()) + { + list> lScoreAndMatch; + + // Only compare against those keyframes that share enough words + int maxCommonWords = 0; + for (list::iterator lit = lKFsSharingWordsLoop.begin(), lend = lKFsSharingWordsLoop.end(); lit != lend; lit++) + { + if ((*lit)->mnLoopWords > maxCommonWords) + maxCommonWords = (*lit)->mnLoopWords; + } + + int minCommonWords = maxCommonWords * 0.8f; + + int nscores = 0; + + // Compute similarity score. Retain the matches whose score is higher than minScore + for (list::iterator lit = lKFsSharingWordsLoop.begin(), lend = lKFsSharingWordsLoop.end(); lit != lend; lit++) + { + KeyFrame *pKFi = *lit; + + if (pKFi->mnLoopWords > minCommonWords) + { + nscores++; + + float si = mpVoc->score(pKF->mBowVec, pKFi->mBowVec); + + pKFi->mLoopScore = si; + if (si >= minScore) + lScoreAndMatch.push_back(make_pair(si, pKFi)); + } + } + + if (!lScoreAndMatch.empty()) + { + list> lAccScoreAndMatch; + float bestAccScore = minScore; + + // Lets now accumulate score by covisibility + for (list>::iterator it = lScoreAndMatch.begin(), itend = lScoreAndMatch.end(); it != itend; it++) + { + KeyFrame *pKFi = it->second; + vector vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10); + + float bestScore = it->first; + float accScore = it->first; + KeyFrame *pBestKF = pKFi; + for (vector::iterator vit = vpNeighs.begin(), vend = vpNeighs.end(); vit != vend; vit++) + { + KeyFrame *pKF2 = *vit; + if (pKF2->mnLoopQuery == pKF->mnId && pKF2->mnLoopWords > minCommonWords) + { + accScore += pKF2->mLoopScore; + if (pKF2->mLoopScore > bestScore) + { + pBestKF = pKF2; + bestScore = pKF2->mLoopScore; + } + } + } + + lAccScoreAndMatch.push_back(make_pair(accScore, pBestKF)); + if (accScore > bestAccScore) + bestAccScore = accScore; + } + + // Return all those keyframes with a score higher than 0.75*bestScore + float minScoreToRetain = 0.75f * bestAccScore; + + set spAlreadyAddedKF; + vpLoopCand.reserve(lAccScoreAndMatch.size()); + + for (list>::iterator it = lAccScoreAndMatch.begin(), itend = lAccScoreAndMatch.end(); it != itend; it++) + { + if (it->first > minScoreToRetain) + { + KeyFrame *pKFi = it->second; + if (!spAlreadyAddedKF.count(pKFi)) + { + vpLoopCand.push_back(pKFi); + spAlreadyAddedKF.insert(pKFi); + } + } + } + } + } + + if (!lKFsSharingWordsMerge.empty()) + { + // cout << "BoW candidates: " << lKFsSharingWordsMerge.size() << endl; + list> lScoreAndMatch; + + // Only compare against those keyframes that share enough words + int maxCommonWords = 0; + for (list::iterator lit = lKFsSharingWordsMerge.begin(), lend = lKFsSharingWordsMerge.end(); lit != lend; lit++) + { + if ((*lit)->mnMergeWords > maxCommonWords) + maxCommonWords = (*lit)->mnMergeWords; + } + // cout << "Max common words: " << maxCommonWords << endl; + + int minCommonWords = maxCommonWords * 0.8f; + + int nscores = 0; + + // Compute similarity score. Retain the matches whose score is higher than minScore + for (list::iterator lit = lKFsSharingWordsMerge.begin(), lend = lKFsSharingWordsMerge.end(); lit != lend; lit++) + { + KeyFrame *pKFi = *lit; + + if (pKFi->mnMergeWords > minCommonWords) + { + nscores++; + + float si = mpVoc->score(pKF->mBowVec, pKFi->mBowVec); + // cout << "KF score: " << si << endl; + + pKFi->mMergeScore = si; + if (si >= minScore) + lScoreAndMatch.push_back(make_pair(si, pKFi)); + } + } + // cout << "BoW candidates2: " << lScoreAndMatch.size() << endl; + + if (!lScoreAndMatch.empty()) + { + list> lAccScoreAndMatch; + float bestAccScore = minScore; + + // Lets now accumulate score by covisibility + for (list>::iterator it = lScoreAndMatch.begin(), itend = lScoreAndMatch.end(); it != itend; it++) + { + KeyFrame *pKFi = it->second; + vector vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10); + + float bestScore = it->first; + float accScore = it->first; + KeyFrame *pBestKF = pKFi; + for (vector::iterator vit = vpNeighs.begin(), vend = vpNeighs.end(); vit != vend; vit++) + { + KeyFrame *pKF2 = *vit; + if (pKF2->mnMergeQuery == pKF->mnId && pKF2->mnMergeWords > minCommonWords) + { + accScore += pKF2->mMergeScore; + if (pKF2->mMergeScore > bestScore) + { + pBestKF = pKF2; + bestScore = pKF2->mMergeScore; + } + } + } + + lAccScoreAndMatch.push_back(make_pair(accScore, pBestKF)); + if (accScore > bestAccScore) + bestAccScore = accScore; + } + + // Return all those keyframes with a score higher than 0.75*bestScore + float minScoreToRetain = 0.75f * bestAccScore; + + // cout << "Min score to retain: " << minScoreToRetain << endl; + + set spAlreadyAddedKF; + vpMergeCand.reserve(lAccScoreAndMatch.size()); + + for (list>::iterator it = lAccScoreAndMatch.begin(), itend = lAccScoreAndMatch.end(); it != itend; it++) + { + if (it->first > minScoreToRetain) + { + KeyFrame *pKFi = it->second; + if (!spAlreadyAddedKF.count(pKFi)) + { + vpMergeCand.push_back(pKFi); + spAlreadyAddedKF.insert(pKFi); + } + } + } + // cout << "Candidates: " << vpMergeCand.size() << endl; + } + } + + //---- + for (DBoW2::BowVector::const_iterator vit = pKF->mBowVec.begin(), vend = pKF->mBowVec.end(); vit != vend; vit++) + { + list &lKFs = mvInvertedFile[vit->first]; + + for (list::iterator lit = lKFs.begin(), lend = lKFs.end(); lit != lend; lit++) + { + KeyFrame *pKFi = *lit; + pKFi->mnLoopQuery = -1; + pKFi->mnMergeQuery = -1; + } + } +} + +void KeyFrameDatabase::DetectBestCandidates(KeyFrame *pKF, vector &vpLoopCand, vector &vpMergeCand, int nMinWords) +{ + list lKFsSharingWords; + set spConnectedKF; + + // Search all keyframes that share a word with current frame + { + unique_lock lock(mMutex); + + spConnectedKF = pKF->GetConnectedKeyFrames(); + + for (DBoW2::BowVector::const_iterator vit = pKF->mBowVec.begin(), vend = pKF->mBowVec.end(); vit != vend; vit++) + { + list &lKFs = mvInvertedFile[vit->first]; + + for (list::iterator lit = lKFs.begin(), lend = lKFs.end(); lit != lend; lit++) + { + KeyFrame *pKFi = *lit; + if (spConnectedKF.find(pKFi) != spConnectedKF.end()) + { + continue; + } + if (pKFi->mnPlaceRecognitionQuery != pKF->mnId) + { + pKFi->mnPlaceRecognitionWords = 0; + pKFi->mnPlaceRecognitionQuery = pKF->mnId; + lKFsSharingWords.push_back(pKFi); + } + pKFi->mnPlaceRecognitionWords++; + } + } + } + if (lKFsSharingWords.empty()) + return; + + // Only compare against those keyframes that share enough words + int maxCommonWords = 0; + for (list::iterator lit = lKFsSharingWords.begin(), lend = lKFsSharingWords.end(); lit != lend; lit++) + { + if ((*lit)->mnPlaceRecognitionWords > maxCommonWords) + maxCommonWords = (*lit)->mnPlaceRecognitionWords; + } + + int minCommonWords = maxCommonWords * 0.8f; + + if (minCommonWords < nMinWords) + { + minCommonWords = nMinWords; + } + + list> lScoreAndMatch; + + int nscores = 0; + + // Compute similarity score. + for (list::iterator lit = lKFsSharingWords.begin(), lend = lKFsSharingWords.end(); lit != lend; lit++) + { + KeyFrame *pKFi = *lit; + + if (pKFi->mnPlaceRecognitionWords > minCommonWords) + { + nscores++; + float si = mpVoc->score(pKF->mBowVec, pKFi->mBowVec); + pKFi->mPlaceRecognitionScore = si; + lScoreAndMatch.push_back(make_pair(si, pKFi)); + } + } + + if (lScoreAndMatch.empty()) + return; + + list> lAccScoreAndMatch; + float bestAccScore = 0; + + // Lets now accumulate score by covisibility + for (list>::iterator it = lScoreAndMatch.begin(), itend = lScoreAndMatch.end(); it != itend; it++) + { + KeyFrame *pKFi = it->second; + vector vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10); + + float bestScore = it->first; + float accScore = bestScore; + KeyFrame *pBestKF = pKFi; + for (vector::iterator vit = vpNeighs.begin(), vend = vpNeighs.end(); vit != vend; vit++) + { + KeyFrame *pKF2 = *vit; + if (pKF2->mnPlaceRecognitionQuery != pKF->mnId) + continue; + + accScore += pKF2->mPlaceRecognitionScore; + if (pKF2->mPlaceRecognitionScore > bestScore) + { + pBestKF = pKF2; + bestScore = pKF2->mPlaceRecognitionScore; + } + } + lAccScoreAndMatch.push_back(make_pair(accScore, pBestKF)); + if (accScore > bestAccScore) + bestAccScore = accScore; + } + + // Return all those keyframes with a score higher than 0.75*bestScore + float minScoreToRetain = 0.75f * bestAccScore; + set spAlreadyAddedKF; + vpLoopCand.reserve(lAccScoreAndMatch.size()); + vpMergeCand.reserve(lAccScoreAndMatch.size()); + for (list>::iterator it = lAccScoreAndMatch.begin(), itend = lAccScoreAndMatch.end(); it != itend; it++) + { + const float &si = it->first; + if (si > minScoreToRetain) + { + KeyFrame *pKFi = it->second; + if (!spAlreadyAddedKF.count(pKFi)) + { + if (pKF->GetMap() == pKFi->GetMap()) + { + vpLoopCand.push_back(pKFi); + } + else + { + vpMergeCand.push_back(pKFi); + } + spAlreadyAddedKF.insert(pKFi); + } + } + } +} + +bool compFirst(const pair &a, const pair &b) +{ + return a.first > b.first; +} + +/** + * @brief 找到N个融合候选N个回环候选 + * + * @param[in] pKF 当前关键帧(我们要寻找这个关键帧的回环候选帧和融合候选帧) + * @param[out] vpLoopCand 记录找到的回环候选帧 + * @param[out] vpMergeCand 记录找到的融合候选帧 + * @param[in] nNumCandidates 期望的候选数目,即回环和候选分别应该有多少个 + */ +void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector &vpLoopCand, vector &vpMergeCand, int nNumCandidates) +{ + // Step 1统计与当前关键帧有相同单词的关键帧 + list lKFsSharingWords; + // set spInsertedKFsSharing; + // 当前关键帧的共视关键帧(避免将当前关键帧的共视关键帧加入回环检测) + set spConnectedKF; + + // Search all keyframes that share a word with current frame + { + unique_lock lock(mMutex); + // 拿到当前关键帧的共视关键帧 + spConnectedKF = pKF->GetConnectedKeyFrames(); + + // 遍历当前关键帧bow向量的每个单词 + for (DBoW2::BowVector::const_iterator vit = pKF->mBowVec.begin(), vend = pKF->mBowVec.end(); vit != vend; vit++) + { // 拿到当前单词的逆向索引(所有有当前单词的关键帧) + list &lKFs = mvInvertedFile[vit->first]; + // 遍历每个有该单词的关键帧 + for (list::iterator lit = lKFs.begin(), lend = lKFs.end(); lit != lend; lit++) + { + KeyFrame *pKFi = *lit; + /*if(spConnectedKF.find(pKFi) != spConnectedKF.end()) + { + continue; + }*/ + // 如果此关键帧没有被当前关键帧访问过(防止重复添加) + if (pKFi->mnPlaceRecognitionQuery != pKF->mnId) + { + // 初始化公共单词数为0 + pKFi->mnPlaceRecognitionWords = 0; + // 如果该关键帧不是当前关键帧的共视关键帧 + if (!spConnectedKF.count(pKFi)) + { + // 标记该关键帧被当前关键帧访问到（也就是有公共单词） + pKFi->mnPlaceRecognitionQuery = pKF->mnId; + // 把当前关键帧添加到有公共单词的关键帧列表中 + lKFsSharingWords.push_back(pKFi); + } + } + // 递增该关键帧与当前关键帧的公共单词数 + pKFi->mnPlaceRecognitionWords++; + /*if(spInsertedKFsSharing.find(pKFi) == spInsertedKFsSharing.end()) + { + lKFsSharingWords.push_back(pKFi); + spInsertedKFsSharing.insert(pKFi); + }*/ + } + } + } + // 如果没有有公共单词的关键帧,直接返回 + if (lKFsSharingWords.empty()) + return; + + // Only compare against those keyframes that share enough words + // Step 2 统计所有候选帧中与当前关键帧的公共单词数最多的单词数maxCommonWords,并筛选 + int maxCommonWords = 0; + for (list::iterator lit = lKFsSharingWords.begin(), lend = lKFsSharingWords.end(); lit != lend; lit++) + { + if ((*lit)->mnPlaceRecognitionWords > maxCommonWords) + maxCommonWords = (*lit)->mnPlaceRecognitionWords; + } + // 取0.8倍为阀值 + int minCommonWords = maxCommonWords * 0.8f; + // 这里的pair是 <相似度,候选帧的指针> : 记录所有大于minCommonWords的候选帧与当前关键帧的相似度 + list> lScoreAndMatch; + // 只是个统计变量,貌似没有用到 + int nscores = 0; + + // Compute similarity score. + // 对所有大于minCommonWords的候选帧计算相似度 + + // 遍历所有有公共单词的候选帧 + for (list::iterator lit = lKFsSharingWords.begin(), lend = lKFsSharingWords.end(); lit != lend; lit++) + { + KeyFrame *pKFi = *lit; + // 如果当前帧的公共单词数大于minCommonWords + if (pKFi->mnPlaceRecognitionWords > minCommonWords) + { + nscores++; //未使用 + // 计算相似度 + float si = mpVoc->score(pKF->mBowVec, pKFi->mBowVec); + // 记录该候选帧与当前帧的相似度 + pKFi->mPlaceRecognitionScore = si; + // 记录到容器里, 每个元素是<相似度,候选帧的指针> + lScoreAndMatch.push_back(make_pair(si, pKFi)); + } + } + // 如果为空,直接返回,表示没有符合上述条件的关键帧 + if (lScoreAndMatch.empty()) + return; + // Step 3 : 用小组得分排序得到top3总分里最高分的关键帧,作为候选帧 + // 统计以组为单位的累计相似度和组内相似度最高的关键帧, 每个pair为<小组总相似度,组内相似度最高的关键帧指针> + list> lAccScoreAndMatch; + float bestAccScore = 0; + + // Lets now accumulate score by covisibility + // 变量所有被lScoreAndMatch记录的pair <相似度,候选关键帧> + for (list>::iterator it = lScoreAndMatch.begin(), itend = lScoreAndMatch.end(); it != itend; it++) + { + // 候选关键帧 + KeyFrame *pKFi = it->second; + // 与候选关键帧共视关系最好的10个关键帧 + vector vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10); + // 初始化最大相似度为该候选关键帧自己的相似度 + float bestScore = it->first; + // 初始化小组累计得分为改候选关键帧自己的相似度 + float accScore = bestScore; + // 初始化组内相似度最高的帧为该候选关键帧本身 + KeyFrame *pBestKF = pKFi; + // 遍历与当前关键帧共视关系最好的10帧 + for (vector::iterator vit = vpNeighs.begin(), vend = vpNeighs.end(); vit != vend; vit++) + { + KeyFrame *pKF2 = *vit; + // 如果该关键帧没有被当前关键帧访问过(也就是没有公共单词)则跳过 + if (pKF2->mnPlaceRecognitionQuery != pKF->mnId) + continue; + // 累加小组总分 + accScore += pKF2->mPlaceRecognitionScore; + // 如果大于组内最高分,则更新当前最高分记录 + if (pKF2->mPlaceRecognitionScore > bestScore) + { + pBestKF = pKF2; + bestScore = pKF2->mPlaceRecognitionScore; + } + } + // 统计以组为单位的累计相似度和组内相似度最高的关键帧, 每个pair为<小组总相似度,组内相似度最高的关键帧指针> + lAccScoreAndMatch.push_back(make_pair(accScore, pBestKF)); + // 统计最高得分, 这个bestAccSocre没有用到 + if (accScore > bestAccScore) + bestAccScore = accScore; + } + + // cout << "Amount of candidates: " << lAccScoreAndMatch.size() << endl; + // 按相似度从大到小排序 + lAccScoreAndMatch.sort(compFirst); + // 最后返回的变量, 记录回环的候选帧 + vpLoopCand.reserve(nNumCandidates); + // 最后返回的变量, 记录融合候选帧 + vpMergeCand.reserve(nNumCandidates); + // 避免重复添加 + set spAlreadyAddedKF; + // cout << "Candidates in score order " << endl; + // for(list >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++) + int i = 0; + list>::iterator it = lAccScoreAndMatch.begin(); + // 遍历lAccScoreAndMatch中所有的pair, 每个pair为<小组总相似度,组内相似度最高的关键帧指针>，nNumCandidates默认为3 + while (i < lAccScoreAndMatch.size() && (vpLoopCand.size() < nNumCandidates || vpMergeCand.size() < nNumCandidates)) + { + // cout << "Accum score: " << it->first << endl; + // 拿到候选关键帧的指针 + KeyFrame *pKFi = it->second; + if (pKFi->isBad()) + continue; + + // 如果没有被重复添加 + if (!spAlreadyAddedKF.count(pKFi)) + { + // 如果候选帧与当前关键帧在同一个地图里,且候选者数量还不足够 + if (pKF->GetMap() == pKFi->GetMap() && vpLoopCand.size() < nNumCandidates) + { + // 添加到回环候选帧里 + vpLoopCand.push_back(pKFi); + } + // 如果候选者与当前关键帧不再同一个地图里, 且候选者数量还不足够, 且候选者所在地图不是bad + else if (pKF->GetMap() != pKFi->GetMap() && vpMergeCand.size() < nNumCandidates && !pKFi->GetMap()->IsBad()) + { + // 添加到融合候选帧里 + vpMergeCand.push_back(pKFi); + } + // 防止重复添加 + spAlreadyAddedKF.insert(pKFi); + } + i++; + it++; + } +} + +vector KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F, Map *pMap) +{ + list lKFsSharingWords; + + // Search all keyframes that share a word with current frame + { + unique_lock lock(mMutex); + + for (DBoW2::BowVector::const_iterator vit = F->mBowVec.begin(), vend = F->mBowVec.end(); vit != vend; vit++) + { + list &lKFs = mvInvertedFile[vit->first]; + + for (list::iterator lit = lKFs.begin(), lend = lKFs.end(); lit != lend; lit++) + { + KeyFrame *pKFi = *lit; + if (pKFi->mnRelocQuery != F->mnId) + { + pKFi->mnRelocWords = 0; + pKFi->mnRelocQuery = F->mnId; + lKFsSharingWords.push_back(pKFi); + } + pKFi->mnRelocWords++; + } + } + } + if (lKFsSharingWords.empty()) + return vector(); + + // Only compare against those keyframes that share enough words + int maxCommonWords = 0; + for (list::iterator lit = lKFsSharingWords.begin(), lend = lKFsSharingWords.end(); lit != lend; lit++) + { + if ((*lit)->mnRelocWords > maxCommonWords) + maxCommonWords = (*lit)->mnRelocWords; + } + + int minCommonWords = maxCommonWords * 0.8f; + + list> lScoreAndMatch; + + int nscores = 0; + + // Compute similarity score. + for (list::iterator lit = lKFsSharingWords.begin(), lend = lKFsSharingWords.end(); lit != lend; lit++) + { + KeyFrame *pKFi = *lit; + + if (pKFi->mnRelocWords > minCommonWords) + { + nscores++; + float si = mpVoc->score(F->mBowVec, pKFi->mBowVec); + pKFi->mRelocScore = si; + lScoreAndMatch.push_back(make_pair(si, pKFi)); + } + } + + if (lScoreAndMatch.empty()) + return vector(); + + list> lAccScoreAndMatch; + float bestAccScore = 0; + + // Lets now accumulate score by covisibility + for (list>::iterator it = lScoreAndMatch.begin(), itend = lScoreAndMatch.end(); it != itend; it++) + { + KeyFrame *pKFi = it->second; + vector vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10); + + float bestScore = it->first; + float accScore = bestScore; + KeyFrame *pBestKF = pKFi; + for (vector::iterator vit = vpNeighs.begin(), vend = vpNeighs.end(); vit != vend; vit++) + { + KeyFrame *pKF2 = *vit; + if (pKF2->mnRelocQuery != F->mnId) + continue; + + accScore += pKF2->mRelocScore; + if (pKF2->mRelocScore > bestScore) + { + pBestKF = pKF2; + bestScore = pKF2->mRelocScore; + } + } + lAccScoreAndMatch.push_back(make_pair(accScore, pBestKF)); + if (accScore > bestAccScore) + bestAccScore = accScore; + } + + // Return all those keyframes with a score higher than 0.75*bestScore + float minScoreToRetain = 0.75f * bestAccScore; + set spAlreadyAddedKF; + vector vpRelocCandidates; + vpRelocCandidates.reserve(lAccScoreAndMatch.size()); + for (list>::iterator it = lAccScoreAndMatch.begin(), itend = lAccScoreAndMatch.end(); it != itend; it++) + { + const float &si = it->first; + if (si > minScoreToRetain) + { + KeyFrame *pKFi = it->second; + if (pKFi->GetMap() != pMap) + continue; + if (!spAlreadyAddedKF.count(pKFi)) + { + vpRelocCandidates.push_back(pKFi); + spAlreadyAddedKF.insert(pKFi); + } + } + } + + return vpRelocCandidates; +} + +void KeyFrameDatabase::SetORBVocabulary(ORBVocabulary *pORBVoc) +{ + ORBVocabulary **ptr; + ptr = (ORBVocabulary **)(&mpVoc); + *ptr = pORBVoc; + + mvInvertedFile.clear(); + mvInvertedFile.resize(mpVoc->size()); +} + +} // namespace ORB_SLAM diff --git a/ORB-SLAM3-UESTC/Workspace/src/LocalMapping.cc b/ORB-SLAM3-UESTC/Workspace/src/LocalMapping.cc new file mode 100644 index 0000000..1346052 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/LocalMapping.cc @@ -0,0 +1,1956 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#include "LocalMapping.h" +#include "LoopClosing.h" +#include "ORBmatcher.h" +#include "Optimizer.h" +#include "Converter.h" +#include "GeometricTools.h" + +#include +#include + +namespace ORB_SLAM3 +{ + +/** + * @brief 局部地图线程构造函数 + * @param pSys 系统类指针 + * @param pAtlas atlas + * @param bMonocular 是否是单目 (bug)用float赋值了 + * @param bInertial 是否是惯性模式 + * @param _strSeqName 序列名字，没用到 + */ +LocalMapping::LocalMapping(System* pSys, Atlas *pAtlas, const float bMonocular, bool bInertial, const string &_strSeqName): + mpSystem(pSys), mbMonocular(bMonocular), mbInertial(bInertial), mbResetRequested(false), mbResetRequestedActiveMap(false), mbFinishRequested(false), mbFinished(true), mpAtlas(pAtlas), bInitializing(false), + mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbNotStop(false), mbAcceptKeyFrames(true), + mIdxInit(0), mScale(1.0), mInitSect(0), mbNotBA1(true), mbNotBA2(true), mIdxIteration(0), infoInertial(Eigen::MatrixXd::Zero(9,9)) +{ + + /* + * mbStopRequested: 外部线程调用，为true，表示外部线程请求停止 local mapping + * mbStopped: 为true表示可以并终止localmapping 线程 + * mbNotStop: true，表示不要停止 localmapping 线程，因为要插入关键帧了。需要和 mbStopped 结合使用 + * mbAcceptKeyFrames: true，允许接受关键帧。tracking 和local mapping 之间的关键帧调度 + * mbAbortBA: 是否流产BA优化的标志位 + * mbFinishRequested: 请求终止当前线程的标志。注意只是请求，不一定终止。终止要看 mbFinished + * mbResetRequested: 请求当前线程复位的标志。true，表示一直请求复位，但复位还未完成；表示复位完成为false + * mbFinished: 判断最终LocalMapping::Run() 是否完成的标志。 + */ + mnMatchesInliers = 0; + + mbBadImu = false; + + mTinit = 0.f; + + mNumLM = 0; + mNumKFCulling=0; + +#ifdef REGISTER_TIMES + nLBA_exec = 0; + nLBA_abort = 0; +#endif + +} + +/** + * @brief 设置回环类指针 + * @param pLoopCloser 回环类指针 + */ +void LocalMapping::SetLoopCloser(LoopClosing* pLoopCloser) +{ + mpLoopCloser = pLoopCloser; +} + +/** + * @brief 设置跟踪类指针 + * @param pLoopCloser 回环类指针 + */ +void LocalMapping::SetTracker(Tracking *pTracker) +{ + mpTracker=pTracker; +} + +/** + * @brief 局部地图线程主函数 + */ +void LocalMapping::Run() +{ + // 标记状态，表示当前run函数正在运行，尚未结束 + mbFinished = false; + + // 主循环 + while(1) + { + // Tracking will see that Local Mapping is busy + // Step 1 告诉Tracking，LocalMapping正处于繁忙状态，请不要给我发送关键帧打扰我 + // LocalMapping线程处理的关键帧都是Tracking线程发过来的 + SetAcceptKeyFrames(false); + + // Check if there are keyframes in the queue + // 等待处理的关键帧列表不为空 并且imu正常 + if(CheckNewKeyFrames() && !mbBadImu) + { +#ifdef REGISTER_TIMES + double timeLBA_ms = 0; + double timeKFCulling_ms = 0; + + std::chrono::steady_clock::time_point time_StartProcessKF = std::chrono::steady_clock::now(); +#endif + // BoW conversion and insertion in Map + // Step 2 处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等 + ProcessNewKeyFrame(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndProcessKF = std::chrono::steady_clock::now(); + + double timeProcessKF = std::chrono::duration_cast >(time_EndProcessKF - time_StartProcessKF).count(); + vdKFInsert_ms.push_back(timeProcessKF); +#endif + + // Check recent MapPoints + // Step 3 根据地图点的观测情况剔除质量不好的地图点 + MapPointCulling(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndMPCulling = std::chrono::steady_clock::now(); + + double timeMPCulling = std::chrono::duration_cast >(time_EndMPCulling - time_EndProcessKF).count(); + vdMPCulling_ms.push_back(timeMPCulling); +#endif + + // Triangulate new MapPoints + // Step 4 当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳 + CreateNewMapPoints(); + + // 注意orbslam2中放在了函数SearchInNeighbors（用到了mbAbortBA）后面，应该放这里更合适 + mbAbortBA = false; + + // 已经处理完队列中的最后的一个关键帧 + if(!CheckNewKeyFrames()) + { + // Find more matches in neighbor keyframes and fuse point duplications + // Step 5 检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点 + // 先完成相邻关键帧与当前关键帧的地图点的融合（在相邻关键帧中查找当前关键帧的地图点）， + // 再完成当前关键帧与相邻关键帧的地图点的融合（在当前关键帧中查找当前相邻关键帧的地图点） + SearchInNeighbors(); + } + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndMPCreation = std::chrono::steady_clock::now(); + + double timeMPCreation = std::chrono::duration_cast >(time_EndMPCreation - time_EndMPCulling).count(); + vdMPCreation_ms.push_back(timeMPCreation); +#endif + + bool b_doneLBA = false; + int num_FixedKF_BA = 0; + int num_OptKF_BA = 0; + int num_MPs_BA = 0; + int num_edges_BA = 0; + + // 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping + if(!CheckNewKeyFrames() && !stopRequested()) + { + // 当前地图中关键帧数目大于2个 + if(mpAtlas->KeyFramesInMap()>2) + { + // Step 6.1 处于IMU模式并且当前关键帧所在的地图已经完成IMU初始化 + if(mbInertial && mpCurrentKeyFrame->GetMap()->isImuInitialized()) + { + // 计算上一关键帧到当前关键帧相机光心的距离 + 上上关键帧到上一关键帧相机光心的距离 + float dist = (mpCurrentKeyFrame->mPrevKF->GetCameraCenter() - mpCurrentKeyFrame->GetCameraCenter()).norm() + + (mpCurrentKeyFrame->mPrevKF->mPrevKF->GetCameraCenter() - mpCurrentKeyFrame->mPrevKF->GetCameraCenter()).norm(); + // 如果距离大于5厘米，记录当前KF和上一KF时间戳的差，累加到mTinit + if(dist>0.05) + mTinit += mpCurrentKeyFrame->mTimeStamp - mpCurrentKeyFrame->mPrevKF->mTimeStamp; + // 当前关键帧所在的地图尚未完成IMU BA2（IMU第三阶段初始化） + if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA2()) + { + // 如果累计时间差小于10s 并且 距离小于2厘米，认为运动幅度太小，不足以初始化IMU，将mbBadImu设置为true + if((mTinit<10.f) && (dist<0.02)) + { + cout << "Not enough motion for initializing. Reseting..." << endl; + unique_lock lock(mMutexReset); + mbResetRequestedActiveMap = true; + mpMapToReset = mpCurrentKeyFrame->GetMap(); + mbBadImu = true; // 在跟踪线程里会重置当前活跃地图 + } + } + // 判断成功跟踪匹配的点数是否足够多 + // 条件---------1.1、跟踪成功的内点数目大于75-----1.2、并且是单目--或--2.1、跟踪成功的内点数目大于100-----2.2、并且不是单目 + bool bLarge = ((mpTracker->GetMatchesInliers()>75)&&mbMonocular)||((mpTracker->GetMatchesInliers()>100)&&!mbMonocular); + // 局部地图+IMU一起优化，优化关键帧位姿、地图点、IMU参数 + Optimizer::LocalInertialBA(mpCurrentKeyFrame, &mbAbortBA, mpCurrentKeyFrame->GetMap(),num_FixedKF_BA,num_OptKF_BA,num_MPs_BA,num_edges_BA, bLarge, !mpCurrentKeyFrame->GetMap()->GetIniertialBA2()); + b_doneLBA = true; + } + else + { + // Step 6.2 不是IMU模式或者当前关键帧所在的地图还未完成IMU初始化 + // 局部地图BA，不包括IMU数据 + // 注意这里的第二个参数是按地址传递的,当这里的 mbAbortBA 状态发生变化时，能够及时执行/停止BA + // 局部地图优化，不包括IMU信息。优化关键帧位姿、地图点 + Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpCurrentKeyFrame->GetMap(),num_FixedKF_BA,num_OptKF_BA,num_MPs_BA,num_edges_BA); + b_doneLBA = true; + } + + } +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndLBA = std::chrono::steady_clock::now(); + + if(b_doneLBA) + { + timeLBA_ms = std::chrono::duration_cast >(time_EndLBA - time_EndMPCreation).count(); + vdLBA_ms.push_back(timeLBA_ms); + + nLBA_exec += 1; + if(mbAbortBA) + { + nLBA_abort += 1; + } + vnLBA_edges.push_back(num_edges_BA); + vnLBA_KFopt.push_back(num_OptKF_BA); + vnLBA_KFfixed.push_back(num_FixedKF_BA); + vnLBA_MPs.push_back(num_MPs_BA); + } + +#endif + + // Initialize IMU here + // Step 7 当前关键帧所在地图未完成IMU初始化（第一阶段） + if(!mpCurrentKeyFrame->GetMap()->isImuInitialized() && mbInertial) + { + // 在函数InitializeIMU里设置IMU成功初始化标志 SetImuInitialized + // IMU第一次初始化 + if (mbMonocular) + InitializeIMU(1e2, 1e10, true); + else + InitializeIMU(1e2, 1e5, true); + } + + + // Check redundant local Keyframes + // 跟踪中关键帧插入条件比较松，交给LocalMapping线程的关键帧会比较密，这里再删除冗余 + // Step 8 检测并剔除当前帧相邻的关键帧中冗余的关键帧 + // 冗余的判定：该关键帧的90%的地图点可以被其它关键帧观测到 + KeyFrameCulling(); + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndKFCulling = std::chrono::steady_clock::now(); + + timeKFCulling_ms = std::chrono::duration_cast >(time_EndKFCulling - time_EndLBA).count(); + vdKFCulling_ms.push_back(timeKFCulling_ms); +#endif + // Step 9 如果距离IMU第一阶段初始化成功累计时间差小于100s，进行VIBA + if ((mTinit<50.0f) && mbInertial) + { + // Step 9.1 根据条件判断是否进行VIBA1（IMU第二次初始化） + // 条件：1、当前关键帧所在的地图还未完成IMU初始化---并且--------2、正常跟踪状态---------- + if(mpCurrentKeyFrame->GetMap()->isImuInitialized() && mpTracker->mState==Tracking::OK) // Enter here everytime local-mapping is called + { + // 当前关键帧所在的地图还未完成VIBA 1 + if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA1()){ + // 如果累计时间差大于5s，开始VIBA1（IMU第二阶段初始化） + if (mTinit>5.0f) + { + cout << "start VIBA 1" << endl; + mpCurrentKeyFrame->GetMap()->SetIniertialBA1(); + if (mbMonocular) + InitializeIMU(1.f, 1e5, true); + else + InitializeIMU(1.f, 1e5, true); + + cout << "end VIBA 1" << endl; + } + } + // Step 9.2 根据条件判断是否进行VIBA2（IMU第三次初始化） + // 当前关键帧所在的地图还未完成VIBA 2 + else if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA2()){ + if (mTinit>15.0f){ + cout << "start VIBA 2" << endl; + mpCurrentKeyFrame->GetMap()->SetIniertialBA2(); + if (mbMonocular) + InitializeIMU(0.f, 0.f, true); + else + InitializeIMU(0.f, 0.f, true); + + cout << "end VIBA 2" << endl; + } + } + + // scale refinement + // Step 9.3 在关键帧小于100时，会在满足一定时间间隔后多次进行尺度、重力方向优化 + if (((mpAtlas->KeyFramesInMap())<=200) && + ((mTinit>25.0f && mTinit<25.5f)|| + (mTinit>35.0f && mTinit<35.5f)|| + (mTinit>45.0f && mTinit<45.5f)|| + (mTinit>55.0f && mTinit<55.5f)|| + (mTinit>65.0f && mTinit<65.5f)|| + (mTinit>75.0f && mTinit<75.5f))){ + if (mbMonocular) + // 使用了所有关键帧，但只优化尺度和重力方向以及速度和偏执（其实就是一切跟惯性相关的量） + ScaleRefinement(); + } + } + } + } + +#ifdef REGISTER_TIMES + vdLBASync_ms.push_back(timeKFCulling_ms); + vdKFCullingSync_ms.push_back(timeKFCulling_ms); +#endif + // Step 10 将当前帧加入到闭环检测队列中 + mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame); + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndLocalMap = std::chrono::steady_clock::now(); + + double timeLocalMap = std::chrono::duration_cast >(time_EndLocalMap - time_StartProcessKF).count(); + vdLMTotal_ms.push_back(timeLocalMap); +#endif + } + else if(Stop() && !mbBadImu) // 当要终止当前线程的时候 + { + // Safe area to stop + while(isStopped() && !CheckFinish()) + { + // 如果还没有结束利索,那么等等它 + usleep(3000); + } + // 然后确定终止了就跳出这个线程的主循环 + if(CheckFinish()) + break; + } + // 查看是否有复位线程的请求 + ResetIfRequested(); + + // Tracking will see that Local Mapping is busy + // 开始接收关键帧 + SetAcceptKeyFrames(true); + + // 如果当前线程已经结束了就跳出主循环 + if(CheckFinish()) + break; + + usleep(3000); + } + + // 设置线程已经终止 + SetFinish(); +} + +/** + * @brief 插入关键帧,由外部（Tracking）线程调用;这里只是插入到列表中,等待线程主函数对其进行处理 + * @param pKF 新的关键帧 + */ +void LocalMapping::InsertKeyFrame(KeyFrame *pKF) +{ + unique_lock lock(mMutexNewKFs); + // 将关键帧插入到列表中 + mlNewKeyFrames.push_back(pKF); + mbAbortBA=true; +} + +/** + * @brief 查看列表中是否有等待被插入的关键帧 + */ +bool LocalMapping::CheckNewKeyFrames() +{ + unique_lock lock(mMutexNewKFs); + return(!mlNewKeyFrames.empty()); +} + +/** + * @brief 处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等 + */ +void LocalMapping::ProcessNewKeyFrame() +{ + // Step 1：从缓冲队列中取出一帧关键帧 + // 该关键帧队列是Tracking线程向LocalMapping中插入的关键帧组成 + { + unique_lock lock(mMutexNewKFs); + // 取出列表中最前面的关键帧，作为当前要处理的关键帧 + mpCurrentKeyFrame = mlNewKeyFrames.front(); + // 取出最前面的关键帧后，在原来的列表里删掉该关键帧 + mlNewKeyFrames.pop_front(); + } + + // Compute Bags of Words structures + // Step 2：计算该关键帧特征点的Bow信息 + mpCurrentKeyFrame->ComputeBoW(); + + // Associate MapPoints to the new keyframe and update normal and descriptor + // Step 3：当前处理关键帧中有效的地图点，更新normal，描述子等信息 + // TrackLocalMap中和当前帧新匹配上的地图点和当前关键帧进行关联绑定 + const vector vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches(); + // 对当前处理的这个关键帧中的所有的地图点展开遍历 + for(size_t i=0; iisBad()) + { + if(!pMP->IsInKeyFrame(mpCurrentKeyFrame)) + { + // 如果地图点不是来自当前帧的观测，为当前地图点添加观测 + pMP->AddObservation(mpCurrentKeyFrame, i); + // 获得该点的平均观测方向和观测距离范围 + pMP->UpdateNormalAndDepth(); + // 更新地图点的最佳描述子 + pMP->ComputeDistinctiveDescriptors(); + } + else // this can only happen for new stereo points inserted by the Tracking + { + // 如果当前帧中已经包含了这个地图点,但是这个地图点中却没有包含这个关键帧的信息 + // 这些地图点可能来自双目或RGBD跟踪过程中新生成的地图点，或者是CreateNewMapPoints 中通过三角化产生 + // 将上述地图点放入mlpRecentAddedMapPoints，等待后续MapPointCulling函数的检验 + mlpRecentAddedMapPoints.push_back(pMP); + } + } + } + } + + // Update links in the Covisibility Graph + // Step 4：更新关键帧间的连接关系（共视图） + mpCurrentKeyFrame->UpdateConnections(); + + // Insert Keyframe in Map + // Step 5：将该关键帧插入到地图中 + mpAtlas->AddKeyFrame(mpCurrentKeyFrame); +} + +/** + * @brief 处理新的关键帧，使队列为空，注意这里只是处理了关键帧，并没有生成MP + */ +void LocalMapping::EmptyQueue() +{ + while(CheckNewKeyFrames()) + ProcessNewKeyFrame(); +} + +/** + * @brief 检查新增地图点，根据地图点的观测情况剔除质量不好的新增的地图点 + * mlpRecentAddedMapPoints: 存储新增的地图点，这里是要删除其中不靠谱的 + */ +void LocalMapping::MapPointCulling() +{ + // Check Recent Added MapPoints + list::iterator lit = mlpRecentAddedMapPoints.begin(); + const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId; + + // Step 1：根据相机类型设置不同的观测阈值 + int nThObs; + if(mbMonocular) + nThObs = 2; + else + nThObs = 3; + const int cnThObs = nThObs; + + int borrar = mlpRecentAddedMapPoints.size(); + // Step 2：遍历检查的新添加的MapPoints + while(lit!=mlpRecentAddedMapPoints.end()) + { + MapPoint* pMP = *lit; + + if(pMP->isBad()) + // Step 2.1：已经是坏点的MapPoints直接从检查链表中删除 + lit = mlpRecentAddedMapPoints.erase(lit); + else if(pMP->GetFoundRatio()<0.25f) + { + // Step 2.2：跟踪到该MapPoint的Frame数相比预计可观测到该MapPoint的Frame数的比例小于25%，删除 + // (mnFound/mnVisible） < 25% + // mnFound ：地图点被多少帧（包括普通帧）看到，次数越多越好 + // mnVisible：地图点应该被看到的次数 + // (mnFound/mnVisible）：对于大FOV镜头这个比例会高，对于窄FOV镜头这个比例会低 + pMP->SetBadFlag(); + lit = mlpRecentAddedMapPoints.erase(lit); + } + else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs) + { + // Step 2.3：从该点建立开始，到现在已经过了不小于2个关键帧 + // 但是观测到该点的关键帧数却不超过cnThObs帧，那么删除该点 + pMP->SetBadFlag(); + lit = mlpRecentAddedMapPoints.erase(lit); + } + // Step 2.4：从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点 + // 因此没有SetBadFlag()，仅从队列中删除，放弃继续对该MapPoint的检测 + else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3) + lit = mlpRecentAddedMapPoints.erase(lit); + else + { + lit++; + borrar--; + } + } +} + +/** + * @brief 用当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳 + */ +void LocalMapping::CreateNewMapPoints() +{ + // Retrieve neighbor keyframes in covisibility graph + // nn表示搜索最佳共视关键帧的数目 + // 不同传感器下要求不一样,单目的时候需要有更多的具有较好共视关系的关键帧来建立地图 + int nn = 10; + // For stereo inertial case 这具原注释有问题吧 应该是For Monocular case + // 0.4版本的这个参数为20 + if(mbMonocular) + nn=30; + // Step 1：在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻关键帧 + vector vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn); + + // imu模式下在附近添加更多的帧进来 + if (mbInertial) + { + KeyFrame* pKF = mpCurrentKeyFrame; + int count=0; + // 在总数不够且上一关键帧存在，且添加的帧没有超过总数时 + while((vpNeighKFs.size()<=nn)&&(pKF->mPrevKF)&&(count++::iterator it = std::find(vpNeighKFs.begin(), vpNeighKFs.end(), pKF->mPrevKF); + if(it==vpNeighKFs.end()) + vpNeighKFs.push_back(pKF->mPrevKF); + pKF = pKF->mPrevKF; + } + } + + float th = 0.6f; + // 特征点匹配配置 最小距离 < 0.6*次小距离，比较苛刻了。不检查旋转 + ORBmatcher matcher(th,false); + + // 取出当前帧从世界坐标系到相机坐标系的变换矩阵 + Sophus::SE3 sophTcw1 = mpCurrentKeyFrame->GetPose(); + Eigen::Matrix eigTcw1 = sophTcw1.matrix3x4(); + Eigen::Matrix Rcw1 = eigTcw1.block<3,3>(0,0); + Eigen::Matrix Rwc1 = Rcw1.transpose(); + Eigen::Vector3f tcw1 = sophTcw1.translation(); + // 得到当前关键帧（左目）光心在世界坐标系中的坐标、内参 + Eigen::Vector3f Ow1 = mpCurrentKeyFrame->GetCameraCenter(); + + const float &fx1 = mpCurrentKeyFrame->fx; + const float &fy1 = mpCurrentKeyFrame->fy; + const float &cx1 = mpCurrentKeyFrame->cx; + const float &cy1 = mpCurrentKeyFrame->cy; + const float &invfx1 = mpCurrentKeyFrame->invfx; + const float &invfy1 = mpCurrentKeyFrame->invfy; + + // 用于后面的点深度的验证;这里的1.5是经验值 + const float ratioFactor = 1.5f*mpCurrentKeyFrame->mfScaleFactor; + + // 以下是统计点数用的，没啥作用 + int countStereo = 0; + int countStereoGoodProj = 0; + int countStereoAttempt = 0; + int totalStereoPts = 0; + // Search matches with epipolar restriction and triangulate + + // Step 2：遍历相邻关键帧vpNeighKFs + for(size_t i=0; i0 && CheckNewKeyFrames()) + return; + + KeyFrame* pKF2 = vpNeighKFs[i]; + + GeometricCamera* pCamera1 = mpCurrentKeyFrame->mpCamera, *pCamera2 = pKF2->mpCamera; + + // Check first that baseline is not too short + // 邻接的关键帧光心在世界坐标系中的坐标 + Eigen::Vector3f Ow2 = pKF2->GetCameraCenter(); + // 基线向量，两个关键帧间的相机位移 + Eigen::Vector3f vBaseline = Ow2-Ow1; + // 基线长度 + const float baseline = vBaseline.norm(); + + // Step 3：判断相机运动的基线是不是足够长 + if(!mbMonocular) + { + // 如果是双目相机，关键帧间距小于本身的基线时不生成3D点 + // 因为太短的基线下能够恢复的地图点不稳定 + if(baselinemb) + continue; + } + else + { + // 单目相机情况 + // 邻接关键帧的场景深度中值 + const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2); + // baseline与景深的比例 + const float ratioBaselineDepth = baseline/medianDepthKF2; + // 如果特别远(比例特别小)，基线太短恢复3D点不准，那么跳过当前邻接的关键帧，不生成3D点 + if(ratioBaselineDepth<0.01) + continue; + } + + // Search matches that fullfil epipolar constraint + // Step 4：通过BoW对两关键帧的未匹配的特征点快速匹配，用极线约束抑制离群点，生成新的匹配点对 + vector > vMatchedIndices; + // 当惯性模式下，并且经过三次初始化，且为刚丢失状态 + bool bCoarse = mbInertial && mpTracker->mState==Tracking::RECENTLY_LOST && mpCurrentKeyFrame->GetMap()->GetIniertialBA2(); + + // 通过极线约束的方式找到匹配点（且该点还没有成为MP，注意非单目已经生成的MP这里直接跳过不做匹配，所以最后并不会覆盖掉特征点对应的MP） + matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,vMatchedIndices,false,bCoarse); + + // 取出与mpCurrentKeyFrame共视关键帧的内外参信息 + Sophus::SE3 sophTcw2 = pKF2->GetPose(); + Eigen::Matrix eigTcw2 = sophTcw2.matrix3x4(); + Eigen::Matrix Rcw2 = eigTcw2.block<3,3>(0,0); + Eigen::Matrix Rwc2 = Rcw2.transpose(); + Eigen::Vector3f tcw2 = sophTcw2.translation(); + + const float &fx2 = pKF2->fx; + const float &fy2 = pKF2->fy; + const float &cx2 = pKF2->cx; + const float &cy2 = pKF2->cy; + const float &invfx2 = pKF2->invfx; + const float &invfy2 = pKF2->invfy; + + // Triangulate each match + // Step 5：对每对匹配通过三角化生成3D点,和 Triangulate函数差不多 + const int nmatches = vMatchedIndices.size(); + for(int ikp=0; ikp NLeft == -1) ? mpCurrentKeyFrame->mvKeysUn[idx1] + : (idx1 < mpCurrentKeyFrame -> NLeft) ? mpCurrentKeyFrame -> mvKeys[idx1] + : mpCurrentKeyFrame -> mvKeysRight[idx1 - mpCurrentKeyFrame -> NLeft]; + // mvuRight中存放着极限校准后双目特征点在右目对应的像素横坐标，如果不是基线校准的双目或者没有找到匹配点，其值将为-1（或者rgbd） + const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1]; + bool bStereo1 = (!mpCurrentKeyFrame->mpCamera2 && kp1_ur>=0); + // 查看点idx1是否为右目的点 + const bool bRight1 = (mpCurrentKeyFrame -> NLeft == -1 || idx1 < mpCurrentKeyFrame -> NLeft) ? false + : true; + + + // 5.2 + // 当前匹配在邻接关键帧中的特征点 + const cv::KeyPoint &kp2 = (pKF2 -> NLeft == -1) ? pKF2->mvKeysUn[idx2] + : (idx2 < pKF2 -> NLeft) ? pKF2 -> mvKeys[idx2] + : pKF2 -> mvKeysRight[idx2 - pKF2 -> NLeft]; + // mvuRight中存放着双目的深度值，如果不是双目，其值将为-1 + // mvuRight中存放着极限校准后双目特征点在右目对应的像素横坐标，如果不是基线校准的双目或者没有找到匹配点，其值将为-1（或者rgbd） + const float kp2_ur = pKF2->mvuRight[idx2]; + bool bStereo2 = (!pKF2->mpCamera2 && kp2_ur>=0); + // 查看点idx2是否为右目的点 + const bool bRight2 = (pKF2 -> NLeft == -1 || idx2 < pKF2 -> NLeft) ? false + : true; + + // 5.3 当目前为左右目时，确定两个点所在相机之间的位姿关系 + if(mpCurrentKeyFrame->mpCamera2 && pKF2->mpCamera2){ + if(bRight1 && bRight2){ + sophTcw1 = mpCurrentKeyFrame->GetRightPose(); + Ow1 = mpCurrentKeyFrame->GetRightCameraCenter(); + + sophTcw2 = pKF2->GetRightPose(); + Ow2 = pKF2->GetRightCameraCenter(); + + pCamera1 = mpCurrentKeyFrame->mpCamera2; + pCamera2 = pKF2->mpCamera2; + } + else if(bRight1 && !bRight2){ + sophTcw1 = mpCurrentKeyFrame->GetRightPose(); + Ow1 = mpCurrentKeyFrame->GetRightCameraCenter(); + + sophTcw2 = pKF2->GetPose(); + Ow2 = pKF2->GetCameraCenter(); + + pCamera1 = mpCurrentKeyFrame->mpCamera2; + pCamera2 = pKF2->mpCamera; + } + else if(!bRight1 && bRight2){ + sophTcw1 = mpCurrentKeyFrame->GetPose(); + Ow1 = mpCurrentKeyFrame->GetCameraCenter(); + + sophTcw2 = pKF2->GetRightPose(); + Ow2 = pKF2->GetRightCameraCenter(); + + pCamera1 = mpCurrentKeyFrame->mpCamera; + pCamera2 = pKF2->mpCamera2; + } + else{ + sophTcw1 = mpCurrentKeyFrame->GetPose(); + Ow1 = mpCurrentKeyFrame->GetCameraCenter(); + + sophTcw2 = pKF2->GetPose(); + Ow2 = pKF2->GetCameraCenter(); + + pCamera1 = mpCurrentKeyFrame->mpCamera; + pCamera2 = pKF2->mpCamera; + } + eigTcw1 = sophTcw1.matrix3x4(); + Rcw1 = eigTcw1.block<3,3>(0,0); + Rwc1 = Rcw1.transpose(); + tcw1 = sophTcw1.translation(); + + eigTcw2 = sophTcw2.matrix3x4(); + Rcw2 = eigTcw2.block<3,3>(0,0); + Rwc2 = Rcw2.transpose(); + tcw2 = sophTcw2.translation(); + } + + // Check parallax between rays + // Step 5.4：利用匹配点反投影得到视差角 + // 特征点反投影,其实得到的是在各自相机坐标系下的一个非归一化的方向向量,和这个点的反投影射线重合 + Eigen::Vector3f xn1 = pCamera1->unprojectEig(kp1.pt); + Eigen::Vector3f xn2 = pCamera2->unprojectEig(kp2.pt); + // 由相机坐标系转到世界坐标系(得到的是那条反投影射线的一个同向向量在世界坐标系下的表示,还是只能够表示方向)，得到视差角余弦值 + Eigen::Vector3f ray1 = Rwc1 * xn1; + Eigen::Vector3f ray2 = Rwc2 * xn2; + // 这个就是求向量之间角度公式 + const float cosParallaxRays = ray1.dot(ray2)/(ray1.norm() * ray2.norm()); + + // 加1是为了让cosParallaxStereo随便初始化为一个很大的值 + float cosParallaxStereo = cosParallaxRays+1; + float cosParallaxStereo1 = cosParallaxStereo; + float cosParallaxStereo2 = cosParallaxStereo; + + // Step 5.5：对于双目，利用双目得到视差角；单目相机没有特殊操作 + if(bStereo1) + // 传感器是双目相机,并且当前的关键帧的这个点有对应的深度 + // 假设是平行的双目相机，计算出两个相机观察这个点的时候的视差角; + // ? 感觉直接使用向量夹角的方式计算会准确一些啊（双目的时候），那么为什么不直接使用那个呢？ + // 回答：因为双目深度值、基线是更可靠的，比特征匹配再三角化出来的稳 + cosParallaxStereo1 = cos(2*atan2(mpCurrentKeyFrame->mb/2,mpCurrentKeyFrame->mvDepth[idx1])); + else if(bStereo2) + //传感器是双目相机,并且邻接的关键帧的这个点有对应的深度，和上面一样操作 + cosParallaxStereo2 = cos(2*atan2(pKF2->mb/2,pKF2->mvDepth[idx2])); + + // 统计用的 + if (bStereo1 || bStereo2) totalStereoPts++; + + // 得到双目观测的视差角 + cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2); + + // Step 5.6：三角化恢复3D点 + Eigen::Vector3f x3D; + + bool goodProj = false; + bool bPointStereo = false; + // cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)表明视线角正常 + // cosParallaxRays0 && (bStereo1 || bStereo2 || + (cosParallaxRays<0.9996 && mbInertial) || (cosParallaxRays<0.9998 && !mbInertial))) + { + // 三角化，包装成了函数 + goodProj = GeometricTools::Triangulate(xn1, xn2, eigTcw1, eigTcw2, x3D); + if(!goodProj) + continue; + } + else if(bStereo1 && cosParallaxStereo1UnprojectStereo(idx1, x3D); + } + else if(bStereo2 && cosParallaxStereo2UnprojectStereo(idx2, x3D); + } + else + { + continue; //No stereo and very low parallax + } + + // 成功三角化 + if(goodProj && bPointStereo) + countStereoGoodProj++; + + if(!goodProj) + continue; + + //Check triangulation in front of cameras + // Step 5.7：检测生成的3D点是否在相机前方,不在的话就放弃这个点 + float z1 = Rcw1.row(2).dot(x3D) + tcw1(2); + if(z1<=0) + continue; + + float z2 = Rcw2.row(2).dot(x3D) + tcw2(2); + if(z2<=0) + continue; + + //Check reprojection error in first keyframe + // Step 5.7：计算3D点在当前关键帧下的重投影误差 + const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave]; + const float x1 = Rcw1.row(0).dot(x3D)+tcw1(0); + const float y1 = Rcw1.row(1).dot(x3D)+tcw1(1); + const float invz1 = 1.0/z1; + + if(!bStereo1) + { + // 单目情况下 + cv::Point2f uv1 = pCamera1->project(cv::Point3f(x1,y1,z1)); + float errX1 = uv1.x - kp1.pt.x; + float errY1 = uv1.y - kp1.pt.y; + + // 假设测量有一个像素的偏差，2自由度卡方检验阈值是5.991 + if((errX1*errX1+errY1*errY1)>5.991*sigmaSquare1) + continue; + + } + else + { + // 双目情况 + float u1 = fx1*x1*invz1+cx1; + // 根据视差公式计算假想的右目坐标 + float u1_r = u1 - mpCurrentKeyFrame->mbf*invz1; + float v1 = fy1*y1*invz1+cy1; + float errX1 = u1 - kp1.pt.x; + float errY1 = v1 - kp1.pt.y; + float errX1_r = u1_r - kp1_ur; + // 自由度为3，卡方检验阈值是7.8 + if((errX1*errX1+errY1*errY1+errX1_r*errX1_r)>7.8*sigmaSquare1) + continue; + } + + //Check reprojection error in second keyframe + // 计算3D点在另一个关键帧下的重投影误差，操作同上 + const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave]; + const float x2 = Rcw2.row(0).dot(x3D)+tcw2(0); + const float y2 = Rcw2.row(1).dot(x3D)+tcw2(1); + const float invz2 = 1.0/z2; + if(!bStereo2) + { + cv::Point2f uv2 = pCamera2->project(cv::Point3f(x2,y2,z2)); + float errX2 = uv2.x - kp2.pt.x; + float errY2 = uv2.y - kp2.pt.y; + if((errX2*errX2+errY2*errY2)>5.991*sigmaSquare2) + continue; + } + else + { + float u2 = fx2*x2*invz2+cx2; + float u2_r = u2 - mpCurrentKeyFrame->mbf*invz2; + float v2 = fy2*y2*invz2+cy2; + float errX2 = u2 - kp2.pt.x; + float errY2 = v2 - kp2.pt.y; + float errX2_r = u2_r - kp2_ur; + if((errX2*errX2+errY2*errY2+errX2_r*errX2_r)>7.8*sigmaSquare2) + continue; + } + + //Check scale consistency + // Step 5.8：检查尺度连续性 + + // 世界坐标系下，3D点与相机间的向量，方向由相机指向3D点 + Eigen::Vector3f normal1 = x3D - Ow1; + float dist1 = normal1.norm(); + + Eigen::Vector3f normal2 = x3D - Ow2; + float dist2 = normal2.norm(); + + if(dist1==0 || dist2==0) + continue; + + if(mbFarPoints && (dist1>=mThFarPoints||dist2>=mThFarPoints)) // MODIFICATION + continue; + // ratioDist是不考虑金字塔尺度下的距离比例 + const float ratioDist = dist2/dist1; + // 金字塔尺度因子的比例 + const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave]/pKF2->mvScaleFactors[kp2.octave]; + + // 距离的比例和图像金字塔的比例不应该差太多，否则就跳过 + if(ratioDist*ratioFactorratioOctave*ratioFactor) + continue; + + // Triangulation is succesfull + // Step 6：三角化生成3D点成功，构造成MapPoint + MapPoint* pMP = new MapPoint(x3D, mpCurrentKeyFrame, mpAtlas->GetCurrentMap()); + if (bPointStereo) + countStereo++; + + // Step 6.1：为该MapPoint添加属性： + // a.观测到该MapPoint的关键帧 + pMP->AddObservation(mpCurrentKeyFrame,idx1); + pMP->AddObservation(pKF2,idx2); + + mpCurrentKeyFrame->AddMapPoint(pMP,idx1); + pKF2->AddMapPoint(pMP,idx2); + + // b.该MapPoint的描述子 + pMP->ComputeDistinctiveDescriptors(); + + // c.该MapPoint的平均观测方向和深度范围 + pMP->UpdateNormalAndDepth(); + + mpAtlas->AddMapPoint(pMP); + // Step 7：将新产生的点放入检测队列 + // 这些MapPoints都会经过MapPointCulling函数的检验 + mlpRecentAddedMapPoints.push_back(pMP); + } + } +} + +/** + * @brief 检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints + */ +void LocalMapping::SearchInNeighbors() +{ + // Retrieve neighbor keyframes + // Step 1：获得当前关键帧在共视图中权重排名前nn的邻接关键帧 + // 开始之前先定义几个概念 + // 当前关键帧的邻接关键帧，称为一级相邻关键帧，也就是邻居 + // 与一级相邻关键帧相邻的关键帧，称为二级相邻关键帧，也就是邻居的邻居 + + // 单目情况要30个邻接关键帧,0.4版本是20个，又加了许多 + int nn = 10; + if(mbMonocular) + nn=30; + + // 和当前关键帧相邻的关键帧，也就是一级相邻关键帧 + const vector vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn); + // Step 2：存储一级相邻关键帧及其二级相邻关键帧 + vector vpTargetKFs; + // 开始对所有候选的一级关键帧展开遍历： + for(vector::const_iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++) + { + KeyFrame* pKFi = *vit; + // 没有和当前帧进行过融合的操作 + if(pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId) + continue; + // 加入一级相邻关键帧 + vpTargetKFs.push_back(pKFi); + // 标记已经加入 + pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId; + } + + // Add some covisible of covisible + // Extend to some second neighbors if abort is not requested + // 以一级相邻关键帧的共视关系最好的5个相邻关键帧 作为二级相邻关键帧 + for(int i=0, imax=vpTargetKFs.size(); i vpSecondNeighKFs = vpTargetKFs[i]->GetBestCovisibilityKeyFrames(20); + // 遍历得到的二级相邻关键帧 + for(vector::const_iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++) + { + KeyFrame* pKFi2 = *vit2; + if(pKFi2->isBad() || pKFi2->mnFuseTargetForKF==mpCurrentKeyFrame->mnId || pKFi2->mnId==mpCurrentKeyFrame->mnId) + continue; + // 存入二级相邻关键帧 + vpTargetKFs.push_back(pKFi2); + pKFi2->mnFuseTargetForKF=mpCurrentKeyFrame->mnId; + } + if (mbAbortBA) + break; + } + + // Extend to temporal neighbors + // imu模式下加了一些prevKF + if(mbInertial) + { + KeyFrame* pKFi = mpCurrentKeyFrame->mPrevKF; + while(vpTargetKFs.size()<20 && pKFi) + { + if(pKFi->isBad() || pKFi->mnFuseTargetForKF==mpCurrentKeyFrame->mnId) + { + pKFi = pKFi->mPrevKF; + continue; + } + vpTargetKFs.push_back(pKFi); + pKFi->mnFuseTargetForKF=mpCurrentKeyFrame->mnId; + pKFi = pKFi->mPrevKF; + } + } + + // Search matches by projection from current KF in target KFs + // 使用默认参数, 最优和次优比例0.6,匹配时检查特征点的旋转 + ORBmatcher matcher; + + // Step 3：将当前帧的地图点分别与一级二级相邻关键帧地图点进行融合 -- 正向 + vector vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches(); + for(vector::iterator vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++) + { + KeyFrame* pKFi = *vit; + + // 将地图点投影到关键帧中进行匹配和融合；融合策略如下 + // 1.如果地图点能匹配关键帧的特征点，并且该点有对应的地图点，那么选择观测数目多的替换两个地图点 + // 2.如果地图点能匹配关键帧的特征点，并且该点没有对应的地图点，那么为该点添加该投影地图点 + // 注意这个时候对地图点融合的操作是立即生效的 + matcher.Fuse(pKFi,vpMapPointMatches); + if(pKFi->NLeft != -1) matcher.Fuse(pKFi,vpMapPointMatches,true); + } + + + if (mbAbortBA) + return; + + // Search matches by projection from target KFs in current KF + // Step 4：将一级二级相邻关键帧地图点分别与当前关键帧地图点进行融合 -- 反向 + // 用于进行存储要融合的一级邻接和二级邻接关键帧所有MapPoints的集合 + vector vpFuseCandidates; + vpFuseCandidates.reserve(vpTargetKFs.size()*vpMapPointMatches.size()); + + // Step 4.1：遍历每一个一级邻接和二级邻接关键帧，收集他们的地图点存储到 vpFuseCandidates + for(vector::iterator vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++) + { + KeyFrame* pKFi = *vitKF; + + vector vpMapPointsKFi = pKFi->GetMapPointMatches(); + + // 遍历当前一级邻接和二级邻接关键帧中所有的MapPoints,找出需要进行融合的并且加入到集合中 + for(vector::iterator vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++) + { + MapPoint* pMP = *vitMP; + if(!pMP) + continue; + + // 如果地图点是坏点，或者已经加进集合vpFuseCandidates，跳过 + if(pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId) + continue; + + // 加入集合，并标记已经加入 + pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId; + vpFuseCandidates.push_back(pMP); + } + } + + // Step 4.2：进行地图点投影融合,和正向融合操作是完全相同的 + // 不同的是正向操作是"每个关键帧和当前关键帧的地图点进行融合",而这里的是"当前关键帧和所有邻接关键帧的地图点进行融合" + matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates); + if(mpCurrentKeyFrame->NLeft != -1) matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates,true); + + + // Update points + // Step 5：更新当前帧地图点的描述子、深度、观测主方向等属性 + vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches(); + for(size_t i=0, iend=vpMapPointMatches.size(); iisBad()) + { + // 在所有找到pMP的关键帧中，获得最佳的描述子 + pMP->ComputeDistinctiveDescriptors(); + + // 更新平均观测方向和观测距离 + pMP->UpdateNormalAndDepth(); + } + } + } + + // Update connections in covisibility graph + // Step 6：更新当前帧的MapPoints后更新与其它帧的连接关系 + // 更新covisibility图 + mpCurrentKeyFrame->UpdateConnections(); +} + +/** + * @brief 外部线程调用,请求停止当前线程的工作; 其实是回环检测线程调用,来避免在进行全局优化的过程中局部建图线程添加新的关键帧 + */ +void LocalMapping::RequestStop() +{ + unique_lock lock(mMutexStop); + mbStopRequested = true; + unique_lock lock2(mMutexNewKFs); + mbAbortBA = true; +} + +/** + * @brief 检查是否要把当前的局部建图线程停止工作,运行的时候要检查是否有终止请求,如果有就执行. 由run函数调用 + */ +bool LocalMapping::Stop() +{ + unique_lock lock(mMutexStop); + // 如果当前线程还没有准备停止,但是已经有终止请求了,那么就准备停止当前线程 + if(mbStopRequested && !mbNotStop) + { + mbStopped = true; + cout << "Local Mapping STOP" << endl; + return true; + } + + return false; +} + +/** + * @brief 检查mbStopped是否为true，为true表示可以并终止localmapping 线程 + */ +bool LocalMapping::isStopped() +{ + unique_lock lock(mMutexStop); + return mbStopped; +} + +/** + * @brief 求外部线程调用，为true，表示外部线程请求停止 local mapping + */ +bool LocalMapping::stopRequested() +{ + unique_lock lock(mMutexStop); + return mbStopRequested; +} + +/** + * @brief 释放当前还在缓冲区中的关键帧指针 + */ +void LocalMapping::Release() +{ + unique_lock lock(mMutexStop); + unique_lock lock2(mMutexFinish); + if(mbFinished) + return; + mbStopped = false; + mbStopRequested = false; + for(list::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++) + delete *lit; + mlNewKeyFrames.clear(); + + cout << "Local Mapping RELEASE" << endl; +} + +/** + * @brief 查看是否接收关键帧，也就是当前线程是否在处理数据，当然tracking线程也不会全看这个值，他会根据队列阻塞情况 + */ +bool LocalMapping::AcceptKeyFrames() +{ + unique_lock lock(mMutexAccept); + return mbAcceptKeyFrames; +} + +/** + * @brief 设置"允许接受关键帧"的状态标志 + */ +void LocalMapping::SetAcceptKeyFrames(bool flag) +{ + unique_lock lock(mMutexAccept); + mbAcceptKeyFrames=flag; +} + +/** + * @brief 如果不让它暂停，即使发出了暂停信号也不暂停 + */ +bool LocalMapping::SetNotStop(bool flag) +{ + unique_lock lock(mMutexStop); + + if(flag && mbStopped) + return false; + + mbNotStop = flag; + + return true; +} + +/** + * @brief 放弃这次操作，虽然叫BA但并不是只断优化 + */ +void LocalMapping::InterruptBA() +{ + mbAbortBA = true; +} + +/** + * @brief 检测当前关键帧在共视图中的关键帧，根据地图点在共视图中的冗余程度剔除该共视关键帧 + * 冗余关键帧的判定：90%以上的地图点能被其他关键帧（至少3个）观测到 + */ +void LocalMapping::KeyFrameCulling() +{ + // Check redundant keyframes (only local keyframes) + // A keyframe is considered redundant if the 90% of the MapPoints it sees, are seen + // in at least other 3 keyframes (in the same or finer scale) + // We only consider close stereo points + + // 该函数里变量层层深入，这里列一下： + // mpCurrentKeyFrame：当前关键帧，本程序就是判断它是否需要删除 + // pKF： mpCurrentKeyFrame的某一个共视关键帧 + // vpMapPoints：pKF对应的所有地图点 + // pMP：vpMapPoints中的某个地图点 + // observations：所有能观测到pMP的关键帧 + // pKFi：observations中的某个关键帧 + // scaleLeveli：pKFi的金字塔尺度 + // scaleLevel：pKF的金字塔尺度 + const int Nd = 21; + // 更新共视关系 + mpCurrentKeyFrame->UpdateBestCovisibles(); + // 1. 根据Covisibility Graph提取当前帧的共视关键帧 + vector vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames(); + + float redundant_th; + // 非IMU模式时 + if(!mbInertial) + redundant_th = 0.9; + // 单目+imu 时 + else if (mbMonocular) + redundant_th = 0.9; + // 双目+imu时 + else + redundant_th = 0.5; + + const bool bInitImu = mpAtlas->isImuInitialized(); + int count=0; + + // Compoute last KF from optimizable window: + unsigned int last_ID; + if (mbInertial) + { + int count = 0; + KeyFrame* aux_KF = mpCurrentKeyFrame; + // 找到第前21个关键帧的关键帧id + while(countmPrevKF) + { + aux_KF = aux_KF->mPrevKF; + count++; + } + last_ID = aux_KF->mnId; + } + + + // 对所有的共视关键帧进行遍历 + for(vector::iterator vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++) + { + count++; + KeyFrame* pKF = *vit; + + // 如果是地图里第1个关键帧或者是标记为坏帧，则跳过 + if((pKF->mnId==pKF->GetMap()->GetInitKFid()) || pKF->isBad()) + continue; + // Step 2：提取每个共视关键帧的地图点 + const vector vpMapPoints = pKF->GetMapPointMatches(); + + // 记录某个点被观测次数，后面并未使用 + int nObs = 3; + const int thObs=nObs; // 观测次数阈值，默认为3 + // 记录冗余观测点的数目 + int nRedundantObservations=0; + int nMPs=0; + + // Step 3：遍历该共视关键帧的所有地图点，判断是否90%以上的地图点能被其它至少3个关键帧（同样或者更低层级）观测到 + for(size_t i=0, iend=vpMapPoints.size(); iisBad()) + { + if(!mbMonocular) + { + // 对于双目，仅考虑近处（不超过基线的40倍 ）的地图点 + if(pKF->mvDepth[i]>pKF->mThDepth || pKF->mvDepth[i]<0) + continue; + } + + nMPs++; + // pMP->Observations() 是观测到该地图点的相机总数目（单目1，双目2） + if(pMP->Observations()>thObs) + { + const int &scaleLevel = (pKF -> NLeft == -1) ? pKF->mvKeysUn[i].octave + : (i < pKF -> NLeft) ? pKF -> mvKeys[i].octave + : pKF -> mvKeysRight[i].octave; + const map> observations = pMP->GetObservations(); + int nObs=0; + // 遍历观测到该地图点的关键帧 + for(map>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) + { + KeyFrame* pKFi = mit->first; + if(pKFi==pKF) + continue; + tuple indexes = mit->second; + int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes); + int scaleLeveli = -1; + if(pKFi -> NLeft == -1) + scaleLeveli = pKFi->mvKeysUn[leftIndex].octave; + else { + if (leftIndex != -1) { + scaleLeveli = pKFi->mvKeys[leftIndex].octave; + } + if (rightIndex != -1) { + int rightLevel = pKFi->mvKeysRight[rightIndex - pKFi->NLeft].octave; + scaleLeveli = (scaleLeveli == -1 || scaleLeveli > rightLevel) ? rightLevel + : scaleLeveli; + } + } + // 尺度约束：为什么pKF 尺度+1 要大于等于 pKFi 尺度？ + // 回答：因为同样或更低金字塔层级的地图点更准确 + if(scaleLeveli<=scaleLevel+1) + { + nObs++; + // 已经找到3个满足条件的关键帧，就停止不找了 + if(nObs>thObs) + break; + } + } + // 地图点至少被3个关键帧观测到，就记录为冗余点，更新冗余点计数数目 + if(nObs>thObs) + { + nRedundantObservations++; + } + } + } + } + } + + // Step 4：该关键帧90%以上的有效地图点被判断为冗余的，则删除该关键帧 + if(nRedundantObservations>redundant_th*nMPs) + { + // imu模式下需要更改前后关键帧的连续性，且预积分要叠加起来 + if (mbInertial) + { + // 关键帧少于Nd个，跳过不删 + if (mpAtlas->KeyFramesInMap()<=Nd) + continue; + + // 关键帧与当前关键帧id差一个，跳过不删 + if(pKF->mnId>(mpCurrentKeyFrame->mnId-2)) + continue; + + // 关键帧具有前后关键帧 + if(pKF->mPrevKF && pKF->mNextKF) + { + const float t = pKF->mNextKF->mTimeStamp-pKF->mPrevKF->mTimeStamp; + // 下面两个括号里的内容一模一样 + // imu初始化了，且距当前帧的ID超过21，且前后两个关键帧时间间隔小于3s + // 或者时间间隔小于0.5s + if((bInitImu && (pKF->mnIdmNextKF->mpImuPreintegrated->MergePrevious(pKF->mpImuPreintegrated); + pKF->mNextKF->mPrevKF = pKF->mPrevKF; + pKF->mPrevKF->mNextKF = pKF->mNextKF; + pKF->mNextKF = NULL; + pKF->mPrevKF = NULL; + pKF->SetBadFlag(); + } + // 没经过imu初始化的第三阶段，且关键帧与其前一个关键帧的距离小于0.02m，且前后两个关键帧时间间隔小于3s + else if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA2() && ((pKF->GetImuPosition()-pKF->mPrevKF->GetImuPosition()).norm()<0.02) && (t<3)) + { + pKF->mNextKF->mpImuPreintegrated->MergePrevious(pKF->mpImuPreintegrated); + pKF->mNextKF->mPrevKF = pKF->mPrevKF; + pKF->mPrevKF->mNextKF = pKF->mNextKF; + pKF->mNextKF = NULL; + pKF->mPrevKF = NULL; + pKF->SetBadFlag(); + } + } + } + // 非imu就没那么多事儿了，直接干掉 + else + { + pKF->SetBadFlag(); + } + } + // 遍历共视关键帧个数超过一定，就不弄了 + if((count > 20 && mbAbortBA) || count>100) + { + break; + } + } +} + +/** + * @brief 请求reset + */ +void LocalMapping::RequestReset() +{ + { + unique_lock lock(mMutexReset); + cout << "LM: Map reset recieved" << endl; + mbResetRequested = true; + } + cout << "LM: Map reset, waiting..." << endl; + + // 一直等到局部建图线程响应之后才可以退出 + while(1) + { + { + unique_lock lock2(mMutexReset); + if(!mbResetRequested) + break; + } + usleep(3000); + } + cout << "LM: Map reset, Done!!!" << endl; +} + +/** + * @brief 接收重置当前地图的信号 + */ +void LocalMapping::RequestResetActiveMap(Map* pMap) +{ + { + unique_lock lock(mMutexReset); + cout << "LM: Active map reset recieved" << endl; + mbResetRequestedActiveMap = true; + mpMapToReset = pMap; + } + cout << "LM: Active map reset, waiting..." << endl; + + while(1) + { + { + unique_lock lock2(mMutexReset); + if(!mbResetRequestedActiveMap) + break; + } + usleep(3000); + } + cout << "LM: Active map reset, Done!!!" << endl; +} + +/** + * @brief 检查是否有复位线程的请求 + */ +void LocalMapping::ResetIfRequested() +{ + bool executed_reset = false; + { + unique_lock lock(mMutexReset); + // 执行复位操作:清空关键帧缓冲区,清空待cull的地图点缓冲 + if(mbResetRequested) + { + executed_reset = true; + + cout << "LM: Reseting Atlas in Local Mapping..." << endl; + mlNewKeyFrames.clear(); + mlpRecentAddedMapPoints.clear(); + // 恢复为false表示复位过程完成 + mbResetRequested = false; + mbResetRequestedActiveMap = false; + + // Inertial parameters + mTinit = 0.f; + mbNotBA2 = true; + mbNotBA1 = true; + mbBadImu=false; + + mIdxInit=0; + + cout << "LM: End reseting Local Mapping..." << endl; + } + + if(mbResetRequestedActiveMap) { + executed_reset = true; + cout << "LM: Reseting current map in Local Mapping..." << endl; + mlNewKeyFrames.clear(); + mlpRecentAddedMapPoints.clear(); + + // Inertial parameters + mTinit = 0.f; + mbNotBA2 = true; + mbNotBA1 = true; + mbBadImu=false; + + mbResetRequested = false; + mbResetRequestedActiveMap = false; + cout << "LM: End reseting Local Mapping..." << endl; + } + } + if(executed_reset) + cout << "LM: Reset free the mutex" << endl; + +} + +/** + * @brief 请求终止当前线程 + */ +void LocalMapping::RequestFinish() +{ + unique_lock lock(mMutexFinish); + mbFinishRequested = true; +} + +/** + * @brief 查看完成信号，跳出while循环 + */ +bool LocalMapping::CheckFinish() +{ + unique_lock lock(mMutexFinish); + return mbFinishRequested; +} + +/** + * @brief 设置当前线程已经真正地结束了 + */ +void LocalMapping::SetFinish() +{ + unique_lock lock(mMutexFinish); + mbFinished = true; + unique_lock lock2(mMutexStop); + mbStopped = true; +} + +/** + * @brief 当前线程的run函数是否已经终止 + */ +bool LocalMapping::isFinished() +{ + unique_lock lock(mMutexFinish); + return mbFinished; +} + +/** + * @brief imu初始化 + * @param priorG 陀螺仪偏置的信息矩阵系数，主动设置时一般bInit为true，也就是只优化最后一帧的偏置，这个数会作为计算信息矩阵时使用 + * @param priorA 加速度计偏置的信息矩阵系数 + * @param bFIBA 是否做BA优化，目前都为true + */ +void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) +{ + // 1. 将所有关键帧放入列表及向量里，且查看是否满足初始化条件 + if (mbResetRequested) + return; + + float minTime; + int nMinKF; + // 从时间及帧数上限制初始化，不满足下面的不进行初始化 + if (mbMonocular) + { + minTime = 2.0; + nMinKF = 10; + } + else + { + minTime = 1.0; + nMinKF = 10; + } + + // 当前地图大于10帧才进行初始化 + if(mpAtlas->KeyFramesInMap() lpKF; + KeyFrame* pKF = mpCurrentKeyFrame; + while(pKF->mPrevKF) + { + lpKF.push_front(pKF); + pKF = pKF->mPrevKF; + } + lpKF.push_front(pKF); + // 同样内容再构建一个和lpKF一样的容器vpKF + vector vpKF(lpKF.begin(),lpKF.end()); + if(vpKF.size()mTimeStamp; + if(mpCurrentKeyFrame->mTimeStamp-mFirstTsGetMap()->isImuInitialized()) + { + Eigen::Matrix3f Rwg; + Eigen::Vector3f dirG; + dirG.setZero(); + + int have_imu_num = 0; + for(vector::iterator itKF = vpKF.begin(); itKF!=vpKF.end(); itKF++) + { + if (!(*itKF)->mpImuPreintegrated) + continue; + if (!(*itKF)->mPrevKF) + continue; + + have_imu_num++; + // 初始化时关于速度的预积分定义Ri.t()*(s*Vj - s*Vi - Rwg*g*tij) + dirG -= (*itKF)->mPrevKF->GetImuRotation() * (*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity(); + // 求取实际的速度，位移/时间 + Eigen::Vector3f _vel = ((*itKF)->GetImuPosition() - (*itKF)->mPrevKF->GetImuPosition())/(*itKF)->mpImuPreintegrated->dT; + (*itKF)->SetVelocity(_vel); + (*itKF)->mPrevKF->SetVelocity(_vel); + } + + if (have_imu_num < 6) + { + cout << "imu初始化失败, 由于带有imu预积分信息的关键帧数量太少" << endl; + bInitializing=false; + mbBadImu = true; + return; + } + + // dirG = sV1 - sVn + n*Rwg*g*t + // 归一化，约等于重力在世界坐标系下的方向 + dirG = dirG/dirG.norm(); + // 原本的重力方向 + Eigen::Vector3f gI(0.0f, 0.0f, -1.0f); + // 求“重力在重力坐标系下的方向”与的“重力在世界坐标系（纯视觉）下的方向”叉乘 + Eigen::Vector3f v = gI.cross(dirG); + // 求叉乘模长 + const float nv = v.norm(); + // 求转角大小 + const float cosg = gI.dot(dirG); + const float ang = acos(cosg); + // v/nv 表示垂直于两个向量的轴 ang 表示转的角度，组成角轴 + Eigen::Vector3f vzg = v*ang/nv; + // 获得重力坐标系到世界坐标系的旋转矩阵的初值 + Rwg = Sophus::SO3f::exp(vzg).matrix(); + mRwg = Rwg.cast(); + mTinit = mpCurrentKeyFrame->mTimeStamp-mFirstTs; + } + else + { + mRwg = Eigen::Matrix3d::Identity(); + mbg = mpCurrentKeyFrame->GetGyroBias().cast(); + mba = mpCurrentKeyFrame->GetAccBias().cast(); + } + + mScale=1.0; + + // 暂时没发现在别的地方出现过 + mInitTime = mpTracker->mLastFrame.mTimeStamp-vpKF.front()->mTimeStamp; + + std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now(); + // 3. 计算残差及偏置差，优化尺度重力方向及速度偏置，偏置先验为0，双目时不优化尺度 + Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale, mbg, mba, mbMonocular, infoInertial, false, false, priorG, priorA); + std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); + + // 尺度太小的话初始化认为失败 + if (mScale<1e-1) + { + cout << "scale too small" << endl; + bInitializing=false; + return; + } + + // 到此时为止，前面做的东西没有改变map + + // Before this line we are not changing the map + { + unique_lock lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate); + // 尺度变化超过设定值，或者非单目时（无论带不带imu，但这个函数只在带imu时才执行，所以这个可以理解为双目imu） + if ((fabs(mScale - 1.f) > 0.00001) || !mbMonocular) { + // 4.1 恢复重力方向与尺度信息 + Sophus::SE3f Twg(mRwg.cast().transpose(), Eigen::Vector3f::Zero()); + mpAtlas->GetCurrentMap()->ApplyScaledRotation(Twg, mScale, true); + // 4.2 更新普通帧的位姿，主要是当前帧与上一帧 + mpTracker->UpdateFrameIMU(mScale, vpKF[0]->GetImuBias(), mpCurrentKeyFrame); + } + + // Check if initialization OK + // 即使初始化成功后面还会执行这个函数重新初始化 + // 在之前没有初始化成功情况下（此时刚刚初始化成功）对每一帧都标记，后面的kf全部都在tracking里面标记为true + // 也就是初始化之前的那些关键帧即使有imu信息也不算 + if (!mpAtlas->isImuInitialized()) + for (int i = 0; i < N; i++) { + KeyFrame *pKF2 = vpKF[i]; + pKF2->bImu = true; + } + } + + // TODO 这步更新是否有必要做待研究，0.4版本是放在FullInertialBA下面做的 + // 这个版本FullInertialBA不直接更新位姿及三位点了 + mpTracker->UpdateFrameIMU(1.0,vpKF[0]->GetImuBias(),mpCurrentKeyFrame); + + // 设置经过初始化了 + if (!mpAtlas->isImuInitialized()) + { + // ! 重要！标记初始化成功 + mpAtlas->SetImuInitialized(); + mpTracker->t0IMU = mpTracker->mCurrentFrame.mTimeStamp; + mpCurrentKeyFrame->bImu = true; + } + + std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now(); + // 代码里都为true + if (bFIBA) + { + // 5. 承接上一步纯imu优化，按照之前的结果更新了尺度信息及适应重力方向，所以要结合地图进行一次视觉加imu的全局优化，这次带了MP等信息 + // ! 1.0版本里面不直接赋值了，而是将所有优化后的信息保存到变量里面 + if (priorA!=0.f) + Optimizer::FullInertialBA(mpAtlas->GetCurrentMap(), 100, false, mpCurrentKeyFrame->mnId, NULL, true, priorG, priorA); + else + Optimizer::FullInertialBA(mpAtlas->GetCurrentMap(), 100, false, mpCurrentKeyFrame->mnId, NULL, false); + } + + std::chrono::steady_clock::time_point t5 = std::chrono::steady_clock::now(); + + Verbose::PrintMess("Global Bundle Adjustment finished\nUpdating map ...", Verbose::VERBOSITY_NORMAL); + + // Get Map Mutex + unique_lock lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate); + + unsigned long GBAid = mpCurrentKeyFrame->mnId; + + // Process keyframes in the queue + // 6. 处理一下新来的关键帧，这些关键帧没有参与优化，但是这部分bInitializing为true，只在第2次跟第3次初始化会有新的关键帧进来 + // 这部分关键帧也需要被更新 + while(CheckNewKeyFrames()) + { + ProcessNewKeyFrame(); + vpKF.push_back(mpCurrentKeyFrame); + lpKF.push_back(mpCurrentKeyFrame); + } + + // Correct keyframes starting at map first keyframe + // 7. 更新位姿与三维点 + // 获取地图中初始关键帧，第一帧肯定经过修正的 + list lpKFtoCheck(mpAtlas->GetCurrentMap()->mvpKeyFrameOrigins.begin(),mpAtlas->GetCurrentMap()->mvpKeyFrameOrigins.end()); + + // 初始就一个关键帧，顺藤摸瓜找到父子相连的所有关键帧 + // 类似于树的广度优先搜索，其实也就是根据父子关系遍历所有的关键帧，有的参与了FullInertialBA有的没参与 + while(!lpKFtoCheck.empty()) + { + // 7.1 获得这个关键帧的子关键帧 + KeyFrame* pKF = lpKFtoCheck.front(); + const set sChilds = pKF->GetChilds(); + Sophus::SE3f Twc = pKF->GetPoseInverse(); // 获得关键帧的优化前的位姿 + // 7.2 遍历这个关键帧所有的子关键帧 + for(set::const_iterator sit=sChilds.begin();sit!=sChilds.end();sit++) + { + // 确认是否能用 + KeyFrame* pChild = *sit; + if(!pChild || pChild->isBad()) + continue; + + // 这个判定为true表示pChild没有参与前面的优化，因此要根据已经优化过的更新，结果全部暂存至变量 + if(pChild->mnBAGlobalForKF!=GBAid) + { + // pChild->GetPose()也是优化前的位姿，Twc也是优化前的位姿 + // 7.3 因此他们的相对位姿是比较准的，可以用于更新pChild的位姿 + Sophus::SE3f Tchildc = pChild->GetPose() * Twc; + // 使用相对位姿，根据pKF优化后的位姿更新pChild位姿，最后结果都暂时放于mTcwGBA + pChild->mTcwGBA = Tchildc * pKF->mTcwGBA; + + // 7.4 使用相同手段更新速度 + Sophus::SO3f Rcor = pChild->mTcwGBA.so3().inverse() * pChild->GetPose().so3(); + if(pChild->isVelocitySet()){ + pChild->mVwbGBA = Rcor * pChild->GetVelocity(); + } + else { + Verbose::PrintMess("Child velocity empty!! ", Verbose::VERBOSITY_NORMAL); + } + + pChild->mBiasGBA = pChild->GetImuBias(); + pChild->mnBAGlobalForKF = GBAid; + + } + // 加入到list中，再去寻找pChild的子关键帧 + lpKFtoCheck.push_back(pChild); + } + + // 7.5 此时pKF的利用价值就没了，但是里面的数值还都是优化前的，优化后的全部放于类似mTcwGBA这样的变量中 + // 所以要更新到正式的状态里，另外mTcwBefGBA要记录更新前的位姿，用于同样的手段更新三维点用 + pKF->mTcwBefGBA = pKF->GetPose(); + pKF->SetPose(pKF->mTcwGBA); + + // 速度偏置同样更新 + if(pKF->bImu) + { + pKF->mVwbBefGBA = pKF->GetVelocity(); + pKF->SetVelocity(pKF->mVwbGBA); + pKF->SetNewBias(pKF->mBiasGBA); + } else { + cout << "KF " << pKF->mnId << " not set to inertial!! \n"; + } + + // pop + lpKFtoCheck.pop_front(); + } + + // Correct MapPoints + // 8. 更新三维点，三维点在优化后同样没有正式的更新，而是找了个中间变量保存了优化后的数值 + const vector vpMPs = mpAtlas->GetCurrentMap()->GetAllMapPoints(); + + for(size_t i=0; iisBad()) + continue; + + // 8.1 如果这个点参与了全局优化，那么直接使用优化后的值来赋值 + if(pMP->mnBAGlobalForKF==GBAid) + { + // If optimized by Global BA, just update + pMP->SetWorldPos(pMP->mPosGBA); + } + // 如果没有参与，与关键帧的更新方式类似 + else + { + // Update according to the correction of its reference keyframe + KeyFrame* pRefKF = pMP->GetReferenceKeyFrame(); + + if(pRefKF->mnBAGlobalForKF!=GBAid) + continue; + + // Map to non-corrected camera + // 8.2 根据优化前的世界坐标系下三维点的坐标以及优化前的关键帧位姿计算这个点在关键帧下的坐标 + Eigen::Vector3f Xc = pRefKF->mTcwBefGBA * pMP->GetWorldPos(); + + // Backproject using corrected camera + // 8.3 根据优化后的位姿转到世界坐标系下作为这个点优化后的三维坐标 + pMP->SetWorldPos(pRefKF->GetPoseInverse() * Xc); + } + } + + Verbose::PrintMess("Map updated!", Verbose::VERBOSITY_NORMAL); + + mnKFs=vpKF.size(); + mIdxInit++; + + // 9. 再有新的来就不要了~不然陷入无限套娃了 + for(list::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++) + { + (*lit)->SetBadFlag(); + delete *lit; + } + mlNewKeyFrames.clear(); + + mpTracker->mState=Tracking::OK; + bInitializing = false; + + mpCurrentKeyFrame->GetMap()->IncreaseChangeIndex(); + + return; +} + +/** + * @brief 通过BA优化进行尺度更新，关键帧小于100，使用了所有关键帧的信息，但只优化尺度和重力方向。每10s在这里的时间段内时多次进行尺度更新 + */ +void LocalMapping::ScaleRefinement() +{ + // Minimum number of keyframes to compute a solution + // Minimum time (seconds) between first and last keyframe to compute a solution. Make the difference between monocular and stereo + // unique_lock lock0(mMutexImuInit); + if (mbResetRequested) + return; + + // Retrieve all keyframes in temporal order + // 1. 检索所有的关键帧（当前地图） + list lpKF; + KeyFrame* pKF = mpCurrentKeyFrame; + while(pKF->mPrevKF) + { + lpKF.push_front(pKF); + pKF = pKF->mPrevKF; + } + lpKF.push_front(pKF); + vector vpKF(lpKF.begin(),lpKF.end()); + + // 加入新添加的帧 + while(CheckNewKeyFrames()) + { + ProcessNewKeyFrame(); + vpKF.push_back(mpCurrentKeyFrame); + lpKF.push_back(mpCurrentKeyFrame); + } + + const int N = vpKF.size(); + + // 2. 更新旋转与尺度 + // 待优化变量的初值 + mRwg = Eigen::Matrix3d::Identity(); + mScale=1.0; + + std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now(); + // 优化重力方向与尺度 + Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale); + std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); + + if (mScale<1e-1) // 1e-1 + { + cout << "scale too small" << endl; + bInitializing=false; + return; + } + + Sophus::SO3d so3wg(mRwg); + + + // Before this line we are not changing the map + // 3. 开始更新地图 + unique_lock lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate); + std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now(); + // 3.1 如果尺度更新较多，或是在双目imu情况下更新地图 + // 0.4版本这个值为0.00001 + if ((fabs(mScale-1.f)>0.002)||!mbMonocular) + { + Sophus::SE3f Tgw(mRwg.cast().transpose(),Eigen::Vector3f::Zero()); + mpAtlas->GetCurrentMap()->ApplyScaledRotation(Tgw,mScale,true); + mpTracker->UpdateFrameIMU(mScale,mpCurrentKeyFrame->GetImuBias(),mpCurrentKeyFrame); + } + std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now(); + + // 3.2 优化的这段时间新进来的kf全部清空不要 + for(list::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++) + { + (*lit)->SetBadFlag(); + delete *lit; + } + mlNewKeyFrames.clear(); + + double t_inertial_only = std::chrono::duration_cast >(t1 - t0).count(); + + // To perform pose-inertial opt w.r.t. last keyframe + mpCurrentKeyFrame->GetMap()->IncreaseChangeIndex(); + + return; +} + +/** + * @brief 返回是否正在做IMU的初始化，在tracking里面使用，如果为true，暂不添加关键帧 + */ +bool LocalMapping::IsInitializing() +{ + return bInitializing; +} + +/** + * @brief 获取当前关键帧的时间戳，System::GetTimeFromIMUInit()中调用 + */ +double LocalMapping::GetCurrKFTime() +{ + + if (mpCurrentKeyFrame) + { + return mpCurrentKeyFrame->mTimeStamp; + } + else + return 0.0; +} + +/** + * @brief 获取当前关键帧 + */ +KeyFrame* LocalMapping::GetCurrKF() +{ + return mpCurrentKeyFrame; +} + +} //namespace ORB_SLAM diff --git a/ORB-SLAM3-UESTC/Workspace/src/LoopClosing.cc b/ORB-SLAM3-UESTC/Workspace/src/LoopClosing.cc new file mode 100644 index 0000000..3df41aa --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/LoopClosing.cc @@ -0,0 +1,3156 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#include "LoopClosing.h" + +#include "Sim3Solver.h" +#include "Converter.h" +#include "Optimizer.h" +#include "ORBmatcher.h" +#include "G2oTypes.h" + +#include +#include + + +namespace ORB_SLAM3 +{ + +/** + * @brief 回环线程构造函数 + * @param pAtlas atlas + * @param pKFDB 关键帧词典数据库 + * @param bFixScale 除了单目都为true，包括imu单目 + * @param bActiveLC 开启回环，默认是开启的 + */ +LoopClosing::LoopClosing(Atlas *pAtlas, KeyFrameDatabase *pDB, ORBVocabulary *pVoc, const bool bFixScale, const bool bActiveLC): + mbResetRequested(false), mbResetActiveMapRequested(false), mbFinishRequested(false), mbFinished(true), mpAtlas(pAtlas), + mpKeyFrameDB(pDB), mpORBVocabulary(pVoc), mpMatchedKF(NULL), mLastLoopKFid(0), mbRunningGBA(false), mbFinishedGBA(true), + mbStopGBA(false), mpThreadGBA(NULL), mbFixScale(bFixScale), mnFullBAIdx(0), mnLoopNumCoincidences(0), mnMergeNumCoincidences(0), + mbLoopDetected(false), mbMergeDetected(false), mnLoopNumNotFound(0), mnMergeNumNotFound(0), mbActiveLC(bActiveLC) +{ + // 连续性阈值 + mnCovisibilityConsistencyTh = 3; + mpLastCurrentKF = static_cast(NULL); + +#ifdef REGISTER_TIMES + + vdDataQuery_ms.clear(); + vdEstSim3_ms.clear(); + vdPRTotal_ms.clear(); + + vdMergeMaps_ms.clear(); + vdWeldingBA_ms.clear(); + vdMergeOptEss_ms.clear(); + vdMergeTotal_ms.clear(); + vnMergeKFs.clear(); + vnMergeMPs.clear(); + nMerges = 0; + + vdLoopFusion_ms.clear(); + vdLoopOptEss_ms.clear(); + vdLoopTotal_ms.clear(); + vnLoopKFs.clear(); + nLoop = 0; + + vdGBA_ms.clear(); + vdUpdateMap_ms.clear(); + vdFGBATotal_ms.clear(); + vnGBAKFs.clear(); + vnGBAMPs.clear(); + nFGBA_exec = 0; + nFGBA_abort = 0; + +#endif + + mstrFolderSubTraj = "SubTrajectories/"; + mnNumCorrection = 0; + mnCorrectionGBA = 0; +} + +void LoopClosing::SetTracker(Tracking *pTracker) +{ + mpTracker=pTracker; +} + +void LoopClosing::SetLocalMapper(LocalMapping *pLocalMapper) +{ + mpLocalMapper=pLocalMapper; +} + +/** + * @brief 回环线程主函数 + */ +void LoopClosing::Run() +{ + mbFinished =false; + + // 线程主循环 + while(1) + { + + //NEW LOOP AND MERGE DETECTION ALGORITHM + //---------------------------- + + // Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的 + // 在LocalMapping中通过 InsertKeyFrame 将关键帧插入闭环检测队列mlpLoopKeyFrameQueue + // Step 1 查看闭环检测队列mlpLoopKeyFrameQueue中有没有关键帧进来 + if(CheckNewKeyFrames()) + { + // 这部分后续未使用 + if(mpLastCurrentKF) + { + mpLastCurrentKF->mvpLoopCandKFs.clear(); + mpLastCurrentKF->mvpMergeCandKFs.clear(); + } +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartPR = std::chrono::steady_clock::now(); +#endif + // Step 2 检测有没有共视的区域 + bool bFindedRegion = NewDetectCommonRegions(); + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndPR = std::chrono::steady_clock::now(); + + double timePRTotal = std::chrono::duration_cast >(time_EndPR - time_StartPR).count(); + vdPRTotal_ms.push_back(timePRTotal); +#endif + if(bFindedRegion) + { + // Step 3 如果检测到融合（当前关键帧与其他地图有关联）, 则合并地图 + if(mbMergeDetected) + { + // 在imu没有初始化就放弃融合 + if ((mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) && + (!mpCurrentKF->GetMap()->isImuInitialized())) + { + cout << "IMU is not initilized, merge is aborted" << endl; + } + else + { + // 拿到融合帧在自己地图所在坐标系(w2)下的位姿 + Sophus::SE3d mTmw = mpMergeMatchedKF->GetPose().cast(); + g2o::Sim3 gSmw2(mTmw.unit_quaternion(), mTmw.translation(), 1.0); + // 拿到当前帧在自己地图所在坐标系(w1)下的位姿 + Sophus::SE3d mTcw = mpCurrentKF->GetPose().cast(); + g2o::Sim3 gScw1(mTcw.unit_quaternion(), mTcw.translation(), 1.0); + // 根据共同区域检测时的Sim3结果得到当前帧在w2下的位姿 + // mg2oMergeSlw 里存放的是融合候选关键帧所在的世界坐标系w2到当前帧的Sim3位姿 + // l = c , w2是融合候选关键帧所在的世界坐标系 + g2o::Sim3 gSw2c = mg2oMergeSlw.inverse(); + // 这个没有用到 : 融合帧在w1下的位姿 + g2o::Sim3 gSw1m = mg2oMergeSlw; + + // 记录焊接变换(Sim3) T_w2_w1 , 这个量实际是两个地图坐标系的关系 T_w2_w1 = T_w2_c * T_c_w1 + mSold_new = (gSw2c * gScw1); + + // 如果是imu模式 + if(mpCurrentKF->GetMap()->IsInertial() && mpMergeMatchedKF->GetMap()->IsInertial()) + { + cout << "Merge check transformation with IMU" << endl; + // 如果尺度变换太大, 认为累积误差较大，则放弃融合 + if(mSold_new.scale()<0.90||mSold_new.scale()>1.1){ + mpMergeLastCurrentKF->SetErase(); + mpMergeMatchedKF->SetErase(); + mnMergeNumCoincidences = 0; + mvpMergeMatchedMPs.clear(); + mvpMergeMPs.clear(); + mnMergeNumNotFound = 0; + mbMergeDetected = false; + Verbose::PrintMess("scale bad estimated. Abort merging", Verbose::VERBOSITY_NORMAL); + continue; + } + // If inertial, force only yaw + // 如果是imu模式并且完成了初始化,强制将焊接变换的 roll 和 pitch 设为0 + // 通过物理约束来保证两个坐标轴都是水平的 + if ((mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) && + mpCurrentKF->GetMap()->GetIniertialBA1()) + { + Eigen::Vector3d phi = LogSO3(mSold_new.rotation().toRotationMatrix()); + phi(0)=0; + phi(1)=0; + mSold_new = g2o::Sim3(ExpSO3(phi),mSold_new.translation(),1.0); + } + } + + // 这个变量没有用到 + mg2oMergeSmw = gSmw2 * gSw2c * gScw1; + + // 更新mg2oMergeScw + mg2oMergeScw = mg2oMergeSlw; + + //mpTracker->SetStepByStep(true); + + Verbose::PrintMess("*Merge detected", Verbose::VERBOSITY_QUIET); + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartMerge = std::chrono::steady_clock::now(); + + nMerges += 1; +#endif + // TODO UNCOMMENT + // 如果是imu模式,则开启 Visual-Inertial Map Merging + // 如果是纯视觉模式,则开启 Visual-Welding Map Merging + if (mpTracker->mSensor==System::IMU_MONOCULAR ||mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) + MergeLocal2(); + else + MergeLocal(); + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndMerge = std::chrono::steady_clock::now(); + + double timeMergeTotal = std::chrono::duration_cast >(time_EndMerge - time_StartMerge).count(); + vdMergeTotal_ms.push_back(timeMergeTotal); +#endif + + Verbose::PrintMess("Merge finished!", Verbose::VERBOSITY_QUIET); + } + // 记录时间戳 + vdPR_CurrentTime.push_back(mpCurrentKF->mTimeStamp); + vdPR_MatchedTime.push_back(mpMergeMatchedKF->mTimeStamp); + // 标记Place recognition结果为地图融合 + vnPR_TypeRecogn.push_back(1); + + // Reset all variables + // 重置所有融合相关变量 + mpMergeLastCurrentKF->SetErase(); + mpMergeMatchedKF->SetErase(); + mnMergeNumCoincidences = 0; + mvpMergeMatchedMPs.clear(); + mvpMergeMPs.clear(); + mnMergeNumNotFound = 0; + mbMergeDetected = false; + + // 重置所有回环相关变量, 说明对与当前帧同时有回环和融合的情况只进行融合 + if(mbLoopDetected) + { + // Reset Loop variables + mpLoopLastCurrentKF->SetErase(); + mpLoopMatchedKF->SetErase(); + mnLoopNumCoincidences = 0; + mvpLoopMatchedMPs.clear(); + mvpLoopMPs.clear(); + mnLoopNumNotFound = 0; + mbLoopDetected = false; + } + + } + + // Step 4 如果(没有检测到融合)检测到回环, 则回环矫正 + if(mbLoopDetected) + { + // 标记时间戳 + bool bGoodLoop = true; + vdPR_CurrentTime.push_back(mpCurrentKF->mTimeStamp); + vdPR_MatchedTime.push_back(mpLoopMatchedKF->mTimeStamp); + vnPR_TypeRecogn.push_back(0); + + Verbose::PrintMess("*Loop detected", Verbose::VERBOSITY_QUIET); + // 更新 mg2oLoopScw + mg2oLoopScw = mg2oLoopSlw; //*mvg2oSim3LoopTcw[nCurrentIndex]; + // 如果是带imu的模式则做下判断，纯视觉跳过 + if(mpCurrentKF->GetMap()->IsInertial()) + { + // 拿到当前关键帧相对于世界坐标系的位姿 + Sophus::SE3d Twc = mpCurrentKF->GetPoseInverse().cast(); + g2o::Sim3 g2oTwc(Twc.unit_quaternion(),Twc.translation(),1.0); + + // mg2oLoopScw是通过回环检测的Sim3计算出的回环矫正后的当前关键帧的初始位姿, Twc是当前关键帧回环矫正前的位姿. + // g2oSww_new 可以理解为correction + g2o::Sim3 g2oSww_new = g2oTwc*mg2oLoopScw; + + // 拿到 roll ,pitch ,yaw + Eigen::Vector3d phi = LogSO3(g2oSww_new.rotation().toRotationMatrix()); + cout << "phi = " << phi.transpose() << endl; + // 这里算是通过imu重力方向验证回环结果, 如果pitch或roll角度偏差稍微有一点大,则回环失败. 对yaw容忍比较大(20度) + if (fabs(phi(0))<0.008f && fabs(phi(1))<0.008f && fabs(phi(2))<0.349f) + { + // 如果是imu模式 + if(mpCurrentKF->GetMap()->IsInertial()) + { + // If inertial, force only yaw + // 如果是imu模式,强制将焊接变换的的 roll 和 pitch 设为0 + if ((mpTracker->mSensor==System::IMU_MONOCULAR ||mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) && + mpCurrentKF->GetMap()->GetIniertialBA2()) + { + phi(0)=0; + phi(1)=0; + g2oSww_new = g2o::Sim3(ExpSO3(phi),g2oSww_new.translation(),1.0); + mg2oLoopScw = g2oTwc.inverse()*g2oSww_new; + } + } + + } + else + { + cout << "BAD LOOP!!!" << endl; + bGoodLoop = false; + } + + } + + if (bGoodLoop) { + + mvpLoopMapPoints = mvpLoopMPs; + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartLoop = std::chrono::steady_clock::now(); + + nLoop += 1; + +#endif + // 开启回环矫正 + CorrectLoop(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndLoop = std::chrono::steady_clock::now(); + + double timeLoopTotal = std::chrono::duration_cast >(time_EndLoop - time_StartLoop).count(); + vdLoopTotal_ms.push_back(timeLoopTotal); +#endif + + mnNumCorrection += 1; + } + + // Reset all variables + // 重置所有的回环变量 + mpLoopLastCurrentKF->SetErase(); + mpLoopMatchedKF->SetErase(); + mnLoopNumCoincidences = 0; + mvpLoopMatchedMPs.clear(); + mvpLoopMPs.clear(); + mnLoopNumNotFound = 0; + mbLoopDetected = false; + } + + } + mpLastCurrentKF = mpCurrentKF; + } + // 查看是否有外部线程请求复位当前线程 + ResetIfRequested(); + + // 查看外部线程是否有终止当前线程的请求,如果有的话就跳出这个线程的主函数的主循环 + if(CheckFinish()){ + break; + } + + usleep(5000); + } + + SetFinish(); +} + +/** + * @brief 插入关键帧 + */ +void LoopClosing::InsertKeyFrame(KeyFrame *pKF) +{ + unique_lock lock(mMutexLoopQueue); + if(pKF->mnId!=0) + mlpLoopKeyFrameQueue.push_back(pKF); +} + +/** + * @brief 查看有没有未处理的关键帧 + */ +bool LoopClosing::CheckNewKeyFrames() +{ + unique_lock lock(mMutexLoopQueue); + return(!mlpLoopKeyFrameQueue.empty()); +} + +/** + * @brief 检测有没有共同区域,包括检测回环和融合匹配,sim3计算,验证 + * 对应于ORB-SLAM2里的函数DetectLoop + * @return true + * @return false + */ +bool LoopClosing::NewDetectCommonRegions() +{ + // To deactivate placerecognition. No loopclosing nor merging will be performed + // 如果一开始就不做回环的话这里就退出了，这个线程也就名存实亡了 + if(!mbActiveLC) + return false; + + { + // Step 1 从队列中取出一个关键帧,作为当前检测共同区域的关键帧 + unique_lock lock(mMutexLoopQueue); + // 从队列头开始取，也就是先取早进来的关键帧 + mpCurrentKF = mlpLoopKeyFrameQueue.front(); + // 取出关键帧后从队列里弹出该关键帧 + mlpLoopKeyFrameQueue.pop_front(); + // Avoid that a keyframe can be erased while it is being process by this thread + // 设置当前关键帧不要在优化的过程中被删除 + mpCurrentKF->SetNotErase(); + mpCurrentKF->mbCurrentPlaceRecognition = true; + // 当前关键帧对应的地图 + mpLastMap = mpCurrentKF->GetMap(); + } + + // Step 2 在某些情况下不进行共同区域检测 + // 1.imu模式下还没经过第二阶段初始化则不考虑 + if(mpLastMap->IsInertial() && !mpLastMap->GetIniertialBA2()) + { + mpKeyFrameDB->add(mpCurrentKF); + mpCurrentKF->SetErase(); + return false; + } + + // 2.双目模式下且当前地图关键帧数少于5则不考虑 + if(mpTracker->mSensor == System::STEREO && mpLastMap->GetAllKeyFrames().size() < 5) //12 + { + // cout << "LoopClousure: Stereo KF inserted without check: " << mpCurrentKF->mnId << endl; + mpKeyFrameDB->add(mpCurrentKF); + mpCurrentKF->SetErase(); + return false; + } + + // 3.当前地图关键帧少于12则不进行检测 + if(mpLastMap->GetAllKeyFrames().size() < 12) + { + // cout << "LoopClousure: Stereo KF inserted without check, map is small: " << mpCurrentKF->mnId << endl; + mpKeyFrameDB->add(mpCurrentKF); + mpCurrentKF->SetErase(); + return false; + } + + //cout << "LoopClousure: Checking KF: " << mpCurrentKF->mnId << endl; + + //Check the last candidates with geometric validation + // Loop candidates + // Step 3 基于前一帧的历史信息判断是否进行时序几何校验,注意这里是基于共视几何校验失败才会运行的代码,阅读代码的时候可以先看后面 + bool bLoopDetectedInKF = false; // 某次时序验证是否成功 + bool bCheckSpatial = false; + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartEstSim3_1 = std::chrono::steady_clock::now(); +#endif + // Step 3.1 回环的时序几何校验。注意初始化时mnLoopNumCoincidences=0, 所以可以先跳过看后面 + // 如果成功验证总次数大于0 + if(mnLoopNumCoincidences > 0) + { + bCheckSpatial = true; + // Find from the last KF candidates + // 通过上一关键帧的信息,计算新的当前帧的sim3 + // Tcl = Tcw*Twl + Sophus::SE3d mTcl = (mpCurrentKF->GetPose() * mpLoopLastCurrentKF->GetPoseInverse()).cast(); + g2o::Sim3 gScl(mTcl.unit_quaternion(),mTcl.translation(),1.0); + g2o::Sim3 gScw = gScl * mg2oLoopSlw; + int numProjMatches = 0; + vector vpMatchedMPs; + + // 通过把候选帧局部窗口内的地图点向新进来的关键帧投影来验证回环检测结果,并优化Sim3位姿 + bool bCommonRegion = DetectAndReffineSim3FromLastKF(mpCurrentKF, mpLoopMatchedKF, gScw, numProjMatches, mvpLoopMPs, vpMatchedMPs); + // 如果找到共同区域(时序验证成功一次) + if(bCommonRegion) + { + //标记时序检验成功一次 + bLoopDetectedInKF = true; + // 累计正检验的成功次数 + mnLoopNumCoincidences++; + // 不再参与新的回环检测 + mpLoopLastCurrentKF->SetErase(); + // 将当前关键帧作为上次关键帧 + mpLoopLastCurrentKF = mpCurrentKF; + mg2oLoopSlw = gScw; // 记录当前优化的结果为{last T_cw}即为 T_lw + // 记录匹配到的点 + mvpLoopMatchedMPs = vpMatchedMPs; + + // 如果验证数大于等于3则为成功回环 + mbLoopDetected = mnLoopNumCoincidences >= 3; + // 记录失败的时序校验数为0 + mnLoopNumNotFound = 0; + //! 这里的条件反了,不过对功能没什么影响,只是打印信息 + if(!mbLoopDetected) + { + cout << "PR: Loop detected with Reffine Sim3" << endl; + } + } + // 如果没找到共同区域(时序验证失败一次) + else + { + // 当前时序验证失败 + bLoopDetectedInKF = false; + // 递增失败的时序验证次数 + mnLoopNumNotFound++; + // 若果连续两帧时序验证失败则整个回环检测失败 + if(mnLoopNumNotFound >= 2) + { + // 失败后标记重置一些信息 + mpLoopLastCurrentKF->SetErase(); + mpLoopMatchedKF->SetErase(); + mnLoopNumCoincidences = 0; + mvpLoopMatchedMPs.clear(); + mvpLoopMPs.clear(); + mnLoopNumNotFound = 0; + } + + } + } + + //Merge candidates + bool bMergeDetectedInKF = false; + // Step 3.2 融合的时序几何校验: 注意初始化时mnMergeNumCoincidences=0, 所以可以先跳过看后面 + // mnMergeNumCoincidences表示成功校验总次数，初始化为0 + // 会先经过后面共视几何校验，如果小于3，会进到如下判断开始时序几何校验 + if(mnMergeNumCoincidences > 0) + { + // Find from the last KF candidates + // 通过上一关键帧的信息,计算新的当前帧的sim3 + // Tcl = Tcw*Twl + Sophus::SE3d mTcl = (mpCurrentKF->GetPose() * mpMergeLastCurrentKF->GetPoseInverse()).cast(); + + g2o::Sim3 gScl(mTcl.unit_quaternion(), mTcl.translation(), 1.0); + // mg2oMergeSlw 中的w指的是融合候选关键帧世界坐标系 + g2o::Sim3 gScw = gScl * mg2oMergeSlw; + int numProjMatches = 0; + vector vpMatchedMPs; + + // 通过把候选帧局部窗口内的地图点向新进来的关键帧投影来验证回环检测结果,并优化Sim3位姿 + bool bCommonRegion = DetectAndReffineSim3FromLastKF(mpCurrentKF, mpMergeMatchedKF, gScw, numProjMatches, mvpMergeMPs, vpMatchedMPs); + // 如果找到共同区域(时序验证成功一次) + if(bCommonRegion) + { + // 标记时序检验成功一次 + bMergeDetectedInKF = true; + + // 成功验证的总次数+1 + mnMergeNumCoincidences++; + // 不再参与新的回环检测 + mpMergeLastCurrentKF->SetErase(); + mpMergeLastCurrentKF = mpCurrentKF; + mg2oMergeSlw = gScw; + mvpMergeMatchedMPs = vpMatchedMPs; + // 如果验证数大于等于3则为成功 + mbMergeDetected = mnMergeNumCoincidences >= 3; + } + // 如果没找到共同区域(时序验证失败一次) + else + { + // 没有找到融合 + mbMergeDetected = false; + // 当前时序验证失败 + bMergeDetectedInKF = false; + + // 递增失败的时序验证次数 + mnMergeNumNotFound++; + // 若果连续两帧时序验证失败则整个融合检测失败 + if(mnMergeNumNotFound >= 2) + { + // 失败后标记重置一些信息 + mpMergeLastCurrentKF->SetErase(); + mpMergeMatchedKF->SetErase(); + mnMergeNumCoincidences = 0; + mvpMergeMatchedMPs.clear(); + mvpMergeMPs.clear(); + mnMergeNumNotFound = 0; + } + + + } + } +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndEstSim3_1 = std::chrono::steady_clock::now(); + + double timeEstSim3 = std::chrono::duration_cast >(time_EndEstSim3_1 - time_StartEstSim3_1).count(); +#endif + // Step 3.3 若校验成功则把当前帧添加进数据库,且返回true表示找到共同区域 + // 注意初始化时mbMergeDetected=mbLoopDetected=false + if(mbMergeDetected || mbLoopDetected) + { +#ifdef REGISTER_TIMES + vdEstSim3_ms.push_back(timeEstSim3); +#endif + mpKeyFrameDB->add(mpCurrentKF); + return true; + } + + //TODO: This is only necessary if we use a minimun score for pick the best candidates + // 这句并没有使用,作者把orbslam2里面通过minScore作为阀值筛选候选帧的策略抛弃了 + const vector vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames(); + + // Extract candidates from the bag of words + // Step 4 若当前关键帧没有被检测到回环或融合,则分别通过bow拿到当前帧最好的三个回环候选帧和融合候选帧 + vector vpMergeBowCand, vpLoopBowCand; + if(!bMergeDetectedInKF || !bLoopDetectedInKF) + { + // Search in BoW +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartQuery = std::chrono::steady_clock::now(); +#endif + // 分别找到3个最好的候选帧, 回环候选帧放在vpLoopBowCand中,融合候选帧放在vpMergeBowCand中 + mpKeyFrameDB->DetectNBestCandidates(mpCurrentKF, vpLoopBowCand, vpMergeBowCand,3); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndQuery = std::chrono::steady_clock::now(); + + double timeDataQuery = std::chrono::duration_cast >(time_EndQuery - time_StartQuery).count(); + vdDataQuery_ms.push_back(timeDataQuery); +#endif + } + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartEstSim3_2 = std::chrono::steady_clock::now(); +#endif + // Check the BoW candidates if the geometric candidate list is empty + //Loop candidates + // Step 4.1 若当前关键帧没有被检测到回环,并且候选帧数量不为0,则对回环候选帧进行论文中第8页的2-5步 + if(!bLoopDetectedInKF && !vpLoopBowCand.empty()) + { + // mnLoopNumCoincidences是成功几何验证的帧数，超过3就认为最终验证成功（mbLoopDetected=true），不超过继续进行时序验证 + // mpLoopMatchedKF 最后成功匹配的候选关键帧 + mbLoopDetected = DetectCommonRegionsFromBoW(vpLoopBowCand, mpLoopMatchedKF, mpLoopLastCurrentKF, mg2oLoopSlw, mnLoopNumCoincidences, mvpLoopMPs, mvpLoopMatchedMPs); + } + // Merge candidates + // Step 4.2 若当前关键帧没有被检测到融合,并且候选帧数量不为0,则对融合候帧进行论文中第8页的2-5步 + if(!bMergeDetectedInKF && !vpMergeBowCand.empty()) + { + // mnLoopNumCoincidences是成功几何验证的帧数，超过3就认为最终验证成功（mbMergeDetected=true），不超过继续进行时序验证 + mbMergeDetected = DetectCommonRegionsFromBoW(vpMergeBowCand, mpMergeMatchedKF, mpMergeLastCurrentKF, mg2oMergeSlw, mnMergeNumCoincidences, mvpMergeMPs, mvpMergeMatchedMPs); + } + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndEstSim3_2 = std::chrono::steady_clock::now(); + + timeEstSim3 += std::chrono::duration_cast >(time_EndEstSim3_2 - time_StartEstSim3_2).count(); + vdEstSim3_ms.push_back(timeEstSim3); +#endif + // Step 5 根据结果确定有没有检测到共同区域 + // 把当前帧添加到关键帧数据库中 + mpKeyFrameDB->add(mpCurrentKF); + // 如果检测到一种类型共同区域返回true + if(mbMergeDetected || mbLoopDetected) + { + return true; + } + // 如果没检测到则把当前关键帧erase(不参与后续共同区域检测) + mpCurrentKF->SetErase(); + // 标记当前关键帧不是当前在进行共同区域检测的帧 + mpCurrentKF->mbCurrentPlaceRecognition = false; + + return false; +} + +/** + * @brief 对新进来的关键帧进行时序几何验证,同时继续优化之前估计的 Tcw + * + * @param[in] pCurrentKF 当前关键帧 + * @param[in] pMatchedKF 候选帧 + * @param[out] gScw 世界坐标系在验证帧下的Sim3 + * @param[out] nNumProjMatches 记录匹配点的数量 + * @param[out] vpMPs 候选帧窗口内所有的地图点 + * @param[out] vpMatchedMPs 候选帧窗口内所有被匹配到的点 + * @return true 时序几何验证成功 + * @return false 时序几何验证失败 + */ +bool LoopClosing::DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches, + std::vector &vpMPs, std::vector &vpMatchedMPs) +{ + set spAlreadyMatchedMPs; + // 1. 重新选点 + // 把候选帧局部窗口内的地图点投向新进来的当前关键帧,看是否有足够的匹配点 + // TODO 清空vpMPs是不是有些多余？经过验证并不多余，点数有一定概率有轻微变化，但不大，这里可以做优化 + nNumProjMatches = FindMatchesByProjection(pCurrentKF, pMatchedKF, gScw, spAlreadyMatchedMPs, vpMPs, vpMatchedMPs); + + int nProjMatches = 30; + int nProjOptMatches = 50; + int nProjMatchesRep = 100; + + // 2.点数如果不符合返回false + if(nNumProjMatches >= nProjMatches) + { + //Verbose::PrintMess("Sim3 reffine: There are " + to_string(nNumProjMatches) + " initial matches ", Verbose::VERBOSITY_DEBUG); + // 3.1 求得gScm 为OptimizeSim3接口准备数据 + Sophus::SE3d mTwm = pMatchedKF->GetPoseInverse().cast(); + g2o::Sim3 gSwm(mTwm.unit_quaternion(),mTwm.translation(),1.0); + g2o::Sim3 gScm = gScw * gSwm; + Eigen::Matrix mHessian7x7; + + // 单目情况下不锁定尺度 + bool bFixedScale = mbFixScale; // TODO CHECK; Solo para el monocular inertial + // 如果是imu模式且未完成初始化,不锁定尺度 + if(mpTracker->mSensor==System::IMU_MONOCULAR && !pCurrentKF->GetMap()->GetIniertialBA2()) + bFixedScale=false; + // 3.2 优化gScm，mp固定 + int numOptMatches = Optimizer::OptimizeSim3(mpCurrentKF, pMatchedKF, vpMatchedMPs, gScm, 10, bFixedScale, mHessian7x7, true); + + //Verbose::PrintMess("Sim3 reffine: There are " + to_string(numOptMatches) + " matches after of the optimization ", Verbose::VERBOSITY_DEBUG); + // 若匹配的数量大于一定的数目 + if(numOptMatches > nProjOptMatches) + { + //!bug, 以下gScw_estimation应该通过上述sim3优化后的位姿来更新。以下mScw应该改为 gscm * gswm^-1 + g2o::Sim3 gScw_estimation(gScw.rotation(), gScw.translation(),1.0); + + vector vpMatchedMP; + vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast(NULL)); + + // 再次通过优化后的Sim3搜索匹配点 + nNumProjMatches = FindMatchesByProjection(pCurrentKF, pMatchedKF, gScw_estimation, spAlreadyMatchedMPs, vpMPs, vpMatchedMPs); + // 若果大于期望数目,接受这个结果 + if(nNumProjMatches >= nProjMatchesRep) + { + gScw = gScw_estimation; + // 验证成功 + return true; + } + } + } + // 验证失败 + return false; +} + +/** + * @brief 实现论文第8页的2-5步中的一部分功能(对后面新进来的关键帧的验证没有放在这个函数里进行) + * 1. 构造局部窗口 + * 2. Ransac 得到 Scm的初始值 + * 3. guided matching refinement + * 4. 利用地图中的共视关键帧验证(共视几何校验) + * + * @param[in] vpBowCand bow 给出的一些候选关键帧 + * @param[out] pMatchedKF2 最后成功匹配的候选关键帧 + * @param[out] pLastCurrentKF 用于记录当前关键帧为上一个关键帧(后续若仍需要时序几何校验需要记录此信息) + * @param[out] g2oScw 候选关键帧世界坐标系到当前关键帧的Sim3变换 + * @param[out] nNumCoincidences 成功几何验证的帧数，超过3就认为几何验证成功，不超过继续进行时序验证 + * @param[out] vpMPs 所有地图点 + * @param[out] vpMatchedMPs 成功匹配的地图点 + * @return true 检测到一个合格的共同区域 + * @return false 没检测到一个合格的共同区域 + */ +bool LoopClosing::DetectCommonRegionsFromBoW( + std::vector &vpBowCand, KeyFrame* &pMatchedKF2, KeyFrame* &pLastCurrentKF, g2o::Sim3 &g2oScw, + int &nNumCoincidences, std::vector &vpMPs, std::vector &vpMatchedMPs) +{ + // 一些后面会使用的阀值 + int nBoWMatches = 20; // 最低bow匹配特征点数 + int nBoWInliers = 15; // RANSAC最低的匹配点数 + int nSim3Inliers = 20; // sim3 最低内点数 + int nProjMatches = 50; // 通过投影得到的匹配点数量最低阀值 + int nProjOptMatches = 80; // 通过更小的半径,更严的距离搜索到的匹配点数量 + + // 1. 获取当前帧的共视帧(在共同区域检测中应该避免当前关键帧的共视关键帧中) + set spConnectedKeyFrames = mpCurrentKF->GetConnectedKeyFrames(); + + // change 定义最佳共视关键帧的数量 0.4版本这里为5 + int nNumCovisibles = 10; + + + ORBmatcher matcherBoW(0.9, true); // 用于search by bow + ORBmatcher matcher(0.75, true); // 用与seach by projection + + // Varibles to select the best numbe + // 一些用于统计最优数据的变量,我们最后返回的是最佳的一个关键帧(几何校验匹配数最高的) + KeyFrame* pBestMatchedKF; + int nBestMatchesReproj = 0; + int nBestNumCoindicendes = 0; + g2o::Sim3 g2oBestScw; + std::vector vpBestMapPoints; + std::vector vpBestMatchedMapPoints; + + // bow中候选关键帧的数量 + int numCandidates = vpBowCand.size(); + + // 这三个变量是作者为了后面打印观察记录的信息,可以忽略 + vector vnStage(numCandidates, 0); + vector vnMatchesStage(numCandidates, 0); + int index = 0; + + //Verbose::PrintMess("BoW candidates: There are " + to_string(vpBowCand.size()) + " possible candidates ", Verbose::VERBOSITY_DEBUG); + // 2. 对每个候选关键帧都进行详细的分析 + for(KeyFrame* pKFi : vpBowCand) + { + if(!pKFi || pKFi->isBad()) + continue; + + // std::cout << "KF candidate: " << pKFi->mnId << std::endl; + // Current KF against KF with covisibles version + // 2.1 获得候选关键帧的局部窗口 W_m + // 拿到候选关键帧的10个最优共视帧 + std::vector vpCovKFi = pKFi->GetBestCovisibilityKeyFrames(nNumCovisibles); + if(vpCovKFi.empty()) + { + std::cout << "Covisible list empty" << std::endl; + vpCovKFi.push_back(pKFi); + } + else + { + // 再加上候选关键帧自己(这里操作比较迷,看起来只是为了把候选关键帧放到容器的第一顺位) + vpCovKFi.push_back(vpCovKFi[0]); + vpCovKFi[0] = pKFi; + } + + // 2.2 1.0版本将这步挪上来了，逻辑有些许变化，可以节省部分时间，主要意思没变 + // 好奇的可以去上个版本看看 + // 标记是否因为窗口内有当前关键帧的共视关键帧 + bool bAbortByNearKF = false; + // 遍历窗口内的每个关键帧 + for(int j=0; j > vvpMatchedMPs; + vvpMatchedMPs.resize(vpCovKFi.size()); + + // 记录整个窗口中有那些点能在Ka中通过bow找到匹配点(这个set是辅助容器,避免重复添加地图点) + std::set spMatchedMPi; + int numBoWMatches = 0; + + // 记录窗口中能通过bow在当前关键帧ka中找到最多匹配点的关键帧 + KeyFrame* pMostBoWMatchesKF = pKFi; + // 记录窗口中能通过bow在当前关键帧ka中找到最多匹配点的数量 + int nMostBoWNumMatches = 0; + + // 下面两个变量是为了sim3 solver准备的 + //记录窗口中的地图点能在当前关键帧中找到的匹配的点(数量的上限是当前关键帧地图点的数量) + std::vector vpMatchedPoints = std::vector(mpCurrentKF->GetMapPointMatches().size(), static_cast(NULL)); + // 记录上面的地图点分别对应窗口中的关键帧(数量的上限是当前关键帧地图点的数量) + std::vector vpKeyFrameMatchedMP = std::vector(mpCurrentKF->GetMapPointMatches().size(), static_cast(NULL)); + + // 记录在W_km中有最多匹配点的帧的局部index, 这个后面没有用到 + int nIndexMostBoWMatchesKF=0; + + // 遍历窗口内Wm的每个关键帧 + // 2.3 通过Bow寻找候选帧窗口内的关键帧地图点与当前关键帧的匹配点 + for(int j=0; jisBad()) + continue; + + int num = matcherBoW.SearchByBoW(mpCurrentKF, vpCovKFi[j], vvpMatchedMPs[j]); + if (num > nMostBoWNumMatches) + { + nMostBoWNumMatches = num; + nIndexMostBoWMatchesKF = j; + } + } + + // 遍历窗口内的每个关键帧 + // 2.4 把窗口内的匹配点转换为Sim3Solver接口定义的格式 + for(int j=0; jisBad()) + continue; + + // 窗口内不同关键帧与当前关键帧可能看到相同的3D点, 利用辅助容器避免重复添加 + if(spMatchedMPi.find(pMPi_j) == spMatchedMPi.end()) + { + // 利用辅助容器记录添加过的点 + spMatchedMPi.insert(pMPi_j); + // 统计窗口内有多少地图点能在当前关键中找到匹配 + numBoWMatches++; + + //记录窗口中的地图点能在当前关键帧中找到的匹配的点 + vpMatchedPoints[k]= pMPi_j; + // 记录上面的地图点分别对应窗口中的关键帧(数量的上限是当前关键帧地图点的数量) + vpKeyFrameMatchedMP[k] = vpCovKFi[j]; + } + } + } + // 1. 前面统计了vpCovKFi中每个帧与当前帧匹配点的位置，可否用点数高的代替 + // 2. 有可能作者认为在DetectNBestCandidates已经找到共视关键帧中分数最多的了，所以这里不做判断直接使用原始的候选关键帧 + //pMostBoWMatchesKF = vpCovKFi[pMostBoWMatchesKF]; + + // 当窗口内的帧不是当前关键帧的相邻帧且匹配点足够多时 + // 3. 利用RANSAC寻找候选关键帧窗口与当前关键帧的相对位姿T_cm的初始值(可能是Sim3) + // nBoWMatches = 20; // 最低bow匹配特征点数 + if(numBoWMatches >= nBoWMatches) // TODO pick a good threshold + { + // Geometric validation + bool bFixedScale = mbFixScale; + // 如果是单目带imu的模式且IMU初始化未完成第三阶段，则不固定scale + if(mpTracker->mSensor==System::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2()) + bFixedScale=false; + + // 3.1 初始化sim3 solver + // Sim3Solver 的接口与orbslam2略有不同, 因为现在是1-N的对应关系 + Sim3Solver solver = Sim3Solver(mpCurrentKF, pMostBoWMatchesKF, vpMatchedPoints, bFixedScale, vpKeyFrameMatchedMP); + // Sim3Solver Ransac 置信度0.99，至少20个inliers 最多300次迭 + solver.SetRansacParameters(0.99, nBoWInliers, 300); // at least 15 inliers + + bool bNoMore = false; + vector vbInliers; + int nInliers; + bool bConverge = false; + Eigen::Matrix4f mTcm; + // 3.2 迭代到收敛 + while(!bConverge && !bNoMore) + { + mTcm = solver.iterate(20,bNoMore, vbInliers, nInliers, bConverge); + //Verbose::PrintMess("BoW guess: Solver achieve " + to_string(nInliers) + " geometrical inliers among " + to_string(nBoWInliers) + " BoW matches", Verbose::VERBOSITY_DEBUG); + } + + // 3.3 Guide matching refinement: 利用初始的Scm信息,进行双向重投影,并非线性优化得到更精确的Scm + if(bConverge) + { + //std::cout << "Check BoW: SolverSim3 converged" << std::endl; + + //Verbose::PrintMess("BoW guess: Convergende with " + to_string(nInliers) + " geometrical inliers among " + to_string(nBoWInliers) + " BoW matches", Verbose::VERBOSITY_DEBUG); + // Match by reprojection + vpCovKFi.clear(); + // 拿到窗口内匹配最多的帧的最佳10个共视帧和它自己组成的窗口 + vpCovKFi = pMostBoWMatchesKF->GetBestCovisibilityKeyFrames(nNumCovisibles); + vpCovKFi.push_back(pMostBoWMatchesKF); + // 这个后面没有用到 + set spCheckKFs(vpCovKFi.begin(), vpCovKFi.end()); + + //std::cout << "There are " << vpCovKFi.size() <<" near KFs" << std::endl; + + // 辅助容器,避免重复添加地图点 + set spMapPoints; + // 这两个容器是searchByProjection定义的容器 + // 记录窗口内地图点 + vector vpMapPoints; + // 记录每个地图点对应的串口内的关键帧 + vector vpKeyFrames; + // vpMapPoints {mp1, mp2, mp3, mp4} + // vpKeyFrames {kf1, kf1, kf2, kf3} + // 遍历窗Wm内的所有关键帧 + for(KeyFrame* pCovKFi : vpCovKFi) + { + // 遍历窗口内每个关键帧的所有地图点 + for(MapPoint* pCovMPij : pCovKFi->GetMapPointMatches()) + { + // 如果指针为空或者改地图点被标记为bad + if(!pCovMPij || pCovMPij->isBad()) + continue; + // 避免重复添加 + if(spMapPoints.find(pCovMPij) == spMapPoints.end()) + { + spMapPoints.insert(pCovMPij); + vpMapPoints.push_back(pCovMPij); + vpKeyFrames.push_back(pCovKFi); + } + } + } + + //std::cout << "There are " << vpKeyFrames.size() <<" KFs which view all the mappoints" << std::endl; + + // 拿到solver 估计的 Scm初始值, 为后续的非线性优化做准备, 在这里 c 表示当前关键帧, m 表示回环/融合候选帧 + g2o::Sim3 gScm(solver.GetEstimatedRotation().cast(),solver.GetEstimatedTranslation().cast(), (double) solver.GetEstimatedScale()); + // 候选关键帧在其世界坐标系下的坐标 + g2o::Sim3 gSmw(pMostBoWMatchesKF->GetRotation().cast(),pMostBoWMatchesKF->GetTranslation().cast(),1.0); + // 利用初始的Scm估计确定世界坐标系在当前相机中的位姿 + g2o::Sim3 gScw = gScm*gSmw; // Similarity matrix of current from the world position + // 准备用来SearchByProjection的位姿信息 + Sophus::Sim3f mScw = Converter::toSophus(gScw); + + // 记录最后searchByProjection的结果 + vector vpMatchedMP; + vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast(NULL)); + vector vpMatchedKF; + vpMatchedKF.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast(NULL)); + // 3.3.1 重新利用之前计算的mScw信息, 通过投影寻找更多的匹配点 + int numProjMatches = matcher.SearchByProjection(mpCurrentKF, mScw, vpMapPoints, vpKeyFrames, vpMatchedMP, vpMatchedKF, 8, 1.5); + //cout <<"BoW: " << numProjMatches << " matches between " << vpMapPoints.size() << " points with coarse Sim3" << endl; + + // 如果拿到了足够多的匹配点, nProjMatches = 50 + if(numProjMatches >= nProjMatches) + { + // Optimize Sim3 transformation with every matches + Eigen::Matrix mHessian7x7; + + bool bFixedScale = mbFixScale; + // 在imu模式下imu未完成第3阶段初始化就不固定scale + if(mpTracker->mSensor==System::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2()) + bFixedScale=false; + // 3.3.2 利用搜索到的更多的匹配点用Sim3优化投影误差得到的更好的 gScm + // pKFi是候选关键帧 + int numOptMatches = Optimizer::OptimizeSim3(mpCurrentKF, pKFi, vpMatchedMP, gScm, 10, mbFixScale, mHessian7x7, true); + + // 3.3.3 如果内点足够多,用更小的半径搜索匹配点,并且再次进行优化(p.s.这里与论文不符,并没有再次优化) + if(numOptMatches >= nSim3Inliers) + { + // 前面已经声明了这些变量了,无需再次声明 + g2o::Sim3 gSmw(pMostBoWMatchesKF->GetRotation().cast(),pMostBoWMatchesKF->GetTranslation().cast(),1.0); + g2o::Sim3 gScw = gScm*gSmw; // Similarity matrix of current from the world position + Sophus::Sim3f mScw = Converter::toSophus(gScw); + + vector vpMatchedMP; + vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast(NULL)); + // 3.3.4 重新利用之前计算的mScw信息, 通过更小的半径和更严格的距离的投影寻找匹配点 + // 5 : 半径的增益系数(对比之前下降了)---> 更小的半径, 1.0 , hamming distance 的阀值增益系数---> 允许更小的距离 + int numProjOptMatches = matcher.SearchByProjection(mpCurrentKF, mScw, vpMapPoints, vpMatchedMP, 5, 1.0); + + // 当新的投影得到的内点数量大于nProjOptMatches=80时 + if(numProjOptMatches >= nProjOptMatches) + { + /// 以下为调试信息 + int max_x = -1, min_x = 1000000; + int max_y = -1, min_y = 1000000; + for(MapPoint* pMPi : vpMatchedMP) + { + if(!pMPi || pMPi->isBad()) + { + continue; + } + + tuple indexes = pMPi->GetIndexInKeyFrame(pKFi); + int index = get<0>(indexes); + if(index >= 0) + { + int coord_x = pKFi->mvKeysUn[index].pt.x; + if(coord_x < min_x) + { + min_x = coord_x; + } + if(coord_x > max_x) + { + max_x = coord_x; + } + int coord_y = pKFi->mvKeysUn[index].pt.y; + if(coord_y < min_y) + { + min_y = coord_y; + } + if(coord_y > max_y) + { + max_y = coord_y; + } + } + } + // 调试完毕 + + // 4. 用当前关键帧的相邻关键来验证前面得到的Tam(共视几何校验) + // 统计验证成功的关键帧数量 + int nNumKFs = 0; + //vpMatchedMPs = vpMatchedMP; + //vpMPs = vpMapPoints; + // Check the Sim3 transformation with the current KeyFrame covisibles + // 4.1 拿到用来验证的关键帧组(后称为验证组): 当前关键帧的共视关键帧，nNumCovisibles = 5; + vector vpCurrentCovKFs = mpCurrentKF->GetBestCovisibilityKeyFrames(nNumCovisibles); + + int j = 0; + // 遍历验证组当有三个关键帧验证成功或遍历所有的关键帧后结束循环 + while(nNumKFs < 3 && jGetPose() * mpCurrentKF->GetPoseInverse()).cast(); + g2o::Sim3 gSjc(mTjc.unit_quaternion(),mTjc.translation(),1.0); + g2o::Sim3 gSjw = gSjc * gScw; + int numProjMatches_j = 0; + vector vpMatchedMPs_j; + // 4.2 几何校验函数, 这个函数里面其实是个searchByProjection : 通过之前计算的位姿转换地图点并通过投影搜索匹配点, 若大于一定数目的任务成功验证一次 + bool bValid = DetectCommonRegionsFromLastKF(pKFj,pMostBoWMatchesKF, gSjw,numProjMatches_j, vpMapPoints, vpMatchedMPs_j); + + // 统计valid的帧的数量 + if(bValid) + { + Sophus::SE3f Tc_w = mpCurrentKF->GetPose(); + Sophus::SE3f Tw_cj = pKFj->GetPoseInverse(); + Sophus::SE3f Tc_cj = Tc_w * Tw_cj; + Eigen::Vector3f vector_dist = Tc_cj.translation(); + nNumKFs++; + } + j++; + } + + // 这里又是没有用的代码,只是记录一点信息,可能是为了方便打印检查 + if(nNumKFs < 3) + { + vnStage[index] = 8; + vnMatchesStage[index] = nNumKFs; + } + + // 记录第二次searchByProjection得到最多匹配点的关键帧的各种信息,最后作为回环帧/融合帧 + if(nBestMatchesReproj < numProjOptMatches) + { + nBestMatchesReproj = numProjOptMatches; // 投影匹配的数量 + nBestNumCoindicendes = nNumKFs; // 成功验证的帧数 + pBestMatchedKF = pMostBoWMatchesKF; // 记录候选帧窗口内与当前关键帧相似度最高的帧 + g2oBestScw = gScw; // 记录最优的位姿(这个位姿是由Tam推到出来的 : Taw = Tam * Tmw,这里a表示c) + vpBestMapPoints = vpMapPoints; // 记录所有的地图点 + vpBestMatchedMapPoints = vpMatchedMP; // 记录所有的地图点中被成功匹配的点 + } + } + } + } + } + /*else + { + Verbose::PrintMess("BoW candidate: it don't match with the current one", Verbose::VERBOSITY_DEBUG); + }*/ + } + index++; + } + + // 如果成功找到了共同区域帧把记录的最优值存到输出变量中 + if(nBestMatchesReproj > 0) + { + pLastCurrentKF = mpCurrentKF; + nNumCoincidences = nBestNumCoindicendes; // 成功几何验证的帧数 + pMatchedKF2 = pBestMatchedKF; + pMatchedKF2->SetNotErase(); + g2oScw = g2oBestScw; + vpMPs = vpBestMapPoints; + vpMatchedMPs = vpBestMatchedMapPoints; + // 如果有三个成功验证则return ture + return nNumCoincidences >= 3; + } + else + { + // 这里是一些无用的变量, 左值只用来打印检查 + int maxStage = -1; + int maxMatched; + for(int i=0; i maxStage) + { + maxStage = vnStage[i]; + maxMatched = vnMatchesStage[i]; + } + } + } + // 如果少于3个当前关键帧的共视关键帧验证了这个候选帧,那么返回失败,注意,这里的失败并不代表最终的验证失败,后续会开启时序校验 + return false; +} + +/** + * @brief 用来验证候选帧的函数, 这个函数的名字取的不好, 函数的本意是想利用候选帧的共视关键帧来验证候选帧,不如改叫做：DetectCommonRegionsFromCoVKF + * + * @param[in] pCurrentKF 当前关键帧 + * @param[in] pMatchedKF 候选帧 + * @param[in] gScw 世界坐标系在验证帧下的Sim3 + * @param[out] nNumProjMatches 最后匹配的数目 + * @param[out] vpMPs 候选帧的窗口内所有的地图点 + * @param[out] vpMatchedMPs 候选帧的窗口内被匹配到的地图点 + * @return true 验证成功 + * @return false 验证失败 + */ +bool LoopClosing::DetectCommonRegionsFromLastKF( + KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches, + std::vector &vpMPs, std::vector &vpMatchedMPs) +{ + // 所有的匹配点 + set spAlreadyMatchedMPs(vpMatchedMPs.begin(), vpMatchedMPs.end()); + // 通过Sim3变换后投影寻找匹配点 + nNumProjMatches = FindMatchesByProjection(pCurrentKF, pMatchedKF, gScw, spAlreadyMatchedMPs, vpMPs, vpMatchedMPs); + + int nProjMatches = 30; + // 如果匹配点数目大于一定的阀值,则认为验证成功,返回ture + if(nNumProjMatches >= nProjMatches) + { + return true; + } + + return false; +} + +/** + * @brief 包装了一下searchByProjection, 把窗口内的所有地图点往当前关键帧上投影, 寻找匹配点 + * + * @param[in] pCurrentKF 当前关键帧 + * @param[in] pMatchedKFw 候选帧 + * @param[in] g2oScw 世界坐标系在验证帧坐标系下的位姿 + * @param[in] spMatchedMPinOrigin 没有用上? + * @param[in] vpMapPoints 候选帧及其共视关键帧组成的窗口里所有的地图点 + * @param[in] vpMatchedMapPoints 候选帧及其共视关键帧组成的窗口里所有被匹配上的地图点 + * @return int 匹配点的数量 + */ +int LoopClosing::FindMatchesByProjection( + KeyFrame* pCurrentKF, KeyFrame* pMatchedKFw, g2o::Sim3 &g2oScw, + set &spMatchedMPinOrigin, vector &vpMapPoints, + vector &vpMatchedMapPoints) +{ + int nNumCovisibles = 10; // change 上个版本为5 + // 拿出候选帧的10个最好的共视关键帧 + vector vpCovKFm = pMatchedKFw->GetBestCovisibilityKeyFrames(nNumCovisibles); + int nInitialCov = vpCovKFm.size(); + // 把自己也加进去, 组成一个局部窗口 + vpCovKFm.push_back(pMatchedKFw); + + // 辅助容器,防止重复添加 + set spCheckKFs(vpCovKFm.begin(), vpCovKFm.end()); + // 拿出当前关键帧的共视关键帧 + set spCurrentCovisbles = pCurrentKF->GetConnectedKeyFrames(); + + // 1. 如果数量不够 扩充窗口 + if(nInitialCov < nNumCovisibles) + { + + // 对于之前里的每个关键帧 + for(int i=0; i vpKFs = vpCovKFm[i]->GetBestCovisibilityKeyFrames(nNumCovisibles); + int nInserted = 0; + int j = 0; + // 这个while循环统计的遍历后面都没有用到, 没有任何作用 + while(j < vpKFs.size() && nInserted < nNumCovisibles) + { + // 如果没有被重复添加且不是当前关键帧的共视关键帧 + if(spCheckKFs.find(vpKFs[j]) == spCheckKFs.end() && spCurrentCovisbles.find(vpKFs[j]) == spCurrentCovisbles.end()) + { + spCheckKFs.insert(vpKFs[j]); + ++nInserted; + + // 改成这样 + vpCovKFm.push_back(vpKFs[j]); + } + ++j; + } + // 这里是原来的代码，这么写不太合适，会出现重复 + // 所以下面的插入可以改成放在if里面 + // 把每个帧的共视关键帧都加到窗口内 + // vpCovKFm.insert(vpCovKFm.end(), vpKFs.begin(), vpKFs.end()); // 已放上面 + } + } + + // 辅助容器, 防止地图点被重复添加 + set spMapPoints; + + // 清空这两个容器,重新进行searchByProjection + vpMapPoints.clear(); + vpMatchedMapPoints.clear(); + // 2. 提取一堆mp + // 对于窗口内每个关键帧 + for(KeyFrame* pKFi : vpCovKFm) + { + // 对于每个关键帧的地图点 + for(MapPoint* pMPij : pKFi->GetMapPointMatches()) + { + // 如果指针不是空,且点不是bad + if(!pMPij || pMPij->isBad()) + continue; + + // 如果没有被重复添加 + if(spMapPoints.find(pMPij) == spMapPoints.end()) + { + spMapPoints.insert(pMPij); + vpMapPoints.push_back(pMPij); + } + } + } + + Sophus::Sim3f mScw = Converter::toSophus(g2oScw); + ORBmatcher matcher(0.9, true); + + // 3. 初始化被匹配到的地图点容器, 匹配上限是当前帧的地图点数量 + vpMatchedMapPoints.resize(pCurrentKF->GetMapPointMatches().size(), static_cast(NULL)); + // 把窗口中的点向当前关键帧投影, 搜索匹配点, 注意验证的时候用的搜索半径是最小的 + int num_matches = matcher.SearchByProjection(pCurrentKF, mScw, vpMapPoints, vpMatchedMapPoints, 3, 1.5); + + return num_matches; +} + +/** + * @brief 相同地图检测到共同区域叫回环，不同地图叫融合，这个函数是在检测到回环后进行修正优化位姿 + */ +void LoopClosing::CorrectLoop() +{ + //cout << "Loop detected!" << endl; + + // Send a stop signal to Local Mapping + // Avoid new keyframes are inserted while correcting the loop + // Step1. 结束局部地图线程、全局BA，为闭环矫正做准备 + // 请求局部地图停止，防止在回环矫正时局部地图线程中InsertKeyFrame函数插入新的关键帧 + mpLocalMapper->RequestStop(); + mpLocalMapper->EmptyQueue(); // Proccess keyframes in the queue + + // 如果正在进行全局BA，丢弃它 + // If a Global Bundle Adjustment is running, abort it + if(isRunningGBA()) + { + cout << "Stoping Global Bundle Adjustment..."; + unique_lock lock(mMutexGBA); + mbStopGBA = true; + // 记录全局BA次数 + mnFullBAIdx++; + + if(mpThreadGBA) + { + mpThreadGBA->detach(); + delete mpThreadGBA; + } + cout << " Done!!" << endl; + } + + // Wait until Local Mapping has effectively stopped + // 一直等到局部地图线程结束再继续 + while(!mpLocalMapper->isStopped()) + { + usleep(1000); + } + + // Ensure current keyframe is updated + //cout << "Start updating connections" << endl; + //assert(mpCurrentKF->GetMap()->CheckEssentialGraph()); + // Step 2. 根据共视关系更新当前关键帧与其它关键帧之间的连接关系 + // 因为之前闭环检测、计算Sim3中改变了该关键帧的地图点，所以需要更新 + mpCurrentKF->UpdateConnections(); + //assert(mpCurrentKF->GetMap()->CheckEssentialGraph()); + + // Retrive keyframes connected to the current keyframe and compute corrected Sim3 pose by propagation + // Step 3. 通过位姿传播，得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的地图点 + // 当前帧与世界坐标系之间的Sim变换在ComputeSim3函数中已经确定并优化， + // 通过相对位姿关系，可以确定这些相连的关键帧与世界坐标系之间的Sim3变换 + + // 取出当前关键帧及其共视关键帧，称为“当前关键帧组” + mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames(); + mvpCurrentConnectedKFs.push_back(mpCurrentKF); + + //std::cout << "Loop: number of connected KFs -> " + to_string(mvpCurrentConnectedKFs.size()) << std::endl; + // CorrectedSim3：存放闭环g2o优化后当前关键帧的共视关键帧的世界坐标系下Sim3 变换 + // NonCorrectedSim3：存放没有矫正的当前关键帧的共视关键帧的世界坐标系下Sim3 变换 + KeyFrameAndPose CorrectedSim3, NonCorrectedSim3; + // 先将mpCurrentKF的Sim3变换存入，认为是准的，所以固定不动 + CorrectedSim3[mpCurrentKF]=mg2oLoopScw; + // 当前关键帧到世界坐标系下的变换矩阵 + Sophus::SE3f Twc = mpCurrentKF->GetPoseInverse(); + Sophus::SE3f Tcw = mpCurrentKF->GetPose(); + g2o::Sim3 g2oScw(Tcw.unit_quaternion().cast(),Tcw.translation().cast(),1.0); + NonCorrectedSim3[mpCurrentKF]=g2oScw; + + // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation) + Sophus::SE3d correctedTcw(mg2oLoopScw.rotation(),mg2oLoopScw.translation() / mg2oLoopScw.scale()); + mpCurrentKF->SetPose(correctedTcw.cast()); + + Map* pLoopMap = mpCurrentKF->GetMap(); + +#ifdef REGISTER_TIMES + /*KeyFrame* pKF = mpCurrentKF; + int numKFinLoop = 0; + while(pKF && pKF->mnId > mpLoopMatchedKF->mnId) + { + pKF = pKF->GetParent(); + numKFinLoop += 1; + } + vnLoopKFs.push_back(numKFinLoop);*/ + + std::chrono::steady_clock::time_point time_StartFusion = std::chrono::steady_clock::now(); +#endif + // 对地图点操作 + { + // Get Map Mutex + unique_lock lock(pLoopMap->mMutexMapUpdate); + + const bool bImuInit = pLoopMap->isImuInitialized(); + // 3.1：通过mg2oLoopScw（认为是准的）来进行位姿传播，得到当前关键帧的共视关键帧的世界坐标系下Sim3 位姿（还没有修正） + // 遍历"当前关键帧组"" + for(vector::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++) + { + KeyFrame* pKFi = *vit; + + if(pKFi!=mpCurrentKF) // 跳过当前关键帧，因为当前关键帧的位姿已经在前面优化过了，在这里是参考基准 + { + // 得到当前关键帧 mpCurrentKF 到其共视关键帧 pKFi 的相对变换 + Sophus::SE3f Tiw = pKFi->GetPose(); + Sophus::SE3d Tic = (Tiw * Twc).cast(); + + // g2oSic：当前关键帧 mpCurrentKF 到其共视关键帧 pKFi 的Sim3 相对变换 + // 这里是non-correct, 所以scale=1.0 + g2o::Sim3 g2oSic(Tic.unit_quaternion(),Tic.translation(),1.0); + // 当前帧的位姿固定不动，其它的关键帧根据相对关系得到Sim3调整的位姿 + g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oLoopScw; + //Pose corrected with the Sim3 of the loop closure + // 存放闭环g2o优化后当前关键帧的共视关键帧的Sim3 位姿 + CorrectedSim3[pKFi]=g2oCorrectedSiw; + + // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation) + Sophus::SE3d correctedTiw(g2oCorrectedSiw.rotation(),g2oCorrectedSiw.translation() / g2oCorrectedSiw.scale()); + pKFi->SetPose(correctedTiw.cast()); + + //Pose without correction + g2o::Sim3 g2oSiw(Tiw.unit_quaternion().cast(),Tiw.translation().cast(),1.0); + NonCorrectedSim3[pKFi]=g2oSiw; + } + } + + // Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop + // 3.2：得到矫正的当前关键帧的共视关键帧位姿后，修正这些关键帧的地图点 + // 遍历待矫正的共视关键帧（不包括当前关键帧） + for(KeyFrameAndPose::iterator mit=CorrectedSim3.begin(), mend=CorrectedSim3.end(); mit!=mend; mit++) + { + KeyFrame* pKFi = mit->first; + g2o::Sim3 g2oCorrectedSiw = mit->second; + g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse(); + + g2o::Sim3 g2oSiw =NonCorrectedSim3[pKFi]; + + // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation) + /*Sophus::SE3d correctedTiw(g2oCorrectedSiw.rotation(),g2oCorrectedSiw.translation() / g2oCorrectedSiw.scale()); + pKFi->SetPose(correctedTiw.cast());*/ + + vector vpMPsi = pKFi->GetMapPointMatches(); + + // 遍历待矫正共视关键帧中的每一个地图点 + for(size_t iMP=0, endMPi = vpMPsi.size(); iMPisBad()) + continue; + if(pMPi->mnCorrectedByKF==mpCurrentKF->mnId) // 标记，防止重复矫正 + continue; + + // Project with non-corrected pose and project back with corrected pose + // 矫正过程本质上也是基于当前关键帧的优化后的位姿展开的 + // 将该未校正的eigP3Dw先从世界坐标系映射到未校正的pKFi相机坐标系，然后再反映射到校正后的世界坐标系下 + Eigen::Vector3d P3Dw = pMPi->GetWorldPos().cast(); + // map(P) 内部做了变换 R*P +t + // 下面变换是：eigP3Dw： world →g2oSiw→ i →g2oCorrectedSwi→ world + Eigen::Vector3d eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(P3Dw)); + + pMPi->SetWorldPos(eigCorrectedP3Dw.cast()); + // 记录矫正该地图点的关键帧id，防止重复 + pMPi->mnCorrectedByKF = mpCurrentKF->mnId; + // 记录该地图点所在的关键帧id + pMPi->mnCorrectedReference = pKFi->mnId; + // 因为地图点更新了，需要更新其平均观测方向以及观测距离范围 + pMPi->UpdateNormalAndDepth(); + } + + // 这块减去了setpose + + // 修改速度 + // Correct velocity according to orientation correction + if(bImuInit) + { + Eigen::Quaternionf Rcor = (g2oCorrectedSiw.rotation().inverse()*g2oSiw.rotation()).cast(); + pKFi->SetVelocity(Rcor*pKFi->GetVelocity()); + } + + // Make sure connections are updated + // 3.3 根据共视关系更新当前帧与其它关键帧之间的连接 + // 地图点的位置改变了,可能会引起共视关系\权值的改变 + pKFi->UpdateConnections(); + } + // TODO Check this index increasement + mpAtlas->GetCurrentMap()->IncreaseChangeIndex(); + + + // Start Loop Fusion + // Update matched map points and replace if duplicated + // Step 4. 检查当前帧的地图点与经过闭环匹配后该帧的地图点是否存在冲突，对冲突的进行替换或填补 + // mvpCurrentMatchedPoints 是当前关键帧和闭环关键帧组的所有地图点进行投影得到的匹配点 + for(size_t i=0; iGetMapPoint(i); + + // 如果有重复的MapPoint，则用匹配的地图点代替现有的 + // 因为匹配的地图点是经过一系列操作后比较精确的，现有的地图点很可能有累计误差 + if(pCurMP) + pCurMP->Replace(pLoopMP); + else + { + // 如果当前帧没有该MapPoint，则直接添加 + mpCurrentKF->AddMapPoint(pLoopMP,i); + pLoopMP->AddObservation(mpCurrentKF,i); + pLoopMP->ComputeDistinctiveDescriptors(); + } + } + } + //cout << "LC: end replacing duplicated" << endl; + } + + // Project MapPoints observed in the neighborhood of the loop keyframe + // into the current keyframe and neighbors using corrected poses. + // Fuse duplications. + // Step 5. 将闭环相连关键帧组mvpLoopMapPoints 投影到当前关键帧组中，进行匹配，融合，新增或替换当前关键帧组中KF的地图点 + // 因为 闭环相连关键帧组mvpLoopMapPoints 在地图中时间比较久经历了多次优化，认为是准确的 + // 而当前关键帧组中的关键帧的地图点是最近新计算的，可能有累积误差 + // CorrectedSim3：存放矫正后当前关键帧的共视关键帧，及其世界坐标系下Sim3 变换 + SearchAndFuse(CorrectedSim3, mvpLoopMapPoints); + + // After the MapPoint fusion, new links in the covisibility graph will appear attaching both sides of the loop + // Step 6. 更新当前关键帧之间的共视相连关系，得到因闭环时MapPoints融合而新得到的连接关系 + // LoopConnections：存储因为闭环地图点调整而新生成的连接关系 + map > LoopConnections; + + // 6.1 遍历当前帧相连关键帧组（一级相连） + for(vector::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++) + { + KeyFrame* pKFi = *vit; + // 6.2 得到与当前帧相连关键帧的相连关键帧（二级相连） + vector vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames(); + + // Update connections. Detect new links. + // 6.3 更新一级相连关键帧的连接关系(会把当前关键帧添加进去,因为地图点已经更新和替换了) + pKFi->UpdateConnections(); + // 6.4 取出该帧更新后的连接关系 + LoopConnections[pKFi]=pKFi->GetConnectedKeyFrames(); + // 6.5 从连接关系中去除闭环之前的二级连接关系，剩下的连接就是由闭环得到的连接关系 + for(vector::iterator vit_prev=vpPreviousNeighbors.begin(), vend_prev=vpPreviousNeighbors.end(); vit_prev!=vend_prev; vit_prev++) + { + LoopConnections[pKFi].erase(*vit_prev); + } + // 6.6 从连接关系中去除闭环之前的一级连接关系，剩下的连接就是由闭环得到的连接关系 + for(vector::iterator vit2=mvpCurrentConnectedKFs.begin(), vend2=mvpCurrentConnectedKFs.end(); vit2!=vend2; vit2++) + { + LoopConnections[pKFi].erase(*vit2); + } + } + + // Optimize graph + bool bFixedScale = mbFixScale; + // TODO CHECK; Solo para el monocular inertial + if(mpTracker->mSensor==System::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2()) + bFixedScale=false; + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndFusion = std::chrono::steady_clock::now(); + + double timeFusion = std::chrono::duration_cast >(time_EndFusion - time_StartFusion).count(); + vdLoopFusion_ms.push_back(timeFusion); +#endif + //cout << "Optimize essential graph" << endl; + if(pLoopMap->IsInertial() && pLoopMap->isImuInitialized()) + { + Optimizer::OptimizeEssentialGraph4DoF(pLoopMap, mpLoopMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections); + } + else + { + //cout << "Loop -> Scale correction: " << mg2oLoopScw.scale() << endl; + // Step 7. 进行EssentialGraph优化，LoopConnections是形成闭环后新生成的连接关系，不包括步骤7中当前帧与闭环匹配帧之间的连接关系 + Optimizer::OptimizeEssentialGraph(pLoopMap, mpLoopMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, bFixedScale); + } +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndOpt = std::chrono::steady_clock::now(); + + double timeOptEss = std::chrono::duration_cast >(time_EndOpt - time_EndFusion).count(); + vdLoopOptEss_ms.push_back(timeOptEss); +#endif + + mpAtlas->InformNewBigChange(); + + // Add loop edge + // Step 7：添加当前帧与闭环匹配帧之间的边（这个连接关系不优化） + // 它在下一次的Essential Graph里面使用 + mpLoopMatchedKF->AddLoopEdge(mpCurrentKF); + mpCurrentKF->AddLoopEdge(mpLoopMatchedKF); + + // Launch a new thread to perform Global Bundle Adjustment (Only if few keyframes, if not it would take too much time) + // 闭环地图没有imu初始化或者 仅有一个地图且内部关键帧<200时才执行全局BA，否则太慢 + if(!pLoopMap->isImuInitialized() || (pLoopMap->KeyFramesInMap()<200 && mpAtlas->CountMaps()==1)) + { + // Step 9. 新建一个线程用于全局BA优化 + // OptimizeEssentialGraph只是优化了一些主要关键帧的位姿，这里进行全局BA可以全局优化所有位姿和MapPoints + + mbRunningGBA = true; + mbFinishedGBA = false; + mbStopGBA = false; + mnCorrectionGBA = mnNumCorrection; + + mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment, this, pLoopMap, mpCurrentKF->mnId); + } + + // Loop closed. Release Local Mapping. + mpLocalMapper->Release(); + + mLastLoopKFid = mpCurrentKF->mnId; //TODO old varible, it is not use in the new algorithm +} + +/** + * @brief 纯视觉地图融合。在检测到成功验证的融合帧后进行 + * 1. 焊缝区域局部BA + * 2. Essential Graph BA + * 融合两张地图为一张地图 + */ +void LoopClosing::MergeLocal() +{ + // 窗口内共视关键帧的数量， 上个0.4版本是15 + int numTemporalKFs = 25; //Temporal KFs in the local window if the map is inertial. + + //Relationship to rebuild the essential graph, it is used two times, first in the local window and later in the rest of the map + // 建立本质图必须的量 + KeyFrame* pNewChild; + KeyFrame* pNewParent; + + // 当前关键帧的窗口 + vector vpLocalCurrentWindowKFs; + // 候选关键帧的窗口 + vector vpMergeConnectedKFs; + + // Flag that is true only when we stopped a running BA, in this case we need relaunch at the end of the merge + // 记录是否把全局BA停下 + bool bRelaunchBA = false; + + //Verbose::PrintMess("MERGE-VISUAL: Check Full Bundle Adjustment", Verbose::VERBOSITY_DEBUG); + // If a Global Bundle Adjustment is running, abort it + if(isRunningGBA()) + { + unique_lock lock(mMutexGBA); + mbStopGBA = true; + + mnFullBAIdx++; + + if(mpThreadGBA) + { + mpThreadGBA->detach(); + delete mpThreadGBA; + } + bRelaunchBA = true; // 以后还会重新开启 + } + + //Verbose::PrintMess("MERGE-VISUAL: Request Stop Local Mapping", Verbose::VERBOSITY_DEBUG); + //cout << "Request Stop Local Mapping" << endl; + // 请求局部建图线程停止 + mpLocalMapper->RequestStop(); + // Wait until Local Mapping has effectively stopped + // 等待局部建图工作停止 + while(!mpLocalMapper->isStopped()) + { + usleep(1000); + } + //cout << "Local Map stopped" << endl; + + mpLocalMapper->EmptyQueue(); + + // Merge map will become in the new active map with the local window of KFs and MPs from the current map. + // Later, the elements of the current map will be transform to the new active map reference, in order to keep real time tracking + // 当前关键帧的共视关键帧和他们观测到的地图点会先被融合, 融合后的图会变成新的当前激活地图. + // 之后, 所有当前地图的其他部分会被转换到当前地图中, 这样是为了保证tracking的实时性. + + // 当前关键帧地图的指针 + Map* pCurrentMap = mpCurrentKF->GetMap(); + // 融合关键帧地图的指针 + Map* pMergeMap = mpMergeMatchedKF->GetMap(); + + //std::cout << "Merge local, Active map: " << pCurrentMap->GetId() << std::endl; + //std::cout << "Merge local, Non-Active map: " << pMergeMap->GetId() << std::endl; + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartMerge = std::chrono::steady_clock::now(); +#endif + + // Ensure current keyframe is updated + // 先保证当前关键帧的链接关系是最新的 + mpCurrentKF->UpdateConnections(); + + // Step 1 构建当前关键帧和融合关键帧的局部窗口(关键帧+地图点) + //Get the current KF and its neighbors(visual->covisibles; inertial->temporal+covisibles) + // 当前关键帧的局部窗口(仅是辅助容器,防止重复添加) + set spLocalWindowKFs; + //Get MPs in the welding area from the current map + // 当前关键帧局部串口能观测到的所有地图点(仅是辅助容器,防止重复添加) + set spLocalWindowMPs; + + // 这段代码只在visual状态下才会被使用，所以只会执行else + // Step 1.1 构造当前关键帧局部共视帧窗口 + if(pCurrentMap->IsInertial() && pMergeMap->IsInertial()) //TODO Check the correct initialization + { + KeyFrame* pKFi = mpCurrentKF; + int nInserted = 0; + while(pKFi && nInserted < numTemporalKFs) + { + spLocalWindowKFs.insert(pKFi); + pKFi = mpCurrentKF->mPrevKF; + nInserted++; + + set spMPi = pKFi->GetMapPoints(); + spLocalWindowMPs.insert(spMPi.begin(), spMPi.end()); + } + + pKFi = mpCurrentKF->mNextKF; + while(pKFi) //! 这里会死循环,不过无所谓，这个外面的if永远不会执行 + { + spLocalWindowKFs.insert(pKFi); + + set spMPi = pKFi->GetMapPoints(); + spLocalWindowMPs.insert(spMPi.begin(), spMPi.end()); + + pKFi = mpCurrentKF->mNextKF; + } + } + else + { + // 把自己先加到窗口内 + spLocalWindowKFs.insert(mpCurrentKF); + } + + // 拿到当前关键帧的numTemporalKFs(15)个最佳共视关键帧 + vector vpCovisibleKFs = mpCurrentKF->GetBestCovisibilityKeyFrames(numTemporalKFs); + // 把当前帧的共视帧都加到窗口里 + spLocalWindowKFs.insert(vpCovisibleKFs.begin(), vpCovisibleKFs.end()); + // 1.0版本新加的，将当前关键帧也添加进来 + spLocalWindowKFs.insert(mpCurrentKF); + + // 限制while循环次数 0.4版本是3 + const int nMaxTries = 5; + int nNumTries = 0; + + // 如果窗口里的关键帧数量不够就再拉一些窗口里的关键帧的共视关键帧(二级共视关键帧)进来 + while(spLocalWindowKFs.size() < numTemporalKFs && nNumTries < nMaxTries) + { + vector vpNewCovKFs; + vpNewCovKFs.empty(); + // 遍历创口内的每一个关键帧 + for(KeyFrame* pKFi : spLocalWindowKFs) + { + // 拿到一些二级共视关键帧 + vector vpKFiCov = pKFi->GetBestCovisibilityKeyFrames(numTemporalKFs/2); + + // 对于每个二级共视关键帧 + for(KeyFrame* pKFcov : vpKFiCov) + { + // 如果指针不为空,且关键帧没有被标记为bad,且没有被添加过则加到窗口内 + if(pKFcov && !pKFcov->isBad() && spLocalWindowKFs.find(pKFcov) == spLocalWindowKFs.end()) + { + vpNewCovKFs.push_back(pKFcov); + } + + } + } + // 用set记录,防止重复添加 + spLocalWindowKFs.insert(vpNewCovKFs.begin(), vpNewCovKFs.end()); + // 递增循环次数 + nNumTries++; + } + + // Step 1.2 把当前关键帧窗口里关键帧观测到的地图点加进来 + for(KeyFrame* pKFi : spLocalWindowKFs) + { + if(!pKFi || pKFi->isBad()) + continue; + + set spMPs = pKFi->GetMapPoints(); + spLocalWindowMPs.insert(spMPs.begin(), spMPs.end()); + } + + //std::cout << "[Merge]: Ma = " << to_string(pCurrentMap->GetId()) << "; #KFs = " << to_string(spLocalWindowKFs.size()) << "; #MPs = " << to_string(spLocalWindowMPs.size()) << std::endl; + + // Step 1.3 构造融合帧的共视帧窗口 + // 融合关键帧的共视关键帧们 + set spMergeConnectedKFs; + // 这段代码只在visual状态下才会被使用，所以只会执行else + if(pCurrentMap->IsInertial() && pMergeMap->IsInertial()) //TODO Check the correct initialization + { + KeyFrame* pKFi = mpMergeMatchedKF; + int nInserted = 0; + while(pKFi && nInserted < numTemporalKFs/2) + { + spMergeConnectedKFs.insert(pKFi); + pKFi = mpCurrentKF->mPrevKF; + nInserted++; + } + + pKFi = mpMergeMatchedKF->mNextKF; + while(pKFi && nInserted < numTemporalKFs) + { + spMergeConnectedKFs.insert(pKFi); + pKFi = mpCurrentKF->mNextKF; + } + } + else + { + // 先把融合关键帧自己添加到窗口内 + spMergeConnectedKFs.insert(mpMergeMatchedKF); + } + // 拿到融合关键帧最好的numTemporalKFs(25)个最佳共视关键帧 + vpCovisibleKFs = mpMergeMatchedKF->GetBestCovisibilityKeyFrames(numTemporalKFs); + spMergeConnectedKFs.insert(vpCovisibleKFs.begin(), vpCovisibleKFs.end()); + spMergeConnectedKFs.insert(mpMergeMatchedKF); + // 记录循环次数 + nNumTries = 0; + // 如果融合关键帧窗口里的关键帧不够就再拉一些窗口里的关键帧的共视帧进来(二级共视关键帧) + while(spMergeConnectedKFs.size() < numTemporalKFs && nNumTries < nMaxTries) + { + vector vpNewCovKFs; + // 遍历融合关键帧内的所有的一级共视关键帧 + for(KeyFrame* pKFi : spMergeConnectedKFs) + { + // 拿到一些二级共视关键帧 + vector vpKFiCov = pKFi->GetBestCovisibilityKeyFrames(numTemporalKFs/2); + // 对于每个二级共视关键帧 + for(KeyFrame* pKFcov : vpKFiCov) + { + // 如果指针不为空,且关键帧没有被标记为bad,且没有被添加过则加到窗口内 + if(pKFcov && !pKFcov->isBad() && spMergeConnectedKFs.find(pKFcov) == spMergeConnectedKFs.end()) + { + vpNewCovKFs.push_back(pKFcov); + } + + } + } + // 用set记录,防止重复添加 + spMergeConnectedKFs.insert(vpNewCovKFs.begin(), vpNewCovKFs.end()); + // 递增循环次数 + nNumTries++; + } + + // Step 1.4 把融合关键帧窗口里关键帧观测到的地图点加进来 + // 融合关键帧共视窗口内的所有地图点 + set spMapPointMerge; + for(KeyFrame* pKFi : spMergeConnectedKFs) + { + set vpMPs = pKFi->GetMapPoints(); + spMapPointMerge.insert(vpMPs.begin(),vpMPs.end()); + } + + // 把融合关键帧窗口内的地图点加到待融合的向量中 + vector vpCheckFuseMapPoint; + vpCheckFuseMapPoint.reserve(spMapPointMerge.size()); + // 把spMapPointMerge拷贝到vpCheckFuseMapPoint里 + std::copy(spMapPointMerge.begin(), spMapPointMerge.end(), std::back_inserter(vpCheckFuseMapPoint)); + + //std::cout << "[Merge]: Mm = " << to_string(pMergeMap->GetId()) << "; #KFs = " << to_string(spMergeConnectedKFs.size()) << "; #MPs = " << to_string(spMapPointMerge.size()) << std::endl; + + + // Step 2 根据之前的Sim3初始值, 记录当前帧窗口内关键帧,地图点的矫正前的值,和矫正后的初始值 + + // Step 2.1 先计算当前关键帧矫正前的值,和矫正后的初始值 + Sophus::SE3d Twc = mpCurrentKF->GetPoseInverse().cast(); + + // 记录没有融合矫正之前的Swc和Scw + g2o::Sim3 g2oNonCorrectedSwc(Twc.unit_quaternion(),Twc.translation(),1.0); + g2o::Sim3 g2oNonCorrectedScw = g2oNonCorrectedSwc.inverse(); + + // 拿到之前通过Sim3(见 NewDetectCommonRegion)计算得到的当前关键帧融合矫正之后的初始位姿 + // mg2oMergeScw存放的是融合关键帧所在的世界坐标系到当前帧的Sim3位姿 + g2o::Sim3 g2oCorrectedScw = mg2oMergeScw; //TODO Check the transformation + + // 记录当前关键帧窗口内所有关键帧融合矫正之前的位姿,和融合矫正之后的初始位姿 + KeyFrameAndPose vCorrectedSim3, vNonCorrectedSim3; + // g2oNonCorrectedScw 是当前关键帧世界坐标系下的 + // g2oCorrectedScw 是融合关键帧所在的世界坐标系下的 + vCorrectedSim3[mpCurrentKF]=g2oCorrectedScw; + vNonCorrectedSim3[mpCurrentKF]=g2oNonCorrectedScw; + + +#ifdef REGISTER_TIMES + vnMergeKFs.push_back(spLocalWindowKFs.size() + spMergeConnectedKFs.size()); + vnMergeMPs.push_back(spLocalWindowMPs.size() + spMapPointMerge.size()); +#endif + + // Step 2.2 通过当前关键帧融合矫正前后的位姿,把当前关键帧的共视窗口里面剩余的关键帧矫正前后的位姿都给填写上 + // 对于每个当前关键帧共视窗口里的关键帧 + for(KeyFrame* pKFi : spLocalWindowKFs) + { + // 跳过空的指针,或者坏的关键帧 + if(!pKFi || pKFi->isBad()) + { + Verbose::PrintMess("Bad KF in correction", Verbose::VERBOSITY_DEBUG); + continue; + } + + if(pKFi->GetMap() != pCurrentMap) + Verbose::PrintMess("Other map KF, this should't happen", Verbose::VERBOSITY_DEBUG); + + // 确保这些共视关键帧在当前地图下 + // 保存第i个共视关键帧融合矫正后的初始位姿 + g2o::Sim3 g2oCorrectedSiw; + + if(pKFi!=mpCurrentKF) + { + // 先记录下融合矫正前的位姿 + Sophus::SE3d Tiw = (pKFi->GetPose()).cast(); + g2o::Sim3 g2oSiw(Tiw.unit_quaternion(),Tiw.translation(),1.0); + //Pose without correction + vNonCorrectedSim3[pKFi]=g2oSiw; + + // 根据链式法则,利用当前关键帧和第i个共视关键帧的位姿关系,以及当前关键帧矫正后的初始位姿,推得第i个共视关键帧的矫正后的初始位姿 + Sophus::SE3d Tic = Tiw*Twc; + g2o::Sim3 g2oSic(Tic.unit_quaternion(),Tic.translation(),1.0); + g2oCorrectedSiw = g2oSic*mg2oMergeScw; + vCorrectedSim3[pKFi]=g2oCorrectedSiw; + } + else + { + g2oCorrectedSiw = g2oCorrectedScw; + } + + // 这一句没有什么作用,下面又被覆盖了 + pKFi->mTcwMerge = pKFi->GetPose(); + + // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation) + // 根据上面计算得到的融合矫正后的初始位姿(Sim3),更新窗口内每个关键帧的mTcwMerge(矫正后的初始位姿) + double s = g2oCorrectedSiw.scale(); + pKFi->mfScale = s; + + // s*Riw * Pw + tiw = Pi 此时Pi在i坐标系下的坐标，尺度保留的是原来的 + // Riw * Pw + tiw/s = Pi/s 此时Pi/s在i坐标系下的坐标，尺度是最新的的，所以Rt要这么保留 + Sophus::SE3d correctedTiw(g2oCorrectedSiw.rotation(), g2oCorrectedSiw.translation() / s); // 修正尺度 + + // 赋值得到的矫正后的初始位姿 + pKFi->mTcwMerge = correctedTiw.cast(); + + // !纯视觉模式，以下代码不执行 + if(pCurrentMap->isImuInitialized()) + { + Eigen::Quaternionf Rcor = (g2oCorrectedSiw.rotation().inverse() * vNonCorrectedSim3[pKFi].rotation()).cast(); + pKFi->mVwbMerge = Rcor * pKFi->GetVelocity(); + } + + //TODO DEBUG to know which are the KFs that had been moved to the other map + } + + int numPointsWithCorrection = 0; + + //for(MapPoint* pMPi : spLocalWindowMPs) + // Step 2.3 记录所有地图点矫正前的位置,和矫正后的初始值 + // 对于每个窗口内的地图点，之前用的for循环，改成迭代器方便删点 + set::iterator itMP = spLocalWindowMPs.begin(); + while(itMP != spLocalWindowMPs.end()) + { + MapPoint* pMPi = *itMP; + // 不好的点去掉，1.0新加 + if(!pMPi || pMPi->isBad()) + { + itMP = spLocalWindowMPs.erase(itMP); + continue; + } + + // 拿到参考关键帧，删掉不存在的，1.0新加 + KeyFrame* pKFref = pMPi->GetReferenceKeyFrame(); + if(vCorrectedSim3.find(pKFref) == vCorrectedSim3.end()) + { + itMP = spLocalWindowMPs.erase(itMP); + numPointsWithCorrection++; + continue; + } + + // 拿到计算好的矫正后参考关键帧的初始位姿 + g2o::Sim3 g2oCorrectedSwi = vCorrectedSim3[pKFref].inverse(); + // 拿到矫正前的参考关键帧的位姿 + g2o::Sim3 g2oNonCorrectedSiw = vNonCorrectedSim3[pKFref]; + + // Project with non-corrected pose and project back with corrected pose + // 先把3D点转换到参考关键帧矫正前的坐标系中 + Eigen::Vector3d P3Dw = pMPi->GetWorldPos().cast(); + // 再转换到矫正后的初始坐标系中 + Eigen::Vector3d eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oNonCorrectedSiw.map(P3Dw)); + // 计算旋转部分的变化 + Eigen::Quaterniond Rcor = g2oCorrectedSwi.rotation() * g2oNonCorrectedSiw.rotation(); + + // 矫正后3d点的初始位置 + pMPi->mPosMerge = eigCorrectedP3Dw.cast(); + // 用旋转部分的变化更新计算3D点normal的新值 + pMPi->mNormalVectorMerge = Rcor.cast() * pMPi->GetNormal(); + + itMP++; + } + /*if(numPointsWithCorrection>0) + { + std::cout << "[Merge]: " << std::to_string(numPointsWithCorrection) << " points removed from Ma due to its reference KF is not in welding area" << std::endl; + std::cout << "[Merge]: Ma has " << std::to_string(spLocalWindowMPs.size()) << " points" << std::endl; + }*/ + // Step 3 两个地图以新（当前帧所在地图）换旧（融合帧所在地图），包括关键帧及地图点关联地图的以新换旧、地图集的以新换旧 + { + // 当前地图会被更新，老的地图中的重复地图点会被剔除 + unique_lock currentLock(pCurrentMap->mMutexMapUpdate); // We update the current map with the Merge information + unique_lock mergeLock(pMergeMap->mMutexMapUpdate); // We remove the Kfs and MPs in the merged area from the old map + + //std::cout << "Merge local window: " << spLocalWindowKFs.size() << std::endl; + //std::cout << "[Merge]: init merging maps " << std::endl; + // 对于当前关键帧共视窗口内的每一个关键帧 + // Step 3.1 更新当前关键帧共视窗口内的关键帧信息 + for(KeyFrame* pKFi : spLocalWindowKFs) + { + if(!pKFi || pKFi->isBad()) + { + //std::cout << "Bad KF in correction" << std::endl; + continue; + } + + //std::cout << "KF id: " << pKFi->mnId << std::endl; + // 记录融合矫正前的位姿 + pKFi->mTcwBefMerge = pKFi->GetPose(); + pKFi->mTwcBefMerge = pKFi->GetPoseInverse(); + // 把这个关键帧的位姿设置为融合矫正后的初始位姿 + pKFi->SetPose(pKFi->mTcwMerge); + + // Make sure connections are updated + // 把这个关键帧的地图设置为融合帧所在的地图 + pKFi->UpdateMap(pMergeMap); + // 记录这个关键帧是被哪个当前关键帧融合的 + pKFi->mnMergeCorrectedForKF = mpCurrentKF->mnId; + // 把这个关键帧所有权给到融合帧所在地图里 + pMergeMap->AddKeyFrame(pKFi); + // 把这个关键帧从当前活跃地图中删掉 + pCurrentMap->EraseKeyFrame(pKFi); + + // 下面是没用的代码 + if(pCurrentMap->isImuInitialized()) + { + pKFi->SetVelocity(pKFi->mVwbMerge); + } + } + + // Step 3.2 更新当前关键帧共视帧窗口所能观测到的地图点的信息 + // 把所有地图点的所有权给到老图里面 + // 对于每个当前关键帧共视窗口内的所有地图点 + for(MapPoint* pMPi : spLocalWindowMPs) + { + if(!pMPi || pMPi->isBad()) + continue; + // 把3D点的位置设置成融合矫正之后的位置 + pMPi->SetWorldPos(pMPi->mPosMerge); + // 把3D点normal设置成融合矫正之后的法向量 + pMPi->SetNormalVector(pMPi->mNormalVectorMerge); + // 把3D点所在的地图设置成融合帧所在的地图 + pMPi->UpdateMap(pMergeMap); + // 把3D点添加进融合帧所在地图 + pMergeMap->AddMapPoint(pMPi); + // 把3D点从当前活跃地图中删掉 + pCurrentMap->EraseMapPoint(pMPi); + } + // Step 3.3 更新剩余信息,如Altas和融合帧所在地图的信息 + // 在Altas中把当前地图休眠，老图重新激活 + mpAtlas->ChangeMap(pMergeMap); + // 当前地图的信息都到融合帧所在地图里去了,可以设置为bad + mpAtlas->SetMapBad(pCurrentMap); + // 记录地图变化次数 + pMergeMap->IncreaseChangeIndex(); + //TODO for debug + pMergeMap->ChangeId(pCurrentMap->GetId()); + + //std::cout << "[Merge]: merging maps finished" << std::endl; + } + + // Step 4 融合新旧地图的生成树 + //Rebuild the essential graph in the local window + // 重新构造essential graph的相关信息 + // 设置当前地图的第一个关键帧不再是第一次生成树了 + pCurrentMap->GetOriginKF()->SetFirstConnection(false); + // 从当前关键帧开始反向遍历整个地图 + pNewChild = mpCurrentKF->GetParent(); // Old parent, it will be the new child of this KF + pNewParent = mpCurrentKF; // Old child, now it will be the parent of its own parent(we need eliminate this KF from children list in its old parent) + // 把当前帧的父亲设置为融合帧 + mpCurrentKF->ChangeParent(mpMergeMatchedKF); + + // mpMergeMatchedKF表示待融合地图中与当前关键帧对应上的帧 + // 因为整个待融合地图要融入到当前地图里，为了避免有两个父节点，mpMergeMatchedKF的原始父节点变成了它的子节点，而当前关键帧成了mpMergeMatchedKF的父节点 + // 同理，为了避免mpMergeMatchedKF原始父节点（现在已成它的子节点）有两个父节点，需要向上一直改到待融合地图最顶端的父节点 + while(pNewChild) + { + // new parent (old child) 不再是 new child (old parent) 的 child 了 + pNewChild->EraseChild(pNewParent); // We remove the relation between the old parent and the new for avoid loop + // 记录原先老parent的parent, 用于后续遍历 + KeyFrame * pOldParent = pNewChild->GetParent(); + + // 把 new parent 设置为 new child 的 parent (父子关系互换) + pNewChild->ChangeParent(pNewParent); + + // 赋值指针, 用于遍历下一组父子 + pNewParent = pNewChild; + pNewChild = pOldParent; + + } + + //Update the connections between the local window + // 更新融合帧局部的连接关系 + mpMergeMatchedKF->UpdateConnections(); + + // 重新拿到融合帧局部的共视帧窗窗口 + vpMergeConnectedKFs = mpMergeMatchedKF->GetVectorCovisibleKeyFrames(); + vpMergeConnectedKFs.push_back(mpMergeMatchedKF); + //vpCheckFuseMapPoint.reserve(spMapPointMerge.size()); + //std::copy(spMapPointMerge.begin(), spMapPointMerge.end(), std::back_inserter(vpCheckFuseMapPoint)); + + // Project MapPoints observed in the neighborhood of the merge keyframe + // into the current keyframe and neighbors using corrected poses. + // Fuse duplications. + //std::cout << "[Merge]: start fuse points" << std::endl; + // Step 5 把融合关键帧的共视窗口里的地图点投到当前关键帧的共视窗口里,把重复的点融合掉(以旧换新) + SearchAndFuse(vCorrectedSim3, vpCheckFuseMapPoint); + //std::cout << "[Merge]: fuse points finished" << std::endl; + + // Update connectivity + // Step 6 因为融合导致地图点变化。需要更新关键帧中图的连接关系 + // 更新当前关键帧共视窗口内所有关键帧的连接 + for(KeyFrame* pKFi : spLocalWindowKFs) + { + if(!pKFi || pKFi->isBad()) + continue; + + pKFi->UpdateConnections(); + } + // 更新融合关键帧共视窗口内所有关键帧的连接 + for(KeyFrame* pKFi : spMergeConnectedKFs) + { + if(!pKFi || pKFi->isBad()) + continue; + + pKFi->UpdateConnections(); + } + + //std::cout << "[Merge]: Start welding bundle adjustment" << std::endl; + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartWeldingBA = std::chrono::steady_clock::now(); + + double timeMergeMaps = std::chrono::duration_cast >(time_StartWeldingBA - time_StartMerge).count(); + vdMergeMaps_ms.push_back(timeMergeMaps); +#endif + + bool bStop = false; + // 为Local BA的接口, 把set转为vector + // Step 7 在缝合(Welding)区域进行local BA + vpLocalCurrentWindowKFs.clear(); //当前关键帧的窗口 + vpMergeConnectedKFs.clear(); //融合关键帧的窗口 + std::copy(spLocalWindowKFs.begin(), spLocalWindowKFs.end(), std::back_inserter(vpLocalCurrentWindowKFs)); + std::copy(spMergeConnectedKFs.begin(), spMergeConnectedKFs.end(), std::back_inserter(vpMergeConnectedKFs)); + if (mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) + { + //! 没有用的代码 + Optimizer::MergeInertialBA(mpCurrentKF,mpMergeMatchedKF,&bStop, pCurrentMap,vCorrectedSim3); + } + else + { + // 运行的 welding BA , 优化所有的当前关键帧共视窗口里的关键帧和地图点, 固定所有融合帧共视窗口里的帧 + Optimizer::LocalBundleAdjustment(mpCurrentKF, vpLocalCurrentWindowKFs, vpMergeConnectedKFs,&bStop); + } + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndWeldingBA = std::chrono::steady_clock::now(); + + double timeWeldingBA = std::chrono::duration_cast >(time_EndWeldingBA - time_StartWeldingBA).count(); + vdWeldingBA_ms.push_back(timeWeldingBA); +#endif + //std::cout << "[Merge]: Welding bundle adjustment finished" << std::endl; + + // Loop closed. Release Local Mapping. + mpLocalMapper->Release(); + + // Step 8 在当前帧整个剩下的地图中（局部窗口外，认为没那么紧急处理）对位姿和地图点进行矫正传播 + + //Update the non critical area from the current map to the merged map + // 把前面优位姿优化的结果传递到地图中其他的关键帧 + vector vpCurrentMapKFs = pCurrentMap->GetAllKeyFrames(); + vector vpCurrentMapMPs = pCurrentMap->GetAllMapPoints(); + + if(vpCurrentMapKFs.size() == 0) + { + + } + else + { + if(mpTracker->mSensor == System::MONOCULAR) + { + // 锁住当前地图 + unique_lock currentLock(pCurrentMap->mMutexMapUpdate); // We update the current map with the Merge information + + // Step 8.1 拿到当前帧所在地图里的所有关键帧, 重复之前的过程, 不过这里是对于地图里剩余的所有的关键帧 + for(KeyFrame* pKFi : vpCurrentMapKFs) + { + if(!pKFi || pKFi->isBad() || pKFi->GetMap() != pCurrentMap) + { + continue; + } + + g2o::Sim3 g2oCorrectedSiw; + + Sophus::SE3d Tiw = (pKFi->GetPose()).cast(); + g2o::Sim3 g2oSiw(Tiw.unit_quaternion(),Tiw.translation(),1.0); + //Pose without correction + vNonCorrectedSim3[pKFi]=g2oSiw; + + Sophus::SE3d Tic = Tiw*Twc; + g2o::Sim3 g2oSim(Tic.unit_quaternion(),Tic.translation(),1.0); + g2oCorrectedSiw = g2oSim*mg2oMergeScw; + vCorrectedSim3[pKFi]=g2oCorrectedSiw; + + // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation) + double s = g2oCorrectedSiw.scale(); + + pKFi->mfScale = s; + + Sophus::SE3d correctedTiw(g2oCorrectedSiw.rotation(),g2oCorrectedSiw.translation() / s); + + pKFi->mTcwBefMerge = pKFi->GetPose(); + pKFi->mTwcBefMerge = pKFi->GetPoseInverse(); + + pKFi->SetPose(correctedTiw.cast()); + + if(pCurrentMap->isImuInitialized()) + { + Eigen::Quaternionf Rcor = (g2oCorrectedSiw.rotation().inverse() * vNonCorrectedSim3[pKFi].rotation()).cast(); + pKFi->SetVelocity(Rcor * pKFi->GetVelocity()); // TODO: should add here scale s + } + + } + for(MapPoint* pMPi : vpCurrentMapMPs) + { + if(!pMPi || pMPi->isBad()|| pMPi->GetMap() != pCurrentMap) + continue; + + KeyFrame* pKFref = pMPi->GetReferenceKeyFrame(); + g2o::Sim3 g2oCorrectedSwi = vCorrectedSim3[pKFref].inverse(); + g2o::Sim3 g2oNonCorrectedSiw = vNonCorrectedSim3[pKFref]; + + // Project with non-corrected pose and project back with corrected pose + Eigen::Vector3d P3Dw = pMPi->GetWorldPos().cast(); + Eigen::Vector3d eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oNonCorrectedSiw.map(P3Dw)); + pMPi->SetWorldPos(eigCorrectedP3Dw.cast()); + + pMPi->UpdateNormalAndDepth(); + } + } + + mpLocalMapper->RequestStop(); + // Wait until Local Mapping has effectively stopped + while(!mpLocalMapper->isStopped()) + { + usleep(1000); + } + + // Optimize graph (and update the loop position for each element form the begining to the end) + // Step 8.2 本质图优化 + if(mpTracker->mSensor != System::MONOCULAR) + { + // 固定 : 所有融合帧共视窗口内的关键帧 + 所有当前关键帧共视窗口内的关键帧 + // 优化: 当前关键帧所在地图里的所有关键帧(除了当前关键帧共视窗口内的关键帧) + 当前地图里的所有地图点 + Optimizer::OptimizeEssentialGraph(mpCurrentKF, vpMergeConnectedKFs, vpLocalCurrentWindowKFs, vpCurrentMapKFs, vpCurrentMapMPs); + } + + + { + // Get Merge Map Mutex + unique_lock currentLock(pCurrentMap->mMutexMapUpdate); // We update the current map with the Merge information + unique_lock mergeLock(pMergeMap->mMutexMapUpdate); // We remove the Kfs and MPs in the merged area from the old map + + //std::cout << "Merge outside KFs: " << vpCurrentMapKFs.size() << std::endl; + // 确保融合后信息被更新 + for(KeyFrame* pKFi : vpCurrentMapKFs) + { + if(!pKFi || pKFi->isBad() || pKFi->GetMap() != pCurrentMap) + { + continue; + } + //std::cout << "KF id: " << pKFi->mnId << std::endl; + + // Make sure connections are updated + // 当前关键帧所在地图更新为融合地图，并建立关联。并从当前地图中删掉 + pKFi->UpdateMap(pMergeMap); + pMergeMap->AddKeyFrame(pKFi); + pCurrentMap->EraseKeyFrame(pKFi); + } + + for(MapPoint* pMPi : vpCurrentMapMPs) + { + if(!pMPi || pMPi->isBad()) + continue; + + pMPi->UpdateMap(pMergeMap); + pMergeMap->AddMapPoint(pMPi); + pCurrentMap->EraseMapPoint(pMPi); + } + } + } + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndOptEss = std::chrono::steady_clock::now(); + + double timeOptEss = std::chrono::duration_cast >(time_EndOptEss - time_EndWeldingBA).count(); + vdMergeOptEss_ms.push_back(timeOptEss); +#endif + + // Essential graph 优化后可以重新开始局部建图了 + mpLocalMapper->Release(); + + // 全局的BA，后面一串的判断都为true + // !pCurrentMap->isImuInitialized()一定是true + // Step 9 全局BA + if(bRelaunchBA && (!pCurrentMap->isImuInitialized() || (pCurrentMap->KeyFramesInMap()<200 && mpAtlas->CountMaps()==1))) + { + // Launch a new thread to perform Global Bundle Adjustment + mbRunningGBA = true; + mbFinishedGBA = false; + mbStopGBA = false; + // 执行全局BA + mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this, pMergeMap, mpCurrentKF->mnId); + } + + // 添加融合边(这里不是参与优化的边,只是记录) + mpMergeMatchedKF->AddMergeEdge(mpCurrentKF); + mpCurrentKF->AddMergeEdge(mpMergeMatchedKF); + + pCurrentMap->IncreaseChangeIndex(); + pMergeMap->IncreaseChangeIndex(); + + // altas移除不好的地图 + mpAtlas->RemoveBadMaps(); + +} + +/** + * @brief 惯性模式下的地图融合 + */ +void LoopClosing::MergeLocal2() +{ + //cout << "Merge detected!!!!" << endl; + // 没用上 + int numTemporalKFs = 11; //TODO (set by parameter): Temporal KFs in the local window if the map is inertial. + + //Relationship to rebuild the essential graph, it is used two times, first in the local window and later in the rest of the map + // 用来重新构造Essential Graph + KeyFrame* pNewChild; + KeyFrame* pNewParent; + + // 没用上 + vector vpLocalCurrentWindowKFs; + vector vpMergeConnectedKFs; + + // 记录用初始Sim3 计算出来的当前关键帧局部共视帧窗口内的所有关键帧矫正前的值和矫正后的初始值 + KeyFrameAndPose CorrectedSim3, NonCorrectedSim3; + // NonCorrectedSim3[mpCurrentKF]=mg2oLoopScw; + + // Flag that is true only when we stopped a running BA, in this case we need relaunch at the end of the merge + // 记录要不要重新进行全局ba + bool bRelaunchBA = false; + + //cout << "Check Full Bundle Adjustment" << endl; + // If a Global Bundle Adjustment is running, abort it + // Step 1 如果正在进行全局BA，停掉它 + if(isRunningGBA()) + { + unique_lock lock(mMutexGBA); + mbStopGBA = true; + + mnFullBAIdx++; + + if(mpThreadGBA) + { + mpThreadGBA->detach(); + delete mpThreadGBA; + } + bRelaunchBA = true; + } + + + //cout << "Request Stop Local Mapping" << endl; + // Step 2 暂停局部建图线程 + mpLocalMapper->RequestStop(); + // Wait until Local Mapping has effectively stopped + // 等待直到完全停掉 + while(!mpLocalMapper->isStopped()) + { + usleep(1000); + } + //cout << "Local Map stopped" << endl; + + // 当前关键帧地图的指针 + Map* pCurrentMap = mpCurrentKF->GetMap(); + // 融合关键帧地图的指针 + Map* pMergeMap = mpMergeMatchedKF->GetMap(); + + // Step 3 利用前面计算的坐标系变换位姿，把整个当前地图（关键帧及地图点）变换到融合帧所在地图 + { + // 把当前关键帧所在的地图位姿带到融合关键帧所在的地图 + // mSold_new = gSw2w1 记录的是当前关键帧世界坐标系到融合关键帧世界坐标系的变换 + float s_on = mSold_new.scale(); + Sophus::SE3f T_on(mSold_new.rotation().cast(), mSold_new.translation().cast()); + + // 锁住altas更新地图 + unique_lock lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate); + + //cout << "KFs before empty: " << mpAtlas->GetCurrentMap()->KeyFramesInMap() << endl; + // 队列里还没来得及处理的关键帧清空 + mpLocalMapper->EmptyQueue(); + //cout << "KFs after empty: " << mpAtlas->GetCurrentMap()->KeyFramesInMap() << endl; + + std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now(); + //cout << "updating active map to merge reference" << endl; + //cout << "curr merge KF id: " << mpCurrentKF->mnId << endl; + //cout << "curr tracking KF id: " << mpTracker->GetLastKeyFrame()->mnId << endl; + // 是否将尺度更新到速度 + bool bScaleVel=false; + if(s_on!=1) // ?判断浮点数和1严格相等是不是不合适？ + bScaleVel=true; + // 利用mSold_new位姿把整个当前地图中的关键帧和地图点变换到融合帧所在地图的坐标系下 + mpAtlas->GetCurrentMap()->ApplyScaledRotation(T_on,s_on,bScaleVel); + // 尺度更新到普通帧位姿 + mpTracker->UpdateFrameIMU(s_on,mpCurrentKF->GetImuBias(),mpTracker->GetLastKeyFrame()); + + std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now(); + } + + // Step 4 如果当前地图IMU没有完全初始化，帮助IMU快速优化； + // 反正都要融合了，这里就拔苗助长完成IMU优化，回头直接全部放到融合地图里就好了 + const int numKFnew=pCurrentMap->KeyFramesInMap(); + + if((mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) + && !pCurrentMap->GetIniertialBA2()) + { + // Map is not completly initialized + Eigen::Vector3d bg, ba; + bg << 0., 0., 0.; + ba << 0., 0., 0.; + // 优化当前地图中参数bg,ba + Optimizer::InertialOptimization(pCurrentMap,bg,ba); + IMU::Bias b (ba[0],ba[1],ba[2],bg[0],bg[1],bg[2]); + unique_lock lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate); + // 用优化得到的 bias 更新普通帧位姿 + mpTracker->UpdateFrameIMU(1.0f,b,mpTracker->GetLastKeyFrame()); + + // Set map initialized + // 设置IMU已经完成初始化 + pCurrentMap->SetIniertialBA2(); + pCurrentMap->SetIniertialBA1(); + pCurrentMap->SetImuInitialized(); + + } + + + //cout << "MergeMap init ID: " << pMergeMap->GetInitKFid() << " CurrMap init ID: " << pCurrentMap->GetInitKFid() << endl; + + // Load KFs and MPs from merge map + //cout << "updating current map" << endl; + // Step 5 地图以旧换新。把融合帧所在地图里的关键帧和地图点从原地图里删掉，变更为当前关键帧所在地图。 + { + // Get Merge Map Mutex (This section stops tracking!!) + unique_lock currentLock(pCurrentMap->mMutexMapUpdate); // We update the current map with the Merge information + unique_lock mergeLock(pMergeMap->mMutexMapUpdate); // We remove the Kfs and MPs in the merged area from the old map + + // 融合帧所在地图的所有关键帧和地图点 + vector vpMergeMapKFs = pMergeMap->GetAllKeyFrames(); + vector vpMergeMapMPs = pMergeMap->GetAllMapPoints(); + + // 遍历每个融合帧所在地图的关键帧 + for(KeyFrame* pKFi : vpMergeMapKFs) + { + if(!pKFi || pKFi->isBad() || pKFi->GetMap() != pMergeMap) + { + continue; + } + + // Make sure connections are updated + // 把该关键帧从融合帧所在地图删掉,加入到当前的地图中 + pKFi->UpdateMap(pCurrentMap); + pCurrentMap->AddKeyFrame(pKFi); + pMergeMap->EraseKeyFrame(pKFi); + } + + // 遍历每个融合帧所在地图的地图点 + for(MapPoint* pMPi : vpMergeMapMPs) + { + if(!pMPi || pMPi->isBad() || pMPi->GetMap() != pMergeMap) + continue; + + // 把地图点添加到当前帧所在地图,从融合帧所在地图删掉 + pMPi->UpdateMap(pCurrentMap); + pCurrentMap->AddMapPoint(pMPi); + pMergeMap->EraseMapPoint(pMPi); + } + // ? BUG! pMergeMap没有设置为BAD + // ? 应该加入如下代码吧？ + // ? mpAtlas->SetMapBad(pMergeMap); + + // Save non corrected poses (already merged maps) + // 存下所有关键帧在融合矫正之前的位姿 + vector vpKFs = pCurrentMap->GetAllKeyFrames(); + for(KeyFrame* pKFi : vpKFs) + { + Sophus::SE3d Tiw = (pKFi->GetPose()).cast(); + g2o::Sim3 g2oSiw(Tiw.unit_quaternion(),Tiw.translation(),1.0); + NonCorrectedSim3[pKFi]=g2oSiw; + } + } + + //cout << "MergeMap init ID: " << pMergeMap->GetInitKFid() << " CurrMap init ID: " << pCurrentMap->GetInitKFid() << endl; + + //cout << "end updating current map" << endl; + + // Critical zone + //bool good = pCurrentMap->CheckEssentialGraph(); + /*if(!good) + cout << "BAD ESSENTIAL GRAPH!!" << endl;*/ + + //cout << "Update essential graph" << endl; + // mpCurrentKF->UpdateConnections(); // to put at false mbFirstConnection + // Step 6 融合新旧地图的生成树 + pMergeMap->GetOriginKF()->SetFirstConnection(false); + pNewChild = mpMergeMatchedKF->GetParent(); // Old parent, it will be the new child of this KF + pNewParent = mpMergeMatchedKF; // Old child, now it will be the parent of its own parent(we need eliminate this KF from children list in its old parent) + mpMergeMatchedKF->ChangeParent(mpCurrentKF); + while(pNewChild) + { + pNewChild->EraseChild(pNewParent); // We remove the relation between the old parent and the new for avoid loop + KeyFrame * pOldParent = pNewChild->GetParent(); + pNewChild->ChangeParent(pNewParent); + pNewParent = pNewChild; + pNewChild = pOldParent; + + } + + + //cout << "MergeMap init ID: " << pMergeMap->GetInitKFid() << " CurrMap init ID: " << pCurrentMap->GetInitKFid() << endl; + + //cout << "end update essential graph" << endl; + + /*good = pCurrentMap->CheckEssentialGraph(); + if(!good) + cout << "BAD ESSENTIAL GRAPH 1!!" << endl;*/ + + //cout << "Update relationship between KFs" << endl; + vector vpCheckFuseMapPoint; // MapPoint vector from current map to allow to fuse duplicated points with the old map (merge) + vector vpCurrentConnectedKFs; + + // 为后续SearchAndFuse准备数据 + // 拿出融合帧的局部窗口, 确保最后是(1+5), 1: 融合帧自己 2: 5个共视关键帧 + mvpMergeConnectedKFs.push_back(mpMergeMatchedKF); + vector aux = mpMergeMatchedKF->GetVectorCovisibleKeyFrames(); + mvpMergeConnectedKFs.insert(mvpMergeConnectedKFs.end(), aux.begin(), aux.end()); + if (mvpMergeConnectedKFs.size()>6) + mvpMergeConnectedKFs.erase(mvpMergeConnectedKFs.begin()+6,mvpMergeConnectedKFs.end()); + /*mvpMergeConnectedKFs = mpMergeMatchedKF->GetVectorCovisibleKeyFrames(); + mvpMergeConnectedKFs.push_back(mpMergeMatchedKF);*/ + + // 拿出当前关键帧的局部窗口, 确保最后是(1+5), 1: 融合帧自己 2: 5个共视关键帧 + mpCurrentKF->UpdateConnections(); + vpCurrentConnectedKFs.push_back(mpCurrentKF); + /*vpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames(); + vpCurrentConnectedKFs.push_back(mpCurrentKF);*/ + aux = mpCurrentKF->GetVectorCovisibleKeyFrames(); + vpCurrentConnectedKFs.insert(vpCurrentConnectedKFs.end(), aux.begin(), aux.end()); + if (vpCurrentConnectedKFs.size()>6) + vpCurrentConnectedKFs.erase(vpCurrentConnectedKFs.begin()+6,vpCurrentConnectedKFs.end()); + + // 所有融合帧局部窗口的地图点 + set spMapPointMerge; + for(KeyFrame* pKFi : mvpMergeConnectedKFs) + { + set vpMPs = pKFi->GetMapPoints(); + spMapPointMerge.insert(vpMPs.begin(),vpMPs.end()); + if(spMapPointMerge.size()>1000) + break; + } + + /*cout << "vpCurrentConnectedKFs.size() " << vpCurrentConnectedKFs.size() << endl; + cout << "mvpMergeConnectedKFs.size() " << mvpMergeConnectedKFs.size() << endl; + cout << "spMapPointMerge.size() " << spMapPointMerge.size() << endl;*/ + + + vpCheckFuseMapPoint.reserve(spMapPointMerge.size()); + std::copy(spMapPointMerge.begin(), spMapPointMerge.end(), std::back_inserter(vpCheckFuseMapPoint)); + //cout << "Finished to update relationship between KFs" << endl; + + //cout << "MergeMap init ID: " << pMergeMap->GetInitKFid() << " CurrMap init ID: " << pCurrentMap->GetInitKFid() << endl; + + /*good = pCurrentMap->CheckEssentialGraph(); + if(!good) + cout << "BAD ESSENTIAL GRAPH 2!!" << endl;*/ + + //cout << "start SearchAndFuse" << endl; + // Step 7 把融合关键帧的共视窗口里的地图点投到当前关键帧的共视窗口里，把重复的点融合掉（以旧换新） + SearchAndFuse(vpCurrentConnectedKFs, vpCheckFuseMapPoint); + //cout << "end SearchAndFuse" << endl; + + //cout << "MergeMap init ID: " << pMergeMap->GetInitKFid() << " CurrMap init ID: " << pCurrentMap->GetInitKFid() << endl; + + /*good = pCurrentMap->CheckEssentialGraph(); + if(!good) + cout << "BAD ESSENTIAL GRAPH 3!!" << endl; + + cout << "Init to update connections" << endl;*/ + + // 更新当前关键帧共视窗口内所有关键帧的连接 + for(KeyFrame* pKFi : vpCurrentConnectedKFs) + { + if(!pKFi || pKFi->isBad()) + continue; + + pKFi->UpdateConnections(); + } + + // 更新融合关键帧共视窗口内所有关键帧的连接 + for(KeyFrame* pKFi : mvpMergeConnectedKFs) + { + if(!pKFi || pKFi->isBad()) + continue; + + pKFi->UpdateConnections(); + } + //cout << "end update connections" << endl; + + //cout << "MergeMap init ID: " << pMergeMap->GetInitKFid() << " CurrMap init ID: " << pCurrentMap->GetInitKFid() << endl; + + /*good = pCurrentMap->CheckEssentialGraph(); + if(!good) + cout << "BAD ESSENTIAL GRAPH 4!!" << endl;*/ + + // TODO Check: If new map is too small, we suppose that not informaiton can be propagated from new to old map + if (numKFnew<10){ + mpLocalMapper->Release(); + return; + } + + /*good = pCurrentMap->CheckEssentialGraph(); + if(!good) + cout << "BAD ESSENTIAL GRAPH 5!!" << endl;*/ + + // Perform BA + bool bStopFlag=false; + KeyFrame* pCurrKF = mpTracker->GetLastKeyFrame(); + //cout << "start MergeInertialBA" << endl; + // Step 8 针对缝合区域的窗口内进行进行welding BA + Optimizer::MergeInertialBA(pCurrKF, mpMergeMatchedKF, &bStopFlag, pCurrentMap,CorrectedSim3); + //cout << "end MergeInertialBA" << endl; + + /*good = pCurrentMap->CheckEssentialGraph(); + if(!good) + cout << "BAD ESSENTIAL GRAPH 6!!" << endl;*/ + + // Release Local Mapping. + mpLocalMapper->Release(); + + + return; +} + +/** + * @brief 1.0版本新的调试函数，暂时跳过 + */ +void LoopClosing::CheckObservations(set &spKFsMap1, set &spKFsMap2) +{ + cout << "----------------------" << endl; + for(KeyFrame* pKFi1 : spKFsMap1) + { + map mMatchedMP; + set spMPs = pKFi1->GetMapPoints(); + + for(MapPoint* pMPij : spMPs) + { + if(!pMPij || pMPij->isBad()) + { + continue; + } + + map> mMPijObs = pMPij->GetObservations(); + for(KeyFrame* pKFi2 : spKFsMap2) + { + if(mMPijObs.find(pKFi2) != mMPijObs.end()) + { + if(mMatchedMP.find(pKFi2) != mMatchedMP.end()) + { + mMatchedMP[pKFi2] = mMatchedMP[pKFi2] + 1; + } + else + { + mMatchedMP[pKFi2] = 1; + } + } + } + + } + + if(mMatchedMP.size() == 0) + { + cout << "CHECK-OBS: KF " << pKFi1->mnId << " has not any matched MP with the other map" << endl; + } + else + { + cout << "CHECK-OBS: KF " << pKFi1->mnId << " has matched MP with " << mMatchedMP.size() << " KF from the other map" << endl; + for(pair matchedKF : mMatchedMP) + { + cout << " -KF: " << matchedKF.first->mnId << ", Number of matches: " << matchedKF.second << endl; + } + } + } + cout << "----------------------" << endl; +} + +/** + * @brief 查找对应MP与融合 + * @param CorrectedPosesMap 关键帧及对应的pose + * @param vpMapPoints 待融合地图的融合帧及其5个共视关键帧对应的mp（1000个以内）（注意此时所有kf与mp全部移至当前地图，这里的待融合地图的说法只为区分，因为还没有融合） + */ +void LoopClosing::SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap, vector &vpMapPoints) +{ + ORBmatcher matcher(0.8); + + int total_replaces = 0; + + // 遍历每个关键帧 + //cout << "[FUSE]: Initially there are " << vpMapPoints.size() << " MPs" << endl; + //cout << "FUSE: Intially there are " << CorrectedPosesMap.size() << " KFs" << endl; + for(KeyFrameAndPose::const_iterator mit=CorrectedPosesMap.begin(), mend=CorrectedPosesMap.end(); mit!=mend;mit++) + { + int num_replaces = 0; + KeyFrame* pKFi = mit->first; + Map* pMap = pKFi->GetMap(); + + g2o::Sim3 g2oScw = mit->second; + Sophus::Sim3f Scw = Converter::toSophus(g2oScw); + + vector vpReplacePoints(vpMapPoints.size(),static_cast(NULL)); + + // 新点表示pKFi对应的点，老点表示pKFi对应的回环点 + // 将vpMapPoints投到pKF里面看看有没有匹配的MP，如果没有直接添加，如果有，暂时将老点放入至vpReplacePoints + // vpReplacePoints下标表示第n个vpMapPoints，存放着新点，可以直接找到对应信息 + int numFused = matcher.Fuse(pKFi,Scw,vpMapPoints,4,vpReplacePoints); + + // Get Map Mutex + unique_lock lock(pMap->mMutexMapUpdate); + // 更新点 + const int nLP = vpMapPoints.size(); + for(int i=0; iReplace(vpMapPoints[i]); + + } + } + + total_replaces += num_replaces; + } + //cout << "[FUSE]: " << total_replaces << " MPs had been fused" << endl; +} + +/** + * @brief 查找对应MP与融合，与上个函数类似 + * @param vConectedKFs 当前地图的当前关键帧及5个共视关键帧 + * @param vpMapPoints 待融合地图的融合帧及其5个共视关键帧对应的mp（1000个以内）（注意此时所有kf与mp全部移至当前地图，这里的待融合地图的说法只为区分，因为还没有融合） + */ +void LoopClosing::SearchAndFuse(const vector &vConectedKFs, vector &vpMapPoints) +{ + ORBmatcher matcher(0.8); + + int total_replaces = 0; + + //cout << "FUSE-POSE: Initially there are " << vpMapPoints.size() << " MPs" << endl; + //cout << "FUSE-POSE: Intially there are " << vConectedKFs.size() << " KFs" << endl; + for(auto mit=vConectedKFs.begin(), mend=vConectedKFs.end(); mit!=mend;mit++) + { + int num_replaces = 0; + KeyFrame* pKF = (*mit); + Map* pMap = pKF->GetMap(); + Sophus::SE3f Tcw = pKF->GetPose(); + Sophus::Sim3f Scw(Tcw.unit_quaternion(),Tcw.translation()); + Scw.setScale(1.f); + /*std::cout << "These should be zeros: " << + Scw.rotationMatrix() - Tcw.rotationMatrix() << std::endl << + Scw.translation() - Tcw.translation() << std::endl << + Scw.scale() - 1.f << std::endl;*/ + vector vpReplacePoints(vpMapPoints.size(),static_cast(NULL)); + matcher.Fuse(pKF,Scw,vpMapPoints,4,vpReplacePoints); + + // Get Map Mutex + unique_lock lock(pMap->mMutexMapUpdate); + const int nLP = vpMapPoints.size(); + for(int i=0; iReplace(vpMapPoints[i]); + } + } + /*cout << "FUSE-POSE: KF " << pKF->mnId << " ->" << num_replaces << " MPs fused" << endl; + total_replaces += num_replaces;*/ + } + //cout << "FUSE-POSE: " << total_replaces << " MPs had been fused" << endl; +} + + +/** + * @brief 由外部线程调用,请求复位当前线程 + */ +void LoopClosing::RequestReset() +{ + { + unique_lock lock(mMutexReset); + mbResetRequested = true; + } + + while(1) + { + { + unique_lock lock2(mMutexReset); + if(!mbResetRequested) + break; + } + usleep(5000); + } +} + +void LoopClosing::RequestResetActiveMap(Map *pMap) +{ + { + unique_lock lock(mMutexReset); + mbResetActiveMapRequested = true; + mpMapToReset = pMap; + } + + while(1) + { + { + unique_lock lock2(mMutexReset); + if(!mbResetActiveMapRequested) + break; + } + usleep(3000); + } +} + +/** + * @brief 当前线程调用,检查是否有外部线程请求复位当前线程,如果有的话就复位回环检测线程 + */ +void LoopClosing::ResetIfRequested() +{ + unique_lock lock(mMutexReset); + // 如果有来自于外部的线程的复位请求,那么就复位当前线程 + if(mbResetRequested) + { + cout << "Loop closer reset requested..." << endl; + // 清空参与和进行回环检测的关键帧队列 + mlpLoopKeyFrameQueue.clear(); + // 上一次没有和任何关键帧形成闭环关系 + mLastLoopKFid=0; //TODO old variable, it is not use in the new algorithm + // 复位请求标志复位 + mbResetRequested=false; + mbResetActiveMapRequested = false; + } + else if(mbResetActiveMapRequested) + { + + for (list::const_iterator it=mlpLoopKeyFrameQueue.begin(); it != mlpLoopKeyFrameQueue.end();) + { + KeyFrame* pKFi = *it; + if(pKFi->GetMap() == mpMapToReset) + { + it = mlpLoopKeyFrameQueue.erase(it); + } + else + ++it; + } + + mLastLoopKFid=mpAtlas->GetLastInitKFid(); //TODO old variable, it is not use in the new algorithm + mbResetActiveMapRequested=false; + + } +} + +/** + * @brief MergeLocal CorrectLoop 中调用 + * @param pActiveMap 当前地图 + * @param nLoopKF 检测到回环成功的关键帧，不是与之匹配的老关键帧 + */ +void LoopClosing::RunGlobalBundleAdjustment(Map* pActiveMap, unsigned long nLoopKF) +{ + Verbose::PrintMess("Starting Global Bundle Adjustment", Verbose::VERBOSITY_NORMAL); + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartFGBA = std::chrono::steady_clock::now(); + + nFGBA_exec += 1; + + vnGBAKFs.push_back(pActiveMap->GetAllKeyFrames().size()); + vnGBAMPs.push_back(pActiveMap->GetAllMapPoints().size()); +#endif + + // imu 初始化成功才返回true，只要一阶段成功就为true + const bool bImuInit = pActiveMap->isImuInitialized(); + + if(!bImuInit) + Optimizer::GlobalBundleAdjustemnt(pActiveMap,10,&mbStopGBA,nLoopKF,false); + else + // 仅有一个地图且内部关键帧<200，并且IMU完成了第一阶段初始化后才会进行下面 + Optimizer::FullInertialBA(pActiveMap,7,false,nLoopKF,&mbStopGBA); + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndGBA = std::chrono::steady_clock::now(); + + double timeGBA = std::chrono::duration_cast >(time_EndGBA - time_StartFGBA).count(); + vdGBA_ms.push_back(timeGBA); + + if(mbStopGBA) + { + nFGBA_abort += 1; + } +#endif + // 记录GBA已经迭代次数,用来检查全局BA过程是否是因为意外结束的 + int idx = mnFullBAIdx; + // Optimizer::GlobalBundleAdjustemnt(mpMap,10,&mbStopGBA,nLoopKF,false); + + // Update all MapPoints and KeyFrames + // Local Mapping was active during BA, that means that there might be new keyframes + // not included in the Global BA and they are not consistent with the updated map. + // We need to propagate the correction through the spanning tree + { + unique_lock lock(mMutexGBA); + if(idx!=mnFullBAIdx) + return; + + if(!bImuInit && pActiveMap->isImuInitialized()) + return; + + if(!mbStopGBA) + { + Verbose::PrintMess("Global Bundle Adjustment finished", Verbose::VERBOSITY_NORMAL); + Verbose::PrintMess("Updating map ...", Verbose::VERBOSITY_NORMAL); + + mpLocalMapper->RequestStop(); + // Wait until Local Mapping has effectively stopped + + while(!mpLocalMapper->isStopped() && !mpLocalMapper->isFinished()) + { + usleep(1000); + } + + // Get Map Mutex + unique_lock lock(pActiveMap->mMutexMapUpdate); + // cout << "LC: Update Map Mutex adquired" << endl; + + //pActiveMap->PrintEssentialGraph(); + // Correct keyframes starting at map first keyframe + list lpKFtoCheck(pActiveMap->mvpKeyFrameOrigins.begin(),pActiveMap->mvpKeyFrameOrigins.end()); + + // 通过树的方式更新未参与全局优化的关键帧，一个关键帧与其父节点的共视点数最多，所以选其作为参考帧 + while(!lpKFtoCheck.empty()) + { + KeyFrame* pKF = lpKFtoCheck.front(); + const set sChilds = pKF->GetChilds(); + //cout << "---Updating KF " << pKF->mnId << " with " << sChilds.size() << " childs" << endl; + //cout << " KF mnBAGlobalForKF: " << pKF->mnBAGlobalForKF << endl; + Sophus::SE3f Twc = pKF->GetPoseInverse(); + //cout << "Twc: " << Twc << endl; + //cout << "GBA: Correct KeyFrames" << endl; + // 广度优先搜索 + for(set::const_iterator sit=sChilds.begin();sit!=sChilds.end();sit++) + { + KeyFrame* pChild = *sit; + if(!pChild || pChild->isBad()) + continue; + + // 专门处理没有参与优化的新关键帧 + if(pChild->mnBAGlobalForKF!=nLoopKF) + { + //cout << "++++New child with flag " << pChild->mnBAGlobalForKF << "; LoopKF: " << nLoopKF << endl; + //cout << " child id: " << pChild->mnId << endl; + Sophus::SE3f Tchildc = pChild->GetPose() * Twc; + //cout << "Child pose: " << Tchildc << endl; + //cout << "pKF->mTcwGBA: " << pKF->mTcwGBA << endl; + pChild->mTcwGBA = Tchildc * pKF->mTcwGBA;//*Tcorc*pKF->mTcwGBA; + + Sophus::SO3f Rcor = pChild->mTcwGBA.so3().inverse() * pChild->GetPose().so3(); + if(pChild->isVelocitySet()){ + pChild->mVwbGBA = Rcor * pChild->GetVelocity(); + } + else + Verbose::PrintMess("Child velocity empty!! ", Verbose::VERBOSITY_NORMAL); + + + //cout << "Child bias: " << pChild->GetImuBias() << endl; + pChild->mBiasGBA = pChild->GetImuBias(); + + + pChild->mnBAGlobalForKF = nLoopKF; // 标记成更新过的 + + } + lpKFtoCheck.push_back(pChild); + } + + //cout << "-------Update pose" << endl; + pKF->mTcwBefGBA = pKF->GetPose(); + //cout << "pKF->mTcwBefGBA: " << pKF->mTcwBefGBA << endl; + pKF->SetPose(pKF->mTcwGBA); + /*cv::Mat Tco_cn = pKF->mTcwBefGBA * pKF->mTcwGBA.inv(); + cv::Vec3d trasl = Tco_cn.rowRange(0,3).col(3); + double dist = cv::norm(trasl); + cout << "GBA: KF " << pKF->mnId << " had been moved " << dist << " meters" << endl; + double desvX = 0; + double desvY = 0; + double desvZ = 0; + if(pKF->mbHasHessian) + { + cv::Mat hessianInv = pKF->mHessianPose.inv(); + + double covX = hessianInv.at(3,3); + desvX = std::sqrt(covX); + double covY = hessianInv.at(4,4); + desvY = std::sqrt(covY); + double covZ = hessianInv.at(5,5); + desvZ = std::sqrt(covZ); + pKF->mbHasHessian = false; + } + if(dist > 1) + { + cout << "--To much distance correction: It has " << pKF->GetConnectedKeyFrames().size() << " connected KFs" << endl; + cout << "--It has " << pKF->GetCovisiblesByWeight(80).size() << " connected KF with 80 common matches or more" << endl; + cout << "--It has " << pKF->GetCovisiblesByWeight(50).size() << " connected KF with 50 common matches or more" << endl; + cout << "--It has " << pKF->GetCovisiblesByWeight(20).size() << " connected KF with 20 common matches or more" << endl; + + cout << "--STD in meters(x, y, z): " << desvX << ", " << desvY << ", " << desvZ << endl; + + + string strNameFile = pKF->mNameFile; + cv::Mat imLeft = cv::imread(strNameFile, CV_LOAD_IMAGE_UNCHANGED); + + cv::cvtColor(imLeft, imLeft, CV_GRAY2BGR); + + vector vpMapPointsKF = pKF->GetMapPointMatches(); + int num_MPs = 0; + for(int i=0; iisBad()) + { + continue; + } + num_MPs += 1; + string strNumOBs = to_string(vpMapPointsKF[i]->Observations()); + cv::circle(imLeft, pKF->mvKeys[i].pt, 2, cv::Scalar(0, 255, 0)); + cv::putText(imLeft, strNumOBs, pKF->mvKeys[i].pt, CV_FONT_HERSHEY_DUPLEX, 1, cv::Scalar(255, 0, 0)); + } + cout << "--It has " << num_MPs << " MPs matched in the map" << endl; + + string namefile = "./test_GBA/GBA_" + to_string(nLoopKF) + "_KF" + to_string(pKF->mnId) +"_D" + to_string(dist) +".png"; + cv::imwrite(namefile, imLeft); + }*/ + + + if(pKF->bImu) + { + //cout << "-------Update inertial values" << endl; + pKF->mVwbBefGBA = pKF->GetVelocity(); + //if (pKF->mVwbGBA.empty()) + // Verbose::PrintMess("pKF->mVwbGBA is empty", Verbose::VERBOSITY_NORMAL); + + //assert(!pKF->mVwbGBA.empty()); + pKF->SetVelocity(pKF->mVwbGBA); + pKF->SetNewBias(pKF->mBiasGBA); + } + + lpKFtoCheck.pop_front(); + } + + //cout << "GBA: Correct MapPoints" << endl; + // Correct MapPoints + // 更新mp点 + const vector vpMPs = pActiveMap->GetAllMapPoints(); + + for(size_t i=0; iisBad()) + continue; + + // NOTICE 并不是所有的地图点都会直接参与到全局BA优化中,但是大部分的地图点需要根据全局BA优化后的结果来重新纠正自己的位姿 + // 如果这个地图点直接参与到了全局BA优化的过程,那么就直接重新设置器位姿即可 + if(pMP->mnBAGlobalForKF==nLoopKF) + { + // If optimized by Global BA, just update + pMP->SetWorldPos(pMP->mPosGBA); + } + else // 如故这个地图点并没有直接参与到全局BA优化的过程中,那么就使用器参考关键帧的新位姿来优化自己的位姿 + { + // Update according to the correction of its reference keyframe + // 说明这个关键帧，在前面的过程中也没有因为“当前关键帧”得到全局BA优化 + //? 可是,为什么会出现这种情况呢? 难道是因为这个地图点的参考关键帧设置成为了bad? + KeyFrame* pRefKF = pMP->GetReferenceKeyFrame(); + + if(pRefKF->mnBAGlobalForKF!=nLoopKF) + continue; + + /*if(pRefKF->mTcwBefGBA.empty()) + continue;*/ + + // Map to non-corrected camera + // cv::Mat Rcw = pRefKF->mTcwBefGBA.rowRange(0,3).colRange(0,3); + // cv::Mat tcw = pRefKF->mTcwBefGBA.rowRange(0,3).col(3); + // 转换到其参考关键帧相机坐标系下的坐标 + Eigen::Vector3f Xc = pRefKF->mTcwBefGBA * pMP->GetWorldPos(); + + // Backproject using corrected camera + // 然后使用已经纠正过的参考关键帧的位姿,再将该地图点变换到世界坐标系下 + pMP->SetWorldPos(pRefKF->GetPoseInverse() * Xc); + } + } + + pActiveMap->InformNewBigChange(); + pActiveMap->IncreaseChangeIndex(); + + // TODO Check this update + // mpTracker->UpdateFrameIMU(1.0f, mpTracker->GetLastKeyFrame()->GetImuBias(), mpTracker->GetLastKeyFrame()); + + mpLocalMapper->Release(); + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndUpdateMap = std::chrono::steady_clock::now(); + + double timeUpdateMap = std::chrono::duration_cast >(time_EndUpdateMap - time_EndGBA).count(); + vdUpdateMap_ms.push_back(timeUpdateMap); + + double timeFGBA = std::chrono::duration_cast >(time_EndUpdateMap - time_StartFGBA).count(); + vdFGBATotal_ms.push_back(timeFGBA); +#endif + Verbose::PrintMess("Map updated!", Verbose::VERBOSITY_NORMAL); + } + + mbFinishedGBA = true; + mbRunningGBA = false; + } +} + +// 由外部线程调用,请求终止当前线程 +void LoopClosing::RequestFinish() +{ + unique_lock lock(mMutexFinish); + // cout << "LC: Finish requested" << endl; + mbFinishRequested = true; +} + +// 当前线程调用,查看是否有外部线程请求当前线程 +bool LoopClosing::CheckFinish() +{ + unique_lock lock(mMutexFinish); + return mbFinishRequested; +} + +// 有当前线程调用,执行完成该函数之后线程主函数退出,线程销毁 +void LoopClosing::SetFinish() +{ + unique_lock lock(mMutexFinish); + mbFinished = true; +} + +// 由外部线程调用,判断当前回环检测线程是否已经正确终止了 +bool LoopClosing::isFinished() +{ + unique_lock lock(mMutexFinish); + return mbFinished; +} + + +} //namespace ORB_SLAM diff --git a/ORB-SLAM3-UESTC/Workspace/src/MLPnPsolver.cpp b/ORB-SLAM3-UESTC/Workspace/src/MLPnPsolver.cpp new file mode 100644 index 0000000..47361f6 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/MLPnPsolver.cpp @@ -0,0 +1,1745 @@ +/** + * This file is part of ORB-SLAM3 + * + * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * + * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public + * License as published by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even + * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with ORB-SLAM3. + * If not, see . + */ + +/****************************************************************************** + * Author: Steffen Urban * + * Contact: urbste@gmail.com * + * License: Copyright (c) 2016 Steffen Urban, ANU. All rights reserved. * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions * + * are met: * + * * Redistributions of source code must retain the above copyright * + * notice, this list of conditions and the following disclaimer. * + * * Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * Neither the name of ANU nor the names of its contributors may be * + * used to endorse or promote products derived from this software without * + * specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE * + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR * + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * + * SUCH DAMAGE. * + ******************************************************************************/ +#include "MLPnPsolver.h" + +#include + +// MLPnP算法，极大似然PnP算法，解决PnP问题，用在重定位中，不用运动的先验知识来估计相机位姿 +// 基于论文《MLPNP - A REAL-TIME MAXIMUM LIKELIHOOD SOLUTION TO THE PERSPECTIVE-N-POINT PROBLEM》 + +namespace ORB_SLAM3 +{ +/** + * @brief MLPnP 构造函数 + * + * @param[in] F 输入帧的数据 + * @param[in] vpMapPointMatches 待匹配的特征点 + * @param[in] mnInliersi 内点的个数 + * @param[in] mnIterations Ransac迭代次数 + * @param[in] mnBestInliers 最佳内点数 + * @param[in] N 所有2D点的个数 + * @param[in] mpCamera 相机模型，利用该变量对3D点进行投影 + */ +MLPnPsolver::MLPnPsolver(const Frame &F, // 输入帧的数据 + const vector &vpMapPointMatches // 待匹配的特征点，是当前帧和候选关键帧用BoW进行快速匹配的结果 + ) : mnInliersi(0), // 内点的个数 + mnIterations(0), // Ransac迭代次数 + mnBestInliers(0), // 最佳内点数 + N(0), // 所有2D点的个数 + mpCamera(F.mpCamera) // 相机模型，利用该变量对3D点进行投影 +{ + mvpMapPointMatches = vpMapPointMatches; // 待匹配的特征点，是当前帧和候选关键帧用BoW进行快速匹配的结果 + mvBearingVecs.reserve(F.mvpMapPoints.size()); // 初始化3D点的单位向量 + mvP2D.reserve(F.mvpMapPoints.size()); // 初始化3D点的投影点 + mvSigma2.reserve(F.mvpMapPoints.size()); // 初始化卡方检验中的sigma值 + mvP3Dw.reserve(F.mvpMapPoints.size()); // 初始化3D点坐标 + mvKeyPointIndices.reserve(F.mvpMapPoints.size()); // 初始化3D点的索引值 + mvAllIndices.reserve(F.mvpMapPoints.size()); // 初始化所有索引值 + + // 一些必要的初始化操作 + int idx = 0; + for (size_t i = 0, iend = mvpMapPointMatches.size(); i < iend; i++) + { + MapPoint *pMP = vpMapPointMatches[i]; + + // 如果pMP存在，则接下来初始化一些参数，否则什么都不做 + if (pMP) + { + // 判断是否是坏点 + if (!pMP->isBad()) + { + // 如果记录的点个数超过总数，则不做任何事情，否则继续记录 + if (i >= F.mvKeysUn.size()) + continue; + const cv::KeyPoint &kp = F.mvKeysUn[i]; + + // 保存3D点的投影点 + mvP2D.push_back(kp.pt); + + // 保存卡方检验中的sigma值 + mvSigma2.push_back(F.mvLevelSigma2[kp.octave]); + + // Bearing vector should be normalized + // 特征点投影，并计算单位向量 + cv::Point3f cv_br = mpCamera->unproject(kp.pt); + cv_br /= cv_br.z; + bearingVector_t br(cv_br.x, cv_br.y, cv_br.z); + mvBearingVecs.push_back(br); + + // 3D coordinates + // 获取当前特征点的3D坐标 + Eigen::Matrix posEig = pMP->GetWorldPos(); + point_t pos(posEig(0), posEig(1), posEig(2)); + mvP3Dw.push_back(pos); + + // 记录当前特征点的索引值，挑选后的 + mvKeyPointIndices.push_back(i); + + // 记录所有特征点的索引值 + mvAllIndices.push_back(idx); + + idx++; + } + } + } + + // 设置RANSAC参数 + SetRansacParameters(); +} + +// RANSAC methods +/** + * @brief MLPnP迭代计算相机位姿 + * + * @param[in] nIterations 迭代次数 + * @param[in] bNoMore 达到最大迭代次数的标志 + * @param[in] vbInliers 内点的标记 + * @param[in] nInliers 总共内点数 + * @return cv::Mat 计算出来的位姿 + */ +bool MLPnPsolver::iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers, Eigen::Matrix4f &Tout) +{ + Tout.setIdentity(); + bNoMore = false; // 已经达到最大迭代次数的标志 + vbInliers.clear(); // 清除保存判断是否是内点的容器 + nInliers = 0; // 当前次迭代时的内点数 + // N为所有2D点的个数, mRansacMinInliers为正常退出RANSAC迭代过程中最少的inlier数 + // Step 1: 判断，如果2D点个数不足以启动RANSAC迭代过程的最小下限，则退出 + if (N < mRansacMinInliers) + { + bNoMore = true; // 已经达到最大迭代次数的标志 + return false; // 函数退出 + } + + // mvAllIndices为所有参与PnP的2D点的索引 + // vAvailableIndices为每次从mvAllIndices中随机挑选mRansacMinSet组3D-2D对应点进行一次RANSAC + vector vAvailableIndices; + + // 当前的迭代次数id + int nCurrentIterations = 0; + + // Step 2: 正常迭代计算进行相机位姿估计，如果满足效果上限，直接返回最佳估计结果，否则就继续利用最小集(6个点)估计位姿 + // 进行迭代的条件: + // 条件1: 历史进行的迭代次数少于最大迭代值 + // 条件2: 当前进行的迭代次数少于当前函数给定的最大迭代值 + while (mnIterations < mRansacMaxIts || nCurrentIterations < nIterations) + { + // 迭代次数更新加1，直到达到最大迭代次数 + nCurrentIterations++; + mnIterations++; + + // 清空已有的匹配点的计数,为新的一次迭代作准备 + vAvailableIndices = mvAllIndices; + + // Bearing vectors and 3D points used for this ransac iteration + // 初始化单位向量和3D点，给当前ransac迭代使用 + bearingVectors_t bearingVecs(mRansacMinSet); + points_t p3DS(mRansacMinSet); + vector indexes(mRansacMinSet); + + // Get min set of points + // 选取最小集,从vAvailableIndices中选取mRansacMinSet个点进行操作，这里应该是6 + for (short i = 0; i < mRansacMinSet; ++i) + { + // 在所有备选点中随机抽取一个，通过随机抽取索引数组vAvailableIndices的索引[randi]来实现 + int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size() - 1); + + // vAvailableIndices[randi]才是备选点真正的索引值，randi是索引数组的索引值，不要搞混了 + int idx = vAvailableIndices[randi]; + + bearingVecs[i] = mvBearingVecs[idx]; + p3DS[i] = mvP3Dw[idx]; + indexes[i] = i; + + // 把抽取出来的点从所有备选点数组里删除掉，概率论中不放回的操作 + vAvailableIndices[randi] = vAvailableIndices.back(); + vAvailableIndices.pop_back(); + } // 选取最小集 + + // By the moment, we are using MLPnP without covariance info + // 目前为止，还没有使用协方差的信息，所以这里生成一个size=1的值为0的协方差矩阵 + // |0 0 0| + // covs[0] = |0 0 0| + // |0 0 0| + // ? 为什么不用协方差的SVD分解，计算耗时还是效果不明显？ + cov3_mats_t covs(1); + + // Result + transformation_t result; + + // Compute camera pose + // 相机位姿估计，MLPnP最主要的操作在这里 + computePose(bearingVecs, p3DS, covs, indexes, result); + + // Save result + // 论文中12个待求值赋值保存在mRi中，每个求解器都有保存各自的计算结果 + mRi[0][0] = result(0, 0); + mRi[0][1] = result(0, 1); + mRi[0][2] = result(0, 2); + + mRi[1][0] = result(1, 0); + mRi[1][1] = result(1, 1); + mRi[1][2] = result(1, 2); + + mRi[2][0] = result(2, 0); + mRi[2][1] = result(2, 1); + mRi[2][2] = result(2, 2); + + mti[0] = result(0, 3); + mti[1] = result(1, 3); + mti[2] = result(2, 3); + + // Check inliers + // 卡方检验内点，和EPnP基本类似 + CheckInliers(); + + if (mnInliersi >= mRansacMinInliers) + { + // If it is the best solution so far, save it + // 如果该结果是目前内点数最多的，说明该结果是目前最好的，保存起来 + if (mnInliersi > mnBestInliers) + { + mvbBestInliers = mvbInliersi; // 每个点是否是内点的标记 + mnBestInliers = mnInliersi; // 内点个数 + + cv::Mat Rcw(3, 3, CV_64F, mRi); + cv::Mat tcw(3, 1, CV_64F, mti); + Rcw.convertTo(Rcw, CV_32F); + tcw.convertTo(tcw, CV_32F); + mBestTcw.setIdentity(); + mBestTcw.block<3, 3>(0, 0) = Converter::toMatrix3f(Rcw); + mBestTcw.block<3, 1>(0, 3) = Converter::toVector3f(tcw); + + Eigen::Matrix eigRcw(mRi[0]); + Eigen::Vector3d eigtcw(mti); + } + + // 用新的内点对相机位姿精求解，提高位姿估计精度，这里如果有足够内点的话，函数直接返回该值，不再继续计算 + if (Refine()) + { + nInliers = mnRefinedInliers; + vbInliers = vector(mvpMapPointMatches.size(), false); + for (int i = 0; i < N; i++) + { + if (mvbRefinedInliers[i]) + vbInliers[mvKeyPointIndices[i]] = true; + } + Tout = mRefinedTcw; + return true; + } + } + } // 迭代 + + // Step 3: 选择最小集中效果最好的相机位姿估计结果,如果没有，只能用6个点去计算这个值了 + // 程序运行到这里，说明Refine失败了，说明精求解过程中，内点的个数不满足最小阈值，那就只能在当前结果中选择内点数最多的那个最小集 + // 但是也意味着这样子的结果最终是用6个点来求出来的，近似效果一般 + if (mnIterations >= mRansacMaxIts) + { + bNoMore = true; + if (mnBestInliers >= mRansacMinInliers) + { + nInliers = mnBestInliers; + vbInliers = vector(mvpMapPointMatches.size(), false); + for (int i = 0; i < N; i++) + { + if (mvbBestInliers[i]) + vbInliers[mvKeyPointIndices[i]] = true; + } + Tout = mBestTcw; + return true; + } + } + + // step 4 相机位姿估计失败，返回零值 + // 程序运行到这里，那说明没有满足条件的相机位姿估计结果，位姿估计失败了 + return false; +} + +/** + * @brief 设置RANSAC迭代的参数 + * + * @param[in] probability 模型最大概率值，默认0.9 + * @param[in] minInliers 内点的最小阈值，默认8 + * @param[in] maxIterations 最大迭代次数，默认300 + * @param[in] minSet 最小集，每次采样六个点，即最小集应该设置为6，论文里面写着I > 5 + * @param[in] epsilon 理论最少内点个数，这里是按照总数的比例计算，所以epsilon是比例，默认是0.4 + * @param[in] th2 卡方检验阈值 + * + */ +void MLPnPsolver::SetRansacParameters(double probability, int minInliers, int maxIterations, int minSet, float epsilon, float th2) +{ + mRansacProb = probability; // 模型最大概率值，默认0.9 + mRansacMinInliers = minInliers; // 内点的最小阈值，默认8 + mRansacMaxIts = maxIterations; // 最大迭代次数，默认300 + mRansacEpsilon = epsilon; // 理论最少内点个数，这里是按照总数的比例计算，所以epsilon是比例，默认是0.4 + mRansacMinSet = minSet; // 每次采样六个点，即最小集应该设置为6，论文里面写着I > 5 + + N = mvP2D.size(); // number of correspondences + + mvbInliersi.resize(N); // 是否是内点的标记位 + + // Adjust Parameters according to number of correspondences + // 计算最少个数点，选择(给定内点数, 最小集, 理论内点数)的最小值 + int nMinInliers = N * mRansacEpsilon; + if (nMinInliers < mRansacMinInliers) + nMinInliers = mRansacMinInliers; + if (nMinInliers < minSet) + nMinInliers = minSet; + mRansacMinInliers = nMinInliers; + + // 根据最终得到的"最小内点数"来调整 内点数/总体数 比例epsilon + if (mRansacEpsilon < (float)mRansacMinInliers / N) + mRansacEpsilon = (float)mRansacMinInliers / N; + + // Set RANSAC iterations according to probability, epsilon, and max iterations + // 根据给出的各种参数计算RANSAC的理论迭代次数,并且敲定最终在迭代过程中使用的RANSAC最大迭代次数 + int nIterations; + + if (mRansacMinInliers == N) + nIterations = 1; + else + nIterations = ceil(log(1 - mRansacProb) / log(1 - pow(mRansacEpsilon, 3))); + + mRansacMaxIts = max(1, min(nIterations, mRansacMaxIts)); + + // 计算不同图层上的特征点在进行内点检验的时候,所使用的不同判断误差阈值 + mvMaxError.resize(mvSigma2.size()); // 层数 + for (size_t i = 0; i < mvSigma2.size(); i++) + mvMaxError[i] = mvSigma2[i] * th2; // 不同的尺度，设置不同的最大偏差 +} + +/** + * @brief 通过之前求解的(R t)检查哪些3D-2D点对属于inliers + */ +void MLPnPsolver::CheckInliers() +{ + mnInliersi = 0; + + // 遍历当前帧中所有的匹配点 + for (int i = 0; i < N; i++) + { + // 取出对应的3D点和2D点 + point_t p = mvP3Dw[i]; + cv::Point3f P3Dw(p(0), p(1), p(2)); + cv::Point2f P2D = mvP2D[i]; + + // 将3D点由世界坐标系旋转到相机坐标系 + float xc = mRi[0][0] * P3Dw.x + mRi[0][1] * P3Dw.y + mRi[0][2] * P3Dw.z + mti[0]; + float yc = mRi[1][0] * P3Dw.x + mRi[1][1] * P3Dw.y + mRi[1][2] * P3Dw.z + mti[1]; + float zc = mRi[2][0] * P3Dw.x + mRi[2][1] * P3Dw.y + mRi[2][2] * P3Dw.z + mti[2]; + + // 将相机坐标系下的3D进行投影 + cv::Point3f P3Dc(xc, yc, zc); + cv::Point2f uv = mpCamera->project(P3Dc); + + // 计算残差 + float distX = P2D.x - uv.x; + float distY = P2D.y - uv.y; + + float error2 = distX * distX + distY * distY; + + // 判定是不是内点 + if (error2 < mvMaxError[i]) + { + mvbInliersi[i] = true; + mnInliersi++; + } + else + { + mvbInliersi[i] = false; + } + } +} + +/** + * @brief 使用新的内点来继续对位姿进行精求解 + */ +bool MLPnPsolver::Refine() +{ + // 记录内点的索引值 + vector vIndices; + vIndices.reserve(mvbBestInliers.size()); + + for (size_t i = 0; i < mvbBestInliers.size(); i++) + { + if (mvbBestInliers[i]) + { + vIndices.push_back(i); + } + } + + // Bearing vectors and 3D points used for this ransac iteration + // 因为是重定义，所以要另外定义局部变量，和iterate里面一样 + // 初始化单位向量和3D点，给当前重定义函数使用 + bearingVectors_t bearingVecs; + points_t p3DS; + vector indexes; + + // 注意这里是用所有内点vIndices.size()来进行相机位姿估计 + // 而iterate里面是用最小集mRansacMinSet(6个)来进行相机位姿估计 + // TODO:有什么区别呢？答：肯定有啦，mRansacMinSet只是粗略解， + // 这里之所以要Refine就是要用所有满足模型的内点来更加精确地近似表达模型 + // 这样求出来的解才会更加准确 + for (size_t i = 0; i < vIndices.size(); i++) + { + int idx = vIndices[i]; + + bearingVecs.push_back(mvBearingVecs[idx]); + p3DS.push_back(mvP3Dw[idx]); + indexes.push_back(i); + } + // 后面操作和iterate类似，就不赘述了 + + // By the moment, we are using MLPnP without covariance info + cov3_mats_t covs(1); + + // Result + transformation_t result; + + // Compute camera pose + computePose(bearingVecs, p3DS, covs, indexes, result); + + // Check inliers + CheckInliers(); + + mnRefinedInliers = mnInliersi; + mvbRefinedInliers = mvbInliersi; + + if (mnInliersi > mRansacMinInliers) + { + cv::Mat Rcw(3, 3, CV_64F, mRi); + cv::Mat tcw(3, 1, CV_64F, mti); + Rcw.convertTo(Rcw, CV_32F); + tcw.convertTo(tcw, CV_32F); + mRefinedTcw.setIdentity(); + + mRefinedTcw.block<3, 3>(0, 0) = Converter::toMatrix3f(Rcw); + mRefinedTcw.block<3, 1>(0, 3) = Converter::toVector3f(tcw); + + Eigen::Matrix eigRcw(mRi[0]); + Eigen::Vector3d eigtcw(mti); + return true; + } + + return false; +} + +// MLPnP methods +/** + * @brief MLPnP相机位姿估计 + * + * @param[in] f 单位向量 + * @param[in] p 点的3D坐标 + * @param[in] covMats 协方差矩阵 + * @param[in] indices 对应点的索引值 + * @param[in] result 相机位姿估计结果 + * + */ +void MLPnPsolver::computePose(const bearingVectors_t &f, const points_t &p, const cov3_mats_t &covMats, + const std::vector &indices, transformation_t &result) +{ + // Step 1: 判断点的数量是否满足计算条件，否则直接报错 + // 因为每个观测值会产生2个残差，所以至少需要6个点来计算公式12，所以要检验当前的点个数是否满足大于5的条件 + size_t numberCorrespondences = indices.size(); + // 当numberCorrespondences不满足>5的条件时会发生错误(如果小于6根本进不来) + assert(numberCorrespondences > 5); + + // ? 用来标记是否满足平面条件，（平面情况下矩阵有相关性，秩为2，矩阵形式可以简化，但需要跟多的约束求解） + bool planar = false; + // compute the nullspace of all vectors + + // compute the nullspace of all vectors + // step 2: 计算点的单位（方向向量）向量的零空间 + // 利用公式7 Jvr(v) = null(v^T) = [r s] + + // 给每个向量都开辟一个零空间，所以数量相等 + std::vector nullspaces(numberCorrespondences); + + // 存储世界坐标系下空间点的矩阵，3行N列，N是numberCorrespondences，即点的总个数 + // |x1, x2, xn| + // points3 = |y1, y2, ..., yn| + // |z1, z2, zn| + Eigen::MatrixXd points3(3, numberCorrespondences); + + // 空间点向量 + // |xi| + // points3v = |yi| + // |zi| + points_t points3v(numberCorrespondences); + + // 单个空间点的齐次坐标矩阵，TODO:没用到啊 + // |xi| + // points4v = |yi| + // |zi| + // |1 | + points4_t points4v(numberCorrespondences); + + // numberCorrespondences不等于所有点，而是提取出来的内点的数量，其作为连续索引值对indices进行索引 + // 因为内点的索引并非连续，想要方便遍历，必须用连续的索引值， + // 所以就用了indices[i]嵌套形式，i表示内点数量numberCorrespondences范围内的连续形式 + // indices里面保存的是不连续的内点的索引值 + for (size_t i = 0; i < numberCorrespondences; i++) + { + // 当前空间点的单位向量,indices[i]是当前点坐标和向量的索引值， + bearingVector_t f_current = f[indices[i]]; + + // 取出当前点记录到 points3 空间点矩阵里 + points3.col(i) = p[indices[i]]; + + // nullspace of right vector + // 求解方程 Jvr(v) = null(v^T) = [r s] + // A = U * S * V^T + // 这里只求解了V的完全解，没有求解U + Eigen::JacobiSVD + svd_f(f_current.transpose(), Eigen::ComputeFullV); + + // 取特征值最小的那两个对应的2个特征向量 + // |r1 s1| + // nullspaces = |r2 s2| + // |r3 s3| + nullspaces[i] = svd_f.matrixV().block(0, 1, 3, 2); + // 取出当前点记录到 points3v 空间点向量 + points3v[i] = p[indices[i]]; + } + // Step 3: 通过计算S的秩来判断是在平面情况还是在正常情况 + // 令S = M * M^T，其中M = [p1,p2,...,pi]，即 points3 空间点矩阵 + ////////////////////////////////////// + // 1. test if we have a planar scene + // 在平面条件下，会产生4个解，因此需要另外判断和解决平面条件下的问题 + ////////////////////////////////////// + + // 令S = M * M^T，其中M = [p1,p2,...,pi]，即 points3 空间点矩阵 + // 如果矩阵S的秩为3且最小特征值不接近于0，则不属于平面条件，否则需要解决一下 + Eigen::Matrix3d planarTest = points3 * points3.transpose(); + + // 为了计算矩阵S的秩，需要对矩阵S进行满秩的QR分解，通过其结果来判断矩阵S的秩，从而判断是否是平面条件 + Eigen::FullPivHouseholderQR rankTest(planarTest); + // int r, c; + // double minEigenVal = abs(eigen_solver.eigenvalues().real().minCoeff(&r, &c)); + // 特征旋转矩阵，用在平面条件下的计算 + Eigen::Matrix3d eigenRot; + eigenRot.setIdentity(); + + // if yes -> transform points to new eigen frame + // if (minEigenVal < 1e-3 || minEigenVal == 0.0) + // rankTest.setThreshold(1e-10); + // 当矩阵S的秩为2时，属于平面条件， + if (rankTest.rank() == 2) + { + planar = true; + // self adjoint is faster and more accurate than general eigen solvers + // also has closed form solution for 3x3 self-adjoint matrices + // in addition this solver sorts the eigenvalues in increasing order + // 计算矩阵S的特征值和特征向量 + Eigen::SelfAdjointEigenSolver eigen_solver(planarTest); + + // 得到QR分解的结果 + eigenRot = eigen_solver.eigenvectors().real(); + + // 把eigenRot变成其转置矩阵，即论文公式20的系数 R_S^T + eigenRot.transposeInPlace(); + + // 公式20： pi' = R_S^T * pi + for (size_t i = 0; i < numberCorrespondences; i++) + points3.col(i) = eigenRot * points3.col(i); + } + ////////////////////////////////////// + // 2. stochastic model + ////////////////////////////////////// + // Step 4: 计算随机模型中的协方差矩阵 + // 但是作者并没有用到协方差信息 + Eigen::SparseMatrix P(2 * numberCorrespondences, + 2 * numberCorrespondences); + bool use_cov = false; + P.setIdentity(); // standard + + // if we do have covariance information + // -> fill covariance matrix + // 如果协方差矩阵的个数等于空间点的个数，说明前面已经计算好了，表示有协方差信息 + // 目前版本是没有用到协方差信息的，所以调用本函数前就把协方差矩阵个数置为1了 + if (covMats.size() == numberCorrespondences) + { + use_cov = true; + int l = 0; + for (size_t i = 0; i < numberCorrespondences; ++i) + { + // invert matrix + cov2_mat_t temp = nullspaces[i].transpose() * covMats[i] * nullspaces[i]; + temp = temp.inverse().eval(); + P.coeffRef(l, l) = temp(0, 0); + P.coeffRef(l, l + 1) = temp(0, 1); + P.coeffRef(l + 1, l) = temp(1, 0); + P.coeffRef(l + 1, l + 1) = temp(1, 1); + l += 2; + } + } + + // Step 5: 构造矩阵A来完成线性方程组的构建 + ////////////////////////////////////// + // 3. fill the design matrix A + ////////////////////////////////////// + // 公式12，设矩阵A，则有 Au = 0 + // u = [r11, r12, r13, r21, r22, r23, r31, r32, r33, t1, t2, t3]^T + // 对单位向量v的2个零空间向量做微分，所以有[dr, ds]^T，一个点有2行，N个点就有2*N行 + const int rowsA = 2 * numberCorrespondences; + + // 对应上面u的列数，因为旋转矩阵有9个元素，加上平移矩阵3个元素，总共12个元素 + int colsA = 12; + Eigen::MatrixXd A; + + // 如果世界点位于分别跨2个坐标轴的平面上，即所有世界点的一个元素是常数的时候，可简单地忽略矩阵A中对应的列 + // 而且这不影响问题的结构本身，所以在计算公式20： pi' = R_S^T * pi的时候，忽略了r11,r21,r31，即第一列 + // 对应的u只有9个元素 u = [r12, r13, r22, r23, r32, r33, t1, t2, t3]^T 所以A的列个数是9个 + if (planar) + { + colsA = 9; + A = Eigen::MatrixXd(rowsA, 9); + } + else + A = Eigen::MatrixXd(rowsA, 12); + A.setZero(); + + // fill design matrix + // 构造矩阵A，分平面和非平面2种情况 + if (planar) + { + for (size_t i = 0; i < numberCorrespondences; ++i) + { + // 列表示当前点的坐标 + point_t pt3_current = points3.col(i); + + // r12 r12 的系数 r1*py 和 s1*py + A(2 * i, 0) = nullspaces[i](0, 0) * pt3_current[1]; + A(2 * i + 1, 0) = nullspaces[i](0, 1) * pt3_current[1]; + // r13 r13 的系数 r1*pz 和 s1*pz + A(2 * i, 1) = nullspaces[i](0, 0) * pt3_current[2]; + A(2 * i + 1, 1) = nullspaces[i](0, 1) * pt3_current[2]; + // r22 r22 的系数 r2*py 和 s2*py + A(2 * i, 2) = nullspaces[i](1, 0) * pt3_current[1]; + A(2 * i + 1, 2) = nullspaces[i](1, 1) * pt3_current[1]; + // r23 r23 的系数 r2*pz 和 s2*pz + A(2 * i, 3) = nullspaces[i](1, 0) * pt3_current[2]; + A(2 * i + 1, 3) = nullspaces[i](1, 1) * pt3_current[2]; + // r32 r32 的系数 r3*py 和 s3*py + A(2 * i, 4) = nullspaces[i](2, 0) * pt3_current[1]; + A(2 * i + 1, 4) = nullspaces[i](2, 1) * pt3_current[1]; + // r33 r33 的系数 r3*pz 和 s3*pz + A(2 * i, 5) = nullspaces[i](2, 0) * pt3_current[2]; + A(2 * i + 1, 5) = nullspaces[i](2, 1) * pt3_current[2]; + // t1 t1 的系数 r1 和 s1 + A(2 * i, 6) = nullspaces[i](0, 0); + A(2 * i + 1, 6) = nullspaces[i](0, 1); + // t2 t2 的系数 r2 和 s2 + A(2 * i, 7) = nullspaces[i](1, 0); + A(2 * i + 1, 7) = nullspaces[i](1, 1); + // t3 t3 的系数 r3 和 s3 + A(2 * i, 8) = nullspaces[i](2, 0); + A(2 * i + 1, 8) = nullspaces[i](2, 1); + } + } + else + { + for (size_t i = 0; i < numberCorrespondences; ++i) + { + point_t pt3_current = points3.col(i); + + // 不是平面的情况下，三个列向量都保留求解 + // r11 + A(2 * i, 0) = nullspaces[i](0, 0) * pt3_current[0]; + A(2 * i + 1, 0) = nullspaces[i](0, 1) * pt3_current[0]; + // r12 + A(2 * i, 1) = nullspaces[i](0, 0) * pt3_current[1]; + A(2 * i + 1, 1) = nullspaces[i](0, 1) * pt3_current[1]; + // r13 + A(2 * i, 2) = nullspaces[i](0, 0) * pt3_current[2]; + A(2 * i + 1, 2) = nullspaces[i](0, 1) * pt3_current[2]; + // r21 + A(2 * i, 3) = nullspaces[i](1, 0) * pt3_current[0]; + A(2 * i + 1, 3) = nullspaces[i](1, 1) * pt3_current[0]; + // r22 + A(2 * i, 4) = nullspaces[i](1, 0) * pt3_current[1]; + A(2 * i + 1, 4) = nullspaces[i](1, 1) * pt3_current[1]; + // r23 + A(2 * i, 5) = nullspaces[i](1, 0) * pt3_current[2]; + A(2 * i + 1, 5) = nullspaces[i](1, 1) * pt3_current[2]; + // r31 + A(2 * i, 6) = nullspaces[i](2, 0) * pt3_current[0]; + A(2 * i + 1, 6) = nullspaces[i](2, 1) * pt3_current[0]; + // r32 + A(2 * i, 7) = nullspaces[i](2, 0) * pt3_current[1]; + A(2 * i + 1, 7) = nullspaces[i](2, 1) * pt3_current[1]; + // r33 + A(2 * i, 8) = nullspaces[i](2, 0) * pt3_current[2]; + A(2 * i + 1, 8) = nullspaces[i](2, 1) * pt3_current[2]; + // t1 + A(2 * i, 9) = nullspaces[i](0, 0); + A(2 * i + 1, 9) = nullspaces[i](0, 1); + // t2 + A(2 * i, 10) = nullspaces[i](1, 0); + A(2 * i + 1, 10) = nullspaces[i](1, 1); + // t3 + A(2 * i, 11) = nullspaces[i](2, 0); + A(2 * i + 1, 11) = nullspaces[i](2, 1); + } + } + + // Step 6: 计算线性方程组的最小二乘解 + ////////////////////////////////////// + // 4. solve least squares + ////////////////////////////////////// + // 求解方程的最小二乘解 + Eigen::MatrixXd AtPA; + if (use_cov) + // 有协方差信息的情况下，一般方程是 A^T*P*A*u = N*u = 0 + AtPA = A.transpose() * P * A; // setting up the full normal equations seems to be unstable + else + // 无协方差信息的情况下，一般方程是 A^T*A*u = N*u = 0 + AtPA = A.transpose() * A; + + // SVD分解，满秩求解V + Eigen::JacobiSVD svd_A(AtPA, Eigen::ComputeFullV); + + // 解就是对应奇异值最小的列向量，即最后一列 + Eigen::MatrixXd result1 = svd_A.matrixV().col(colsA - 1); + + // Step 7: 根据平面和非平面情况下选择最终位姿解 + //////////////////////////////// + // now we treat the results differently, + // depending on the scene (planar or not) + //////////////////////////////// + // transformation_t T_final; + rotation_t Rout; + translation_t tout; + if (planar) // planar case + { + rotation_t tmp; + // until now, we only estimated + // row one and two of the transposed rotation matrix + // 暂时只估计了旋转矩阵的第1行和第2行，先记录到tmp中 + tmp << 0.0, result1(0, 0), result1(1, 0), + 0.0, result1(2, 0), result1(3, 0), + 0.0, result1(4, 0), result1(5, 0); + // double scale = 1 / sqrt(tmp.col(1).norm() * tmp.col(2).norm()); + // row 3 + // 第3行等于第1行和第2行的叉积（这里应该是列，后面转置后成了行） + tmp.col(0) = tmp.col(1).cross(tmp.col(2)); + + // 原来是: + // |r11 r12 r13| + // tmp = |r21 r22 r23| + // |r31 r32 r33| + // 转置变成: + // |r11 r21 r31| + // tmp = |r12 r22 r32| + // |r13 r23 r33| + tmp.transposeInPlace(); + + // 平移部分 t 只表示了正确的方向，没有尺度，需要求解 scale, 先求系数 + double scale = 1.0 / std::sqrt(std::abs(tmp.col(1).norm() * tmp.col(2).norm())); + // find best rotation matrix in frobenius sense + // 利用Frobenious范数计算最佳的旋转矩阵，利用公式(19)， R = U_R*V_R^T + // 本质上，采用矩阵，将其元素平方，将它们加在一起并对结果平方根。计算得出的数字是矩阵的Frobenious范数 + // 由于列向量是单列矩阵，行向量是单行矩阵，所以这些矩阵的Frobenius范数等于向量的长度（L） + Eigen::JacobiSVD svd_R_frob(tmp, Eigen::ComputeFullU | Eigen::ComputeFullV); + + rotation_t Rout1 = svd_R_frob.matrixU() * svd_R_frob.matrixV().transpose(); + + // test if we found a good rotation matrix + // 如果估计出来的旋转矩阵的行列式小于0，则乘以-1（EPnP也是同样的操作） + if (Rout1.determinant() < 0) + Rout1 *= -1.0; + + // rotate this matrix back using the eigen frame + // 因为是在平面情况下计算的，估计出来的旋转矩阵是要做一个转换的，根据公式(21)，R = Rs*R + // 其中，Rs表示特征向量的旋转矩阵 + // 注意eigenRot之前已经转置过了transposeInPlace()，所以这里Rout1在之前也转置了，即tmp.transposeInPlace() + Rout1 = eigenRot.transpose() * Rout1; + + // 估计最终的平移矩阵，带尺度信息的，根据公式(17)，t = t^ / three-party(||r1||*||r2||*||r3||) + // 这里是 t = t^ / sqrt(||r1||*||r2||) + translation_t t = scale * translation_t(result1(6, 0), result1(7, 0), result1(8, 0)); + + // 把之前转置过来的矩阵再转回去，变成公式里面的形态: + // |r11 r12 r13| + // Rout1 = |r21 r22 r23| + // |r31 r32 r33| + Rout1.transposeInPlace(); + + // 这里乘以-1是为了计算4种结果 + Rout1 *= -1; + if (Rout1.determinant() < 0.0) + Rout1.col(2) *= -1; + // now we have to find the best out of 4 combinations + // |r11 r12 r13| + // R1 = |r21 r22 r23| + // |r31 r32 r33| + // |-r11 -r12 -r13| + // R2 = |-r21 -r22 -r23| + // |-r31 -r32 -r33| + rotation_t R1, R2; + R1.col(0) = Rout1.col(0); + R1.col(1) = Rout1.col(1); + R1.col(2) = Rout1.col(2); + R2.col(0) = -Rout1.col(0); + R2.col(1) = -Rout1.col(1); + R2.col(2) = Rout1.col(2); + + // |R1 t| + // Ts = |R1 -t| + // |R2 t| + // |R2 -t| + vector> Ts(4); + Ts[0].block<3, 3>(0, 0) = R1; + Ts[0].block<3, 1>(0, 3) = t; + Ts[1].block<3, 3>(0, 0) = R1; + Ts[1].block<3, 1>(0, 3) = -t; + Ts[2].block<3, 3>(0, 0) = R2; + Ts[2].block<3, 1>(0, 3) = t; + Ts[3].block<3, 3>(0, 0) = R2; + Ts[3].block<3, 1>(0, 3) = -t; + + // 遍历4种解 + vector normVal(4); + for (int i = 0; i < 4; ++i) + { + point_t reproPt; + double norms = 0.0; + // 计算世界点p到切线空间v的投影的残差，对应最小的就是最好的解 + // 用前6个点来验证4种解的残差 + for (int p = 0; p < 6; ++p) + { + // 重投影的向量 + reproPt = Ts[i].block<3, 3>(0, 0) * points3v[p] + Ts[i].block<3, 1>(0, 3); + // 变成单位向量 + reproPt = reproPt / reproPt.norm(); + // f[indices[p]] 是当前空间点的单位向量 + // 利用欧氏距离来表示重投影向量(观测)和当前空间点向量(实际)的偏差 + // 即两个n维向量a(x11,x12,…,x1n)与 b(x21,x22,…,x2n)间的欧氏距离 + norms += (1.0 - reproPt.transpose() * f[indices[p]]); + } + // 统计每种解的误差和，第i个解的误差和放入对应的变量normVal[i] + normVal[i] = norms; + } + + // 搜索容器中的最小值，并返回该值对应的指针 + std::vector::iterator + findMinRepro = std::min_element(std::begin(normVal), std::end(normVal)); + + // 计算容器头指针到最小值指针的距离，即可作为该最小值的索引值 + int idx = std::distance(std::begin(normVal), findMinRepro); + + // 得到最终相机位姿估计的结果 + Rout = Ts[idx].block<3, 3>(0, 0); + tout = Ts[idx].block<3, 1>(0, 3); + } + else // non-planar + { + rotation_t tmp; + // 从AtPA的SVD分解中得到旋转矩阵，先存下来 + // 注意这里的顺序是和公式16不同的 + // |r11 r21 r31| + // tmp = |r12 r22 r32| + // |r13 r23 r33| + tmp << result1(0, 0), result1(3, 0), result1(6, 0), + result1(1, 0), result1(4, 0), result1(7, 0), + result1(2, 0), result1(5, 0), result1(8, 0); + // get the scale + // 计算尺度，根据公式(17)，t = t^ / three-party(||r1||*||r2||*||r3||) + double scale = 1.0 / + std::pow(std::abs(tmp.col(0).norm() * tmp.col(1).norm() * tmp.col(2).norm()), 1.0 / 3.0); + + // double scale = 1.0 / std::sqrt(std::abs(tmp.col(0).norm() * tmp.col(1).norm())); + // find best rotation matrix in frobenius sense + // 利用Frobenious范数计算最佳的旋转矩阵，利用公式(19)， R = U_R*V_R^T + Eigen::JacobiSVD svd_R_frob(tmp, Eigen::ComputeFullU | Eigen::ComputeFullV); + Rout = svd_R_frob.matrixU() * svd_R_frob.matrixV().transpose(); + + // test if we found a good rotation matrix + // 如果估计出来的旋转矩阵的行列式小于0，则乘以-1 + if (Rout.determinant() < 0) + Rout *= -1.0; + + // scale translation + // 从相机坐标系到世界坐标系的转换关系是 lambda*v = R*pi+t + // 从世界坐标系到相机坐标系的转换关系是 pi = R^T*v-R^Tt + // 旋转矩阵的性质 R^-1 = R^T + // 所以，在下面的计算中，需要计算从世界坐标系到相机坐标系的转换，这里tout = -R^T*t，下面再计算前半部分R^T*v + // 先恢复平移部分的尺度再计算 + tout = Rout * (scale * translation_t(result1(9, 0), result1(10, 0), result1(11, 0))); + + // find correct direction in terms of reprojection error, just take the first 6 correspondences + // 非平面情况下，一共有2种解，R,t和R,-t + // 利用前6个点计算重投影误差，选择残差最小的一个解 + vector error(2); + vector> Ts(2); + for (int s = 0; s < 2; ++s) + { + // 初始化error的值为0 + error[s] = 0.0; + + // |1 0 0 0| + // Ts[s] = |0 1 0 0| + // |0 0 1 0| + // |0 0 0 1| + Ts[s] = Eigen::Matrix4d::Identity(); + + // |. . . 0| + // Ts[s] = |. Rout . 0| + // |. . . 0| + // |0 0 0 1| + Ts[s].block<3, 3>(0, 0) = Rout; + if (s == 0) + // |. . . . | + // Ts[s] = |. Rout . tout| + // |. . . . | + // |0 0 0 1 | + Ts[s].block<3, 1>(0, 3) = tout; + else + // |. . . . | + // Ts[s] = |. Rout . -tout| + // |. . . . | + // |0 0 0 1 | + Ts[s].block<3, 1>(0, 3) = -tout; + + // 为了避免Eigen中aliasing的问题，后面在计算矩阵的逆的时候，需要添加eval()条件 + // a = a.transpose(); //error: aliasing + // a = a.transpose().eval(); //ok + // a.transposeInPlace(); //ok + // Eigen中aliasing指的是在赋值表达式的左右两边存在矩阵的重叠区域，这种情况下，有可能得到非预期的结果。 + // 如mat = 2*mat或者mat = mat.transpose()，第一个例子中的alias是没有问题的，而第二的例子则会导致非预期的计算结果。 + Ts[s] = Ts[s].inverse().eval(); + for (int p = 0; p < 6; ++p) + { + // 从世界坐标系到相机坐标系的转换关系是 pi = R^T*v-R^Tt + // Ts[s].block<3, 3>(0, 0) * points3v[p] = Rout = R^T*v + // Ts[s].block<3, 1>(0, 3) = tout = -R^Tt + bearingVector_t v = Ts[s].block<3, 3>(0, 0) * points3v[p] + Ts[s].block<3, 1>(0, 3); + // 变成单位向量 + v = v / v.norm(); + // 计算重投影向量(观测)和当前空间点向量(实际)的偏差 + error[s] += (1.0 - v.transpose() * f[indices[p]]); + } + } + // 选择残差最小的解作为最终解 + if (error[0] < error[1]) + tout = Ts[0].block<3, 1>(0, 3); + else + tout = Ts[1].block<3, 1>(0, 3); + Rout = Ts[0].block<3, 3>(0, 0); + } + + // Step 8: 利用高斯牛顿法对位姿进行精确求解，提高位姿解的精度 + ////////////////////////////////////// + // 5. gauss newton + ////////////////////////////////////// + // 求解非线性方程之前，需要得到罗德里格斯参数，来表达李群(SO3) -> 李代数(so3)的对数映射 + rodrigues_t omega = rot2rodrigues(Rout); + // |r1| + // |r2| + // minx = |r3| + // |t1| + // |t2| + // |t3| + Eigen::VectorXd minx(6); + minx[0] = omega[0]; + minx[1] = omega[1]; + minx[2] = omega[2]; + minx[3] = tout[0]; + minx[4] = tout[1]; + minx[5] = tout[2]; + + // 利用高斯牛顿迭代法来提炼相机位姿 pose + mlpnp_gn(minx, points3v, nullspaces, P, use_cov); + + // 最终输出的结果 + Rout = rodrigues2rot(rodrigues_t(minx[0], minx[1], minx[2])); + tout = translation_t(minx[3], minx[4], minx[5]); + + // result inverse as opengv uses this convention + // 这里是用来计算世界坐标系到相机坐标系的转换，所以是Pc=R^T*Pw-R^T*t，反变换 + result.block<3, 3>(0, 0) = Rout; // Rout.transpose(); + result.block<3, 1>(0, 3) = tout; //-result.block<3, 3>(0, 0) * tout; +} + +/** + * @brief 旋转向量转换成旋转矩阵，即李群->李代数 + * @param[in] omega 旋转向量 + * @param[out] R 旋转矩阵 + */ +Eigen::Matrix3d MLPnPsolver::rodrigues2rot(const Eigen::Vector3d &omega) +{ + // 初始化旋转矩阵 + rotation_t R = Eigen::Matrix3d::Identity(); + + // 求旋转向量的反对称矩阵 + Eigen::Matrix3d skewW; + skewW << 0.0, -omega(2), omega(1), + omega(2), 0.0, -omega(0), + -omega(1), omega(0), 0.0; + + // 求旋转向量的角度 + double omega_norm = omega.norm(); + + // 通过罗德里格斯公式把旋转向量转换成旋转矩阵 + if (omega_norm > std::numeric_limits::epsilon()) + R = R + sin(omega_norm) / omega_norm * skewW + (1 - cos(omega_norm)) / (omega_norm * omega_norm) * (skewW * skewW); + + return R; +} + +/** + * @brief 旋转矩阵转换成旋转向量，即李代数->李群 + * @param[in] R 旋转矩阵 + * @param[out] omega 旋转向量 + 问题: 李群(SO3) -> 李代数(so3)的对数映射 + 利用罗德里格斯公式，已知旋转矩阵的情况下，求解其对数映射ln(R) + 就像《视觉十四讲》里面说到的，使用李代数的一大动机是为了进行优化,而在优化过程中导数是非常必要的信息 + 李群SO(3)中完成两个矩阵乘法，不能用李代数so(3)中的加法表示，问题描述为两个李代数对数映射乘积 + 而两个李代数对数映射乘积的完整形式，可以用BCH公式表达，其中式子(左乘BCH近似雅可比J)可近似表达，该式也就是罗德里格斯公式 + 计算完罗德里格斯参数之后，就可以用非线性优化方法了，里面就要用到导数，其形式就是李代数指数映射 + 所以要在调用非线性MLPnP算法之前计算罗德里格斯参数 + */ +Eigen::Vector3d MLPnPsolver::rot2rodrigues(const Eigen::Matrix3d &R) +{ + rodrigues_t omega; + omega << 0.0, 0.0, 0.0; + + // R.trace() 矩阵的迹，即该矩阵的特征值总和 + double trace = R.trace() - 1.0; + + // 李群(SO3) -> 李代数(so3)的对数映射 + // 对数映射ln(R)∨将一个旋转矩阵转换为一个李代数，但由于对数的泰勒展开不是很优雅所以用本式 + // wnorm是求解出来的角度 + double wnorm = acos(trace / 2.0); + + // 如果wnorm大于运行编译程序的计算机所能识别的最小非零浮点数，则可以生成向量，否则为0 + if (wnorm > std::numeric_limits::epsilon()) + { + // |r11 r21 r31| + // R = |r12 r22 r32| + // |r13 r23 r33| + omega[0] = (R(2, 1) - R(1, 2)); + omega[1] = (R(0, 2) - R(2, 0)); + omega[2] = (R(1, 0) - R(0, 1)); + // theta |r23 - r32| + // ln(R) = ------------ * |r31 - r13| + // 2*sin(theta) |r12 - r21| + double sc = wnorm / (2.0 * sin(wnorm)); + omega *= sc; + } + return omega; +} + +/** + * @brief MLPnP的高斯牛顿解法 + * @param[in] x 未知量矩阵 + * @param[in] pts 3D点矩阵 + * @param[in] nullspaces 零空间向量 + * @param[in] Kll 协方差矩阵 + * @param[in] use_cov 协方差方法使用标记位 + */ +void MLPnPsolver::mlpnp_gn(Eigen::VectorXd &x, const points_t &pts, const std::vector &nullspaces, + const Eigen::SparseMatrix Kll, bool use_cov) +{ + // 计算观测值数量 + const int numObservations = pts.size(); + + // 未知量是旋转向量和平移向量，即R和t，总共6个未知参数 + const int numUnknowns = 6; + + // check redundancy + // 检查观测数量是否满足计算条件，因为每个观测值都提供了2个约束，即r和s，所以这里乘以2 + assert((2 * numObservations - numUnknowns) > 0); + + // ============= + // set all matrices up + // ============= + + Eigen::VectorXd r(2 * numObservations); + Eigen::VectorXd rd(2 * numObservations); + Eigen::MatrixXd Jac(2 * numObservations, numUnknowns); + Eigen::VectorXd g(numUnknowns, 1); + Eigen::VectorXd dx(numUnknowns, 1); // result vector + + Jac.setZero(); + r.setZero(); + dx.setZero(); + g.setZero(); + + int it_cnt = 0; + bool stop = false; + const int maxIt = 5; + double epsP = 1e-5; + + Eigen::MatrixXd JacTSKll; + Eigen::MatrixXd A; + // solve simple gradient descent + while (it_cnt < maxIt && !stop) + { + mlpnp_residuals_and_jacs(x, pts, + nullspaces, + r, Jac, true); + + if (use_cov) + JacTSKll = Jac.transpose() * Kll; + else + JacTSKll = Jac.transpose(); + + A = JacTSKll * Jac; + + // get system matrix + g = JacTSKll * r; + + // solve + Eigen::LDLT chol(A); + dx = chol.solve(g); + // dx = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(g); + // this is to prevent the solution from falling into a wrong minimum + // if the linear estimate is spurious + if (dx.array().abs().maxCoeff() > 5.0 || dx.array().abs().minCoeff() > 1.0) + break; + // observation update + Eigen::MatrixXd dl = Jac * dx; + if (dl.array().abs().maxCoeff() < epsP) + { + stop = true; + x = x - dx; + break; + } + else + x = x - dx; + + ++it_cnt; + } // while + // result +} + +/** + * @brief 计算Jacobian矩阵和残差 + * @param[in] x 未知量矩阵 + * @param[in] pts 3D点矩阵 + * @param[in] nullspaces 零空间向量 + * @param[in] r f(x)函数 + * @param[out] fjac f(x)函数的Jacobian矩阵 + * @param[in] getJacs 是否可以得到Jacobian矩阵标记位 + */ +void MLPnPsolver::mlpnp_residuals_and_jacs(const Eigen::VectorXd &x, const points_t &pts, + const std::vector &nullspaces, Eigen::VectorXd &r, + Eigen::MatrixXd &fjac, bool getJacs) +{ + rodrigues_t w(x[0], x[1], x[2]); + translation_t T(x[3], x[4], x[5]); + + // rotation_t R = math::cayley2rot(c); + rotation_t R = rodrigues2rot(w); + int ii = 0; + + Eigen::MatrixXd jacs(2, 6); + + for (int i = 0; i < pts.size(); ++i) + { + Eigen::Vector3d ptCam = R * pts[i] + T; + ptCam /= ptCam.norm(); + + r[ii] = nullspaces[i].col(0).transpose() * ptCam; + r[ii + 1] = nullspaces[i].col(1).transpose() * ptCam; + if (getJacs) + { + // jacs + mlpnpJacs(pts[i], + nullspaces[i].col(0), nullspaces[i].col(1), + w, T, + jacs); + + // r + fjac(ii, 0) = jacs(0, 0); + fjac(ii, 1) = jacs(0, 1); + fjac(ii, 2) = jacs(0, 2); + + fjac(ii, 3) = jacs(0, 3); + fjac(ii, 4) = jacs(0, 4); + fjac(ii, 5) = jacs(0, 5); + // s + fjac(ii + 1, 0) = jacs(1, 0); + fjac(ii + 1, 1) = jacs(1, 1); + fjac(ii + 1, 2) = jacs(1, 2); + + fjac(ii + 1, 3) = jacs(1, 3); + fjac(ii + 1, 4) = jacs(1, 4); + fjac(ii + 1, 5) = jacs(1, 5); + } + ii += 2; + } +} + +/** + * @brief 计算Jacobian矩阵 + * @param[in] pt 3D点矩阵 + * @param[in] nullspace_r 零空间向量r + * @param[in] nullspace_r 零空间向量s + * @param[in] w 旋转向量w + * @param[in] t 平移向量t + * @param[out] jacs Jacobian矩阵 + */ +void MLPnPsolver::mlpnpJacs(const point_t &pt, const Eigen::Vector3d &nullspace_r, + const Eigen::Vector3d &nullspace_s, const rodrigues_t &w, + const translation_t &t, Eigen::MatrixXd &jacs) +{ + double r1 = nullspace_r[0]; + double r2 = nullspace_r[1]; + double r3 = nullspace_r[2]; + + double s1 = nullspace_s[0]; + double s2 = nullspace_s[1]; + double s3 = nullspace_s[2]; + + double X1 = pt[0]; + double Y1 = pt[1]; + double Z1 = pt[2]; + + double w1 = w[0]; + double w2 = w[1]; + double w3 = w[2]; + + double t1 = t[0]; + double t2 = t[1]; + double t3 = t[2]; + + double t5 = w1 * w1; + double t6 = w2 * w2; + double t7 = w3 * w3; + + // t8 = theta^2 = w1^2+w2^2+w3^2 + double t8 = t5 + t6 + t7; + // t9 = sqrt(t8) = sqrt(theta^2) = theta + double t9 = sqrt(t8); + // t10 = sin(theta) + double t10 = sin(t9); + // t11 = 1 / theta + double t11 = 1.0 / sqrt(t8); + // t12 = cos(theta) + double t12 = cos(t9); + // t13 = cos(theta) - 1 + double t13 = t12 - 1.0; + // t14 = 1 / theta^2 + double t14 = 1.0 / t8; + // t16 = sin(theta)/theta*w3 + // 令 A = sin(theta)/theta = t10*t11 + // t16 = A*w3 + double t16 = t10 * t11 * w3; + // t17 = (cos(theta) - 1)/theta^2 * w1 * w2 + // 令 B = (cos(theta) - 1)/theta^2 = t13*t14 + // t17 = B*w1*w2 + double t17 = t13 * t14 * w1 * w2; + // t19 = sin(theta)/theta*w2 + // = A*w2 + double t19 = t10 * t11 * w2; + // t20 = (cos(theta) - 1) / theta^2 * w1 * w3 + // = B*w1*w3 + double t20 = t13 * t14 * w1 * w3; + // t24 = w2^2+w3^2 + double t24 = t6 + t7; + // t27 = A*w3 + B*w1*w2 + // = -r12 + double t27 = t16 + t17; + // t28 = (A*w3 + B*w1*w2)*Y1 + // = -r12*Y1 + double t28 = Y1 * t27; + // t29 = A*w2 - B*w1*w3 + // = r13 + double t29 = t19 - t20; + // t30 = (A*w2 - B*w1*w3) * Z1 + // = r13 * Z1 + double t30 = Z1 * t29; + // t31 = (cos(theta) - 1) /theta^2 * (w2^2+w3^2) + // = B * (w2^2+w3^2) + double t31 = t13 * t14 * t24; + // t32 = B * (w2^2+w3^2) + 1 + // = r11 + double t32 = t31 + 1.0; + // t33 = (B * (w2^2+w3^2) + 1) * X1 + // = r11 * X1 + double t33 = X1 * t32; + // t15 = t1 - (A*w3 + B*w1*w2)*Y1 + (A*w2 - B*w1*w3) * Z1 + (B * (w2^2+w3^2) + 1) * X1 + // = t1 + r11*X1 + r12*Y1 + r13*Z1 + double t15 = t1 - t28 + t30 + t33; + // t21 = t10 * t11 * w1 = sin(theta) / theta * w1 + // = A*w1 + double t21 = t10 * t11 * w1; + // t22 = t13 * t14 * w2 * w3 = (cos(theta) - 1) / theta^2 * w2 * w3 + // = B*w2*w3 + double t22 = t13 * t14 * w2 * w3; + // t45 = t5 + t7 + // = w1^2 + w3^2 + double t45 = t5 + t7; + // t53 = t16 - t17 + // = A*w3 - B*w1*w2 = r21 + double t53 = t16 - t17; + // t54 = r21*X1 + double t54 = X1 * t53; + // t55 = A*w1 + B*w2*w3 + // = -r23 + double t55 = t21 + t22; + // t56 = -r23*Z1 + double t56 = Z1 * t55; + // t57 = t13 * t14 * t45 = (cos(theta) - 1) / theta^2 * (w1^2 + w3^2) + // = B8(w1^2+w3^2) + double t57 = t13 * t14 * t45; + // t58 = B8(w1^2+w3^2) + 1.0 + // = r22 + double t58 = t57 + 1.0; + // t59 = r22*Y1 + double t59 = Y1 * t58; + // t18 = t2 + t54 - t56 + t59 + // = t2 + r21*X1 + r22*Y1 + r23*Z1 + double t18 = t2 + t54 - t56 + t59; + // t34 = t5 + t6 + // = w1^2+w2^2 + double t34 = t5 + t6; + // t38 = t19 + t20 = A*w2+B*w1*w3 + // = -r31 + double t38 = t19 + t20; + // t39 = -r31*X1 + double t39 = X1 * t38; + // t40 = A*w1 - B*w2*w3 + // = r32 + double t40 = t21 - t22; + // t41 = r32*Y1 + double t41 = Y1 * t40; + // t42 = t13 * t14 * t34 = (cos(theta) - 1) / theta^2 * (w1^2+w2^2) + // = B*(w1^2+w2^2) + double t42 = t13 * t14 * t34; + // t43 = B*(w1^2+w2^2) + 1 + // = r33 + double t43 = t42 + 1.0; + // t44 = r33*Z1 + double t44 = Z1 * t43; + // t23 = t3 - t39 + t41 + t44 = t3 + r31*X1 + r32*Y1 + r33*Z1 + double t23 = t3 - t39 + t41 + t44; + // t25 = 1.0 / pow(theta^2, 3.0 / 2.0) + // = 1 / theta^3 + double t25 = 1.0 / pow(t8, 3.0 / 2.0); + // t26 = 1.0 / (t8 * t8) + // = 1 / theta^4 + double t26 = 1.0 / (t8 * t8); + // t35 = t12 * t14 * w1 * w2 = cos(theta) / theta^2 * w1 * w2 + // 令 cos(theta) / theta^2 = E = t12*t14 + // t35 = E*w1*w2 + double t35 = t12 * t14 * w1 * w2; + // t36 = t5 * t10 * t25 * w3 = w1^2 * sin(theta) / theta^3 * w3 + // 令 sin(theta) / theta^3 = F = t10*t25 + // t36 = F*w1^2*w3 + double t36 = t5 * t10 * t25 * w3; + // t37 = t5 * t13 * t26 * w3 * 2.0 = w1^2 * (cos(theta) - 1) / theta^4 * w3 + // 令 (cos(theta) - 1) / theta^4 = G = t13*t26 + // t37 = G*w1^2*w3 + double t37 = t5 * t13 * t26 * w3 * 2.0; + // t46 = t10 * t25 * w1 * w3 = sin(theta) / theta^3 * w1 * w3 = F*w1*w3 + double t46 = t10 * t25 * w1 * w3; + // t47 = t5 * t10 * t25 * w2 = w1^2 * sin(theta) / theta^3 * w2 = F*w1^2*w2 + double t47 = t5 * t10 * t25 * w2; + // t48 = t5 * t13 * t26 * w2 * 2.0 = w1^2 * (cos(theta) - 1) / theta^4 * 2 * w2 = G*w1^2*w2 + double t48 = t5 * t13 * t26 * w2 * 2.0; + // t49 = t10 * t11 = sin(theta) / theta = A + double t49 = t10 * t11; + // t50 = t5 * t12 * t14 = w1^2 * cos(theta) / theta^2 = E*w1^2 + double t50 = t5 * t12 * t14; + // t51 = t13 * t26 * w1 * w2 * w3 * 2.0 = (cos(theta) - 1) / theta^4 * w1 * w2 * w3 * 2 = 2*G*w1*w2*w3 + double t51 = t13 * t26 * w1 * w2 * w3 * 2.0; + // t52 = t10 * t25 * w1 * w2 * w3 = sin(theta) / theta^3 * w1 * w2 * w3 = F*w1*w2*w3 + double t52 = t10 * t25 * w1 * w2 * w3; + // t60 = t15 * t15 = (t1 + r11*X1 + r12*Y1 + r13*Z1)^2 + double t60 = t15 * t15; + // t61 = t18 * t18 = (t2 + r21*X1 + r22*Y1 + r23*Z1)^2 + double t61 = t18 * t18; + // t62 = t23 * t23 = (t3 + r31*X1 + r32*Y1 + r33*Z1)^2 + double t62 = t23 * t23; + // t63 = t60 + t61 + t62 = (t1 + r11*X1 + r12*Y1 + r13*Z1)^2 + (t2 + r21*X1 + r22*Y1 + r23*Z1)^2 + (t3 + r31*X1 + r32*Y1 + r33*Z1)^2 + double t63 = t60 + t61 + t62; + // t64 = t5 * t10 * t25 = w1^2 * sin(theta) / theta^3 = F*w1^2 + double t64 = t5 * t10 * t25; + // t65 = 1 / sqrt((t1 + r11*X1 + r12*Y1 + r13*Z1)^2 + (t2 + r21*X1 + r22*Y1 + r23*Z1)^2 + (t3 + r31*X1 + r32*Y1 + r33*Z1)^2) + double t65 = 1.0 / sqrt(t63); + // t66 = Y1 * r2 * t6 = Y1 * r2 * w2^2 + double t66 = Y1 * r2 * t6; + // t67 = Z1 * r3 * t7 = Z1 * r3 * w3^2 + double t67 = Z1 * r3 * t7; + // t68 = r1 * t1 * t5 = r1 * t1 * w1^2 + double t68 = r1 * t1 * t5; + // t69 = r1 * t1 * t6 = r1 * t1 * w2^2 + double t69 = r1 * t1 * t6; + // t70 = r1 * t1 * t7 = r1 * t1 * w3^2 + double t70 = r1 * t1 * t7; + // t71 = r2 * t2 * t5 = r2 * t2 * w1^2 + double t71 = r2 * t2 * t5; + // t72 = r2 * t2 * t6 = r2 * t2 * w2^2 + double t72 = r2 * t2 * t6; + // t73 = r2 * t2 * t7 = r2 * t2 * w3^2 + double t73 = r2 * t2 * t7; + // t74 = r3 * t3 * t5 = r3 * t3 * w1^2 + double t74 = r3 * t3 * t5; + // t75 = r3 * t3 * t6 = r3 * t3 * w2^2 + double t75 = r3 * t3 * t6; + // t76 = r3 * t3 * t7 = r3 * t3 * w3^2 + double t76 = r3 * t3 * t7; + // t77 = X1 * r1 * t5 = X1 * r1 * w1^2 + double t77 = X1 * r1 * t5; + double t78 = X1 * r2 * w1 * w2; + double t79 = X1 * r3 * w1 * w3; + double t80 = Y1 * r1 * w1 * w2; + double t81 = Y1 * r3 * w2 * w3; + double t82 = Z1 * r1 * w1 * w3; + double t83 = Z1 * r2 * w2 * w3; + // t84 = X1 * r1 * t6 * t12 = X1 * r1 * w2^2 * cos(theta) + double t84 = X1 * r1 * t6 * t12; + // t85 = X1 * r1 * t7 * t12 = X1 * r1 * w3^2 * cos(theta) + double t85 = X1 * r1 * t7 * t12; + // t86 = Y1 * r2 * t5 * t12 = Y1 * r2 * w1^2 * cos(theta) + double t86 = Y1 * r2 * t5 * t12; + // t87 = Y1 * r2 * t7 * t12 = Y1 * r2 * w3^2 * cos(theta) + double t87 = Y1 * r2 * t7 * t12; + // t88 = Z1 * r3 * t5 * t12 = Z1 * r3 * w1^2 * cos(theta) + double t88 = Z1 * r3 * t5 * t12; + // t89 = Z1 * r3 * t6 * t12 = Z1 * r3 * w2^2 * cos(theta) + double t89 = Z1 * r3 * t6 * t12; + // t90 = X1 * r2 * t9 * t10 * w3 = X1 * r2 * theta * sin(theta) * w3 + double t90 = X1 * r2 * t9 * t10 * w3; + // t91 = Y1 * r3 * t9 * t10 * w1 = Y1 * r3 * theta * sin(theta) * w1 + double t91 = Y1 * r3 * t9 * t10 * w1; + // t92 = Z1 * r1 * t9 * t10 * w2 = Z1 * r1 * theta * sin(theta) * w2 + double t92 = Z1 * r1 * t9 * t10 * w2; + // t102 = X1 * r3 * t9 * t10 * w2 = X1 * r3 * theta * sin(theta) * w2 + double t102 = X1 * r3 * t9 * t10 * w2; + // t103 = Y1 * r1 * t9 * t10 * w3 = Y1 * r1 * theta * sin(theta) * w3 + double t103 = Y1 * r1 * t9 * t10 * w3; + // t104 = Z1 * r2 * t9 * t10 * w1 = Z1 * r2 * theta * sin(theta) * w1 + double t104 = Z1 * r2 * t9 * t10 * w1; + // t105 = X1 * r2 * t12 * w1 * w2 = X1 * r2 * cos(theta) * w1 * w2 + double t105 = X1 * r2 * t12 * w1 * w2; + // t106 = X1 * r3 * t12 * w1 * w3 = X1 * r3 * cos(theta) * w1 * w3 + double t106 = X1 * r3 * t12 * w1 * w3; + // t107 = Y1 * r1 * t12 * w1 * w2 = Y1 * r1 * cos(theta) * w1 * w2 + double t107 = Y1 * r1 * t12 * w1 * w2; + // t108 = Y1 * r3 * t12 * w2 * w3 = Y1 * r3 * cos(theta) * w2 * w3 + double t108 = Y1 * r3 * t12 * w2 * w3; + // t109 = Z1 * r1 * t12 * w1 * w3 = Z1 * r1 * cos(theta) * w1 * w3 + double t109 = Z1 * r1 * t12 * w1 * w3; + // t110 = Z1 * r2 * t12 * w2 * w3 = Z1 * r2 * cos(theta) * w2 * w3 + double t110 = Z1 * r2 * t12 * w2 * w3; + // t93 = t66 + t67 + t68 + t69 + // + t70 + t71 + t72 + t73 + t74 + // + t75 + t76 + t77 + t78 + t79 + // + t80 + t81 + t82 + t83 + t84 + // + t85 + t86 + t87 + t88 + t89 + // + t90 + t91 + t92 - t102 - t103 + // - t104 - t105 - t106 - t107 + // - t108 - t109 - t110 + // + // = (Y1 * r2 * w2^2) + (Z1 * r3 * w3^2) + (r1 * t1 * w1^2) + (r1 * t1 * w2^2) + // + (r1 * t1 * w3^2) + (r2 * t2 * w1^2) + (r2 * t2 * w2^2) + (r2 * t2 * w3^2) + (r3 * t3 * w1^2) + // + (r3 * t3 * w2^2) + (r3 * t3 * w3^2) + (X1 * r1 * w1^2) + (X1 * r2 * w1 * w2) + (X1 * r3 * w1 * w3) + // + (Y1 * r1 * w1 * w2) + (Y1 * r3 * w2 * w3) + (Z1 * r1 * w1 * w3) + (Z1 * r2 * w2 * w3) + (X1 * r1 * w2^2 * cos(theta)) + // + (X1 * r1 * w3^2 * cos(theta)) + (Y1 * r2 * w1^2 * cos(theta)) + (Y1 * r2 * w3^2 * cos(theta)) + (Z1 * r3 * w1^2 * cos(theta)) + (Z1 * r3 * w2^2 * cos(theta)) + // + (X1 * r2 * theta * sin(theta) * w3) + (Y1 * r3 * theta * sin(theta) * w1) + (Z1 * r1 * theta * sin(theta) * w2) - (X1 * r3 * theta * sin(theta) * w2) - (Y1 * r1 * theta * sin(theta) * w3) + // - (Z1 * r2 * theta * sin(theta) * w1) - (X1 * r2 * cos(theta) * w1 * w2) - (X1 * r3 * cos(theta) * w1 * w3) - (Y1 * r1 * cos(theta) * w1 * w2) - () + // - (Y1 * r3 * cos(theta) * w2 * w3) - (Z1 * r1 * cos(theta) * w1 * w3) - (Z1 * r2 * cos(theta) * w2 * w3) + double t93 = + t66 + t67 + t68 + t69 + t70 + t71 + t72 + t73 + t74 + t75 + t76 + t77 + t78 + t79 + t80 + t81 + t82 + + t83 + t84 + t85 + t86 + t87 + t88 + t89 + t90 + t91 + t92 - t102 - t103 - t104 - t105 - t106 - t107 - + t108 - t109 - t110; + // t94 = sin(theta)/theta^3* w1 * w2 = F*w1*w2 + double t94 = t10 * t25 * w1 * w2; + // t95 = w2^2*sin(theta)/theta^3*w3 = F*w2^2*w3 + double t95 = t6 * t10 * t25 * w3; + // t96 = 2 * w2^2 * (cos(theta) - 1) / theta^4 * w3 = 2*G*w2^2*w3 + double t96 = t6 * t13 * t26 * w3 * 2.0; + // t97 = cos(theta) / theta^2 * w2 * w3 = E*w2*w3 + double t97 = t12 * t14 * w2 * w3; + // t98 = w2^2 * sin(theta) / theta^3 * w1 = F*w2^2*w1 + double t98 = t6 * t10 * t25 * w1; + // t99 = 2 * w2^2 * (cos(theta) - 1) / theta^4 * w1 = 2*G*w2^2*w1 + double t99 = t6 * t13 * t26 * w1 * 2.0; + // t100 = w2^2 * sin(theta) / theta^3 = F*w2^2 + double t100 = t6 * t10 * t25; + // t101 = 1.0 / pow(sqrt((t1 + r11*X1 + r12*Y1 + r13*Z1)^2 + (t2 + r21*X1 + r22*Y1 + r23*Z1)^2 + (t3 + r31*X1 + r32*Y1 + r33*Z1)^2), 3.0 / 2.0) + // = (t1 + r11*X1 + r12*Y1 + r13*Z1)^2 + (t2 + r21*X1 + r22*Y1 + r23*Z1)^2 + (t3 + r31*X1 + r32*Y1 + r33*Z1)^3 + double t101 = 1.0 / pow(t63, 3.0 / 2.0); + // t111 = w2^2 * cos(theta) / theta^2 = E*w2^2 + double t111 = t6 * t12 * t14; + // t112 = sin(theta) / theta^3 *w2 * w3 = F*w2*w3 + double t112 = t10 * t25 * w2 * w3; + // t113 = cos(theta) / theta^2 * w1 * w3 = E*w1*w3 + double t113 = t12 * t14 * w1 * w3; + // t114 = w3^2 * sin(theta) / theta^3 * w2 = F*w3^2*w2 + double t114 = t7 * t10 * t25 * w2; + // t115 = t7 * t13 * t26 * w2 * 2.0 = 2*w3^2*(cos(theta) - 1) / theta^4*w2 + double t115 = t7 * t13 * t26 * w2 * 2.0; + // t116 = w3^2 * sin(theta) / theta^3 * w1 = F*w3^2*w1 + double t116 = t7 * t10 * t25 * w1; + // t117 = 2 * w3^2 * (cos(theta) - 1) / theta^4 * w1 = 2*G*w3^2*w1 + double t117 = t7 * t13 * t26 * w1 * 2.0; + // t118 = E*w3^2 + double t118 = t7 * t12 * t14; + // t119 = 2*w1*G*(w2^2+w3^2) + double t119 = t13 * t24 * t26 * w1 * 2.0; + // t120 = F*w1*(w2^2+w3^2) + double t120 = t10 * t24 * t25 * w1; + // t121 = (2*G+F)*w1*(w2^2+w3^2) + double t121 = t119 + t120; + // t122 = 2*G*w1*(w1^2+w2^2) + double t122 = t13 * t26 * t34 * w1 * 2.0; + // t123 = F*w1*(w1^2+w2^2) + double t123 = t10 * t25 * t34 * w1; + // t131 = 2*(cos(theta) - 1) / theta^2*w1 = 2*B*w1 + double t131 = t13 * t14 * w1 * 2.0; + // t124 = t122 + t123 - t131 = 2*G*w1*(w1^2+w2^2)+F*w1*(w1^2+w2^2)-2*B*w1 + // = (2*G+F)*w1*(w1^2+w2^2)-2*B*w1 + double t124 = t122 + t123 - t131; + // t139 = t13 * t14 * w3 = B*w3 + double t139 = t13 * t14 * w3; + // t125 = -t35 + t36 + t37 + t94 - t139 = -E*w1*w2+F*w1^2*w3+G*w1^2*w3+F*w1*w2-B*w3 + double t125 = -t35 + t36 + t37 + t94 - t139; + // t126 = (-E*w1*w2+F*w1^2*w3+G*w1^2*w3+F*w1*w2-B*w3)*X1 + double t126 = X1 * t125; + // t127 = t49 + t50 + t51 + t52 - t64 = A + E*w1^2 + (2*G+F)*w1*w2*w3 - F*w1^2 + double t127 = t49 + t50 + t51 + t52 - t64; + // t128 = (A + E*w1^2 + (2*G+F)*w1*w2*w3 - F*w1^2)*Y1 + double t128 = Y1 * t127; + // t129 = (-E*w1*w2+F*w1^2*w3+G*w1^2*w3+F*w1*w2-B*w3)*X1 + (A + E*w1^2 + (2*G+F)*w1*w2*w3 - F*w1^2)*Y1 - ((2*G+F)*w1*(w1^2+w2^2)-2*B*w1)Z1 + double t129 = t126 + t128 - Z1 * t124; + // t130 = 2 * (t3 + r31*X1 + r32*Y1 + r33*Z1) + // * ((-E*w1*w2+F*w1^2*w3+G*w1^2*w3+F*w1*w2-B*w3)*X1 + (A + E*w1^2 + (2*G+F)*w1*w2*w3 - F*w1^2)*Y1 - ((2*G+F)*w1*(w1^2+w2^2)-2*B*w1)Z1) + double t130 = t23 * t129 * 2.0; + // t132 = t13 * t26 * t45 * w1 * 2.0 = 2*w1*G*(w1^2+w3^2) + double t132 = t13 * t26 * t45 * w1 * 2.0; + // t133 = t10 * t25 * t45 * w1 = F*w1*(w1^2+w3^2) + double t133 = t10 * t25 * t45 * w1; + // t138 = t13 * t14 * w2 = B*w2 + double t138 = t13 * t14 * w2; + // t134 = -t46 + t47 + t48 + t113 - t138 = -F*w1*w3+F*w1^2*w2+G*w1^2*w2+E*w1*w3-B*w2 + double t134 = -t46 + t47 + t48 + t113 - t138; + // t135 = (-F*w1*w3+F*w1^2*w2+G*w1^2*w2+E*w1*w3-B*w2)*X1 + double t135 = X1 * t134; + // t136 = -t49 - t50 + t51 + t52 + t64 = -A-E*w1^2+2*G*w1*w2*w3+F*w1*w2*w3+F*w1^2 + double t136 = -t49 - t50 + t51 + t52 + t64; + // t137 = (-A-E*w1^2+2*G*w1*w2*w3+F*w1*w2*w3+F*w1^2)*Z1 + double t137 = Z1 * t136; + + double t140 = X1 * s1 * t5; + double t141 = Y1 * s2 * t6; + double t142 = Z1 * s3 * t7; + double t143 = s1 * t1 * t5; + double t144 = s1 * t1 * t6; + double t145 = s1 * t1 * t7; + double t146 = s2 * t2 * t5; + double t147 = s2 * t2 * t6; + double t148 = s2 * t2 * t7; + double t149 = s3 * t3 * t5; + double t150 = s3 * t3 * t6; + double t151 = s3 * t3 * t7; + double t152 = X1 * s2 * w1 * w2; + double t153 = X1 * s3 * w1 * w3; + double t154 = Y1 * s1 * w1 * w2; + double t155 = Y1 * s3 * w2 * w3; + double t156 = Z1 * s1 * w1 * w3; + double t157 = Z1 * s2 * w2 * w3; + // t12 = cos(theta) + double t158 = X1 * s1 * t6 * t12; + double t159 = X1 * s1 * t7 * t12; + double t160 = Y1 * s2 * t5 * t12; + double t161 = Y1 * s2 * t7 * t12; + double t162 = Z1 * s3 * t5 * t12; + double t163 = Z1 * s3 * t6 * t12; + // t9 = theta, t10 = sin(theta) + double t164 = X1 * s2 * t9 * t10 * w3; + double t165 = Y1 * s3 * t9 * t10 * w1; + double t166 = Z1 * s1 * t9 * t10 * w2; + double t183 = X1 * s3 * t9 * t10 * w2; + double t184 = Y1 * s1 * t9 * t10 * w3; + double t185 = Z1 * s2 * t9 * t10 * w1; + // t12 = cos(theta) + double t186 = X1 * s2 * t12 * w1 * w2; + double t187 = X1 * s3 * t12 * w1 * w3; + double t188 = Y1 * s1 * t12 * w1 * w2; + double t189 = Y1 * s3 * t12 * w2 * w3; + double t190 = Z1 * s1 * t12 * w1 * w3; + double t191 = Z1 * s2 * t12 * w2 * w3; + // t167 = t140 + t141 + t142 + t143 + t144 + // + t145 + t146 + t147 + t148 + t149 + // + t150 + t151 + t152 + t153 + t154 + // + t155 + t156 + t157 + t158 + t159 + // + t160 + t161 + t162 + t163 + t164 + // + t165 + t166 - t183 - t184 - t185 + // - t186 - t187 - t188 + // - t189 - t190 - t191 + // + // = (X1 * s1 * t5) + (Y1 * s2 * t6) + (Z1 * s3 * t7) + (s1 * t1 * t5) + (s1 * t1 * t6) + // + (s1 * t1 * t7) + (s2 * t2 * t5) + (s2 * t2 * t6) + (s2 * t2 * t7) + (s3 * t3 * t5) + // + (s3 * t3 * t6) + (s3 * t3 * t7) + (X1 * s2 * w1 * w2) + (X1 * s3 * w1 * w3) + (Y1 * s1 * w1 * w2) + // + (Y1 * s3 * w2 * w3) + (Z1 * s1 * w1 * w3) + (Z1 * s2 * w2 * w3) + (X1 * s1 * t6 * t12) + (X1 * s1 * t7 * t12) + // + (Y1 * s2 * t5 * t12) + (Y1 * s2 * t7 * t12) + (Z1 * s3 * t5 * t12) + (Z1 * s3 * t6 * t12) + (X1 * s2 * t9 * t10 * w3) + // + (Y1 * s3 * t9 * t10 * w1) + (Z1 * s1 * t9 * t10 * w2) - (X1 * s3 * t9 * t10 * w2) - (Y1 * s1 * t9 * t10 * w3) - (Z1 * s2 * t9 * t10 * w1) + // - (X1 * s2 * t12 * w1 * w2) - (X1 * s3 * t12 * w1 * w3) - (Y1 * s1 * t12 * w1 * w2) + // - (Y1 * s3 * t12 * w2 * w3) - (Z1 * s1 * t12 * w1 * w3) - (Z1 * s2 * t12 * w2 * w3) + double t167 = + t140 + t141 + t142 + t143 + t144 + t145 + t146 + t147 + t148 + t149 + t150 + t151 + t152 + t153 + t154 + + t155 + t156 + t157 + t158 + t159 + t160 + t161 + t162 + t163 + t164 + t165 + t166 - t183 - t184 - t185 - + t186 - t187 - t188 - t189 - t190 - t191; + // t168 = t13 * t26 * t45 * w2 * 2.0 = 2*G*w2*(w1^2+w3^2) + double t168 = t13 * t26 * t45 * w2 * 2.0; + // t169 = t10 * t25 * t45 * w2 = F*w2*(w1^2+w3^2) + double t169 = t10 * t25 * t45 * w2; + // t170 = t168 + t169 = (2*G+F)*w2*(w1^2+w3^2) + double t170 = t168 + t169; + // t171 = t13 * t26 * t34 * w2 * 2.0 = 2*G*w2*(w1^2+w2^2) + double t171 = t13 * t26 * t34 * w2 * 2.0; + // t172 = t10 * t25 * t34 * w2 = F*w2*(w1^2+w2^2) + double t172 = t10 * t25 * t34 * w2; + // t176 = t13 * t14 * w2 * 2.0 = 2*B*w2 + double t176 = t13 * t14 * w2 * 2.0; + // t173 = t171 + t172 - t176 = (2*G+F)*w2*(w1^2+w2^2) - 2*B*w2 + double t173 = t171 + t172 - t176; + // t174 = -t49 + t51 + t52 + t100 - t111 = -A+2*G*w1*w2*w3+F*w1*w2*w3+F*w2^2-E*w2^2 + double t174 = -t49 + t51 + t52 + t100 - t111; + // t175 = X1 * t174 = (-A+2*G*w1*w2*w3+F*w1*w2*w3+F*w2^2-E*w2^2)*X1 + double t175 = X1 * t174; + // t177 = t13 * t24 * t26 * w2 * 2.0 = 2*w2*G*(w2^2+w3^2) + double t177 = t13 * t24 * t26 * w2 * 2.0; + // t178 = t10 * t24 * t25 * w2 = F*w2*(w2^2+w3^2) + double t178 = t10 * t24 * t25 * w2; + // t192 = t13 * t14 * w1 = B*w1 + double t192 = t13 * t14 * w1; + // t179 = -t97 + t98 + t99 + t112 - t192 = -E*w2*w3+F*w2^2*w1+2*G*w2^2*w1+F*w2*w3-B*w1 + double t179 = -t97 + t98 + t99 + t112 - t192; + // t180 = (-E*w2*w3+F*w2^2*w1+2*G*w2^2*w1+F*w2*w3-B*w1)*Y1 + double t180 = Y1 * t179; + // t181 = A+2*G*w1*w2*w3+F*w1*w2*w3-F*w2^2+E*w2^2 + double t181 = t49 + t51 + t52 - t100 + t111; + // t182 = (A+2*G*w1*w2*w3+F*w1*w2*w3-F*w2^2+E*w2^2)*Z1 + double t182 = Z1 * t181; + // t193 = 2*G*w3*(w2^2+w3^2) + double t193 = t13 * t26 * t34 * w3 * 2.0; + // t194 = F*w3*(w2^2+w3^2) + double t194 = t10 * t25 * t34 * w3; + // t195 = (2*G+F)*w3*(w2^2+w3^2) + double t195 = t193 + t194; + // t196 = 2*G*w3*(w1^2+w3^2) + double t196 = t13 * t26 * t45 * w3 * 2.0; + // t197 = F*w3*(w1^2+w3^2) + double t197 = t10 * t25 * t45 * w3; + // t200 = 2*B*w3 + double t200 = t13 * t14 * w3 * 2.0; + // t198 = (2*G+F)*w3*(w1^2+w3^2) - 2*B*w3 + double t198 = t196 + t197 - t200; + // t199 = F*w3^2 + double t199 = t7 * t10 * t25; + // t201 = 2*w3*G*(w2^2+w3^2) + double t201 = t13 * t24 * t26 * w3 * 2.0; + // t202 = F*w3*(w2^2+w3^2) + double t202 = t10 * t24 * t25 * w3; + // t203 = -t49 + t51 + t52 - t118 + t199 = -A+2*G*w1*w2*w3+F*w1*w2*w3-E*w3^2+F*w3^2 + double t203 = -t49 + t51 + t52 - t118 + t199; + // t204 = (-t49 + t51 + t52 - t118 + t199 = -A+2*G*w1*w2*w3+F*w1*w2*w3-E*w3^2+F*w3^2)*Y1 + double t204 = Y1 * t203; + // t205 = 2*t1 + double t205 = t1 * 2.0; + // t206 = 2*r13*Z1 + double t206 = Z1 * t29 * 2.0; + // t207 = 2*r11*X1 + double t207 = X1 * t32 * 2.0; + // t208 = 2*t1 + 2*r13*Z1 + 2*r11*X1 + 2*r12*Y1 + double t208 = t205 + t206 + t207 - Y1 * t27 * 2.0; + // t209 = 2*t2 + double t209 = t2 * 2.0; + // t210 = 2*r21*X1 + double t210 = X1 * t53 * 2.0; + // t211 = 2*r22*Y1 + double t211 = Y1 * t58 * 2.0; + // t212 = 2*t2 + 2*r21*X1 + 2*r22*Y1 + 2*r23*Z1 + double t212 = t209 + t210 + t211 - Z1 * t55 * 2.0; + + double t213 = t3 * 2.0; + double t214 = Y1 * t40 * 2.0; + double t215 = Z1 * t43 * 2.0; + double t216 = t213 + t214 + t215 - X1 * t38 * 2.0; + + // + jacs(0, 0) = t14 * t65 * (X1 * r1 * w1 * 2.0 + X1 * r2 * w2 + X1 * r3 * w3 + Y1 * r1 * w2 + Z1 * r1 * w3 + r1 * t1 * w1 * 2.0 + r2 * t2 * w1 * 2.0 + r3 * t3 * w1 * 2.0 + Y1 * r3 * t5 * t12 + Y1 * r3 * t9 * t10 - Z1 * r2 * t5 * t12 - Z1 * r2 * t9 * t10 - X1 * r2 * t12 * w2 - X1 * r3 * t12 * w3 - Y1 * r1 * t12 * w2 + Y1 * r2 * t12 * w1 * 2.0 - Z1 * r1 * t12 * w3 + Z1 * r3 * t12 * w1 * 2.0 + Y1 * r3 * t5 * t10 * t11 - Z1 * r2 * t5 * t10 * t11 + X1 * r2 * t12 * w1 * w3 - X1 * r3 * t12 * w1 * w2 - Y1 * r1 * t12 * w1 * w3 + Z1 * r1 * t12 * w1 * w2 - Y1 * r1 * t10 * t11 * w1 * w3 + Z1 * r1 * t10 * t11 * w1 * w2 - X1 * r1 * t6 * t10 * t11 * w1 - X1 * r1 * t7 * t10 * t11 * w1 + X1 * r2 * t5 * t10 * t11 * w2 + X1 * r3 * t5 * t10 * t11 * w3 + Y1 * r1 * t5 * t10 * t11 * w2 - Y1 * r2 * t5 * t10 * t11 * w1 - Y1 * r2 * t7 * t10 * t11 * w1 + Z1 * r1 * t5 * t10 * t11 * w3 - Z1 * r3 * t5 * t10 * t11 * w1 - Z1 * r3 * t6 * t10 * t11 * w1 + X1 * r2 * t10 * t11 * w1 * w3 - X1 * r3 * t10 * t11 * w1 * w2 + Y1 * r3 * t10 * t11 * w1 * w2 * w3 + Z1 * r2 * t10 * t11 * w1 * w2 * w3) - t26 * t65 * t93 * w1 * 2.0 - t14 * t93 * t101 * (t130 + t15 * (-X1 * t121 + Y1 * (t46 + t47 + t48 - t13 * t14 * w2 - t12 * t14 * w1 * w3) + Z1 * (t35 + t36 + t37 - t13 * t14 * w3 - t10 * t25 * w1 * w2)) * 2.0 + t18 * (t135 + t137 - Y1 * (t132 + t133 - t13 * t14 * w1 * 2.0)) * 2.0) * (1.0 / 2.0); + + jacs(0, 1) = t14 * t65 * (X1 * r2 * w1 + Y1 * r1 * w1 + Y1 * r2 * w2 * 2.0 + Y1 * r3 * w3 + Z1 * r2 * w3 + r1 * t1 * w2 * 2.0 + r2 * t2 * w2 * 2.0 + r3 * t3 * w2 * 2.0 - X1 * r3 * t6 * t12 - X1 * r3 * t9 * t10 + Z1 * r1 * t6 * t12 + Z1 * r1 * t9 * t10 + X1 * r1 * t12 * w2 * 2.0 - X1 * r2 * t12 * w1 - Y1 * r1 * t12 * w1 - Y1 * r3 * t12 * w3 - Z1 * r2 * t12 * w3 + Z1 * r3 * t12 * w2 * 2.0 - X1 * r3 * t6 * t10 * t11 + Z1 * r1 * t6 * t10 * t11 + X1 * r2 * t12 * w2 * w3 - Y1 * r1 * t12 * w2 * w3 + Y1 * r3 * t12 * w1 * w2 - Z1 * r2 * t12 * w1 * w2 - Y1 * r1 * t10 * t11 * w2 * w3 + Y1 * r3 * t10 * t11 * w1 * w2 - Z1 * r2 * t10 * t11 * w1 * w2 - X1 * r1 * t6 * t10 * t11 * w2 + X1 * r2 * t6 * t10 * t11 * w1 - X1 * r1 * t7 * t10 * t11 * w2 + Y1 * r1 * t6 * t10 * t11 * w1 - Y1 * r2 * t5 * t10 * t11 * w2 - Y1 * r2 * t7 * t10 * t11 * w2 + Y1 * r3 * t6 * t10 * t11 * w3 - Z1 * r3 * t5 * t10 * t11 * w2 + Z1 * r2 * t6 * t10 * t11 * w3 - Z1 * r3 * t6 * t10 * t11 * w2 + X1 * r2 * t10 * t11 * w2 * w3 + X1 * r3 * t10 * t11 * w1 * w2 * w3 + Z1 * r1 * t10 * t11 * w1 * w2 * w3) - + t26 * t65 * t93 * w2 * 2.0 - t14 * t93 * t101 * (t18 * (Z1 * (-t35 + t94 + t95 + t96 - t13 * t14 * w3) - Y1 * t170 + X1 * (t97 + t98 + t99 - t13 * t14 * w1 - t10 * t25 * w2 * w3)) * 2.0 + t15 * (t180 + t182 - X1 * (t177 + t178 - t13 * t14 * w2 * 2.0)) * 2.0 + t23 * (t175 + Y1 * (t35 - t94 + t95 + t96 - t13 * t14 * w3) - Z1 * t173) * 2.0) * (1.0 / 2.0); + jacs(0, 2) = t14 * t65 * (X1 * r3 * w1 + Y1 * r3 * w2 + Z1 * r1 * w1 + Z1 * r2 * w2 + Z1 * r3 * w3 * 2.0 + r1 * t1 * w3 * 2.0 + r2 * t2 * w3 * 2.0 + r3 * t3 * w3 * 2.0 + X1 * r2 * t7 * t12 + X1 * r2 * t9 * t10 - Y1 * r1 * t7 * t12 - Y1 * r1 * t9 * t10 + X1 * r1 * t12 * w3 * 2.0 - X1 * r3 * t12 * w1 + Y1 * r2 * t12 * w3 * 2.0 - Y1 * r3 * t12 * w2 - Z1 * r1 * t12 * w1 - Z1 * r2 * t12 * w2 + X1 * r2 * t7 * t10 * t11 - Y1 * r1 * t7 * t10 * t11 - X1 * r3 * t12 * w2 * w3 + Y1 * r3 * t12 * w1 * w3 + Z1 * r1 * t12 * w2 * w3 - Z1 * r2 * t12 * w1 * w3 + Y1 * r3 * t10 * t11 * w1 * w3 + Z1 * r1 * t10 * t11 * w2 * w3 - Z1 * r2 * t10 * t11 * w1 * w3 - X1 * r1 * t6 * t10 * t11 * w3 - X1 * r1 * t7 * t10 * t11 * w3 + X1 * r3 * t7 * t10 * t11 * w1 - Y1 * r2 * t5 * t10 * t11 * w3 - Y1 * r2 * t7 * t10 * t11 * w3 + Y1 * r3 * t7 * t10 * t11 * w2 + Z1 * r1 * t7 * t10 * t11 * w1 + Z1 * r2 * t7 * t10 * t11 * w2 - Z1 * r3 * t5 * t10 * t11 * w3 - Z1 * r3 * t6 * t10 * t11 * w3 - X1 * r3 * t10 * t11 * w2 * w3 + X1 * r2 * t10 * t11 * w1 * w2 * w3 + Y1 * r1 * t10 * t11 * w1 * w2 * w3) - + t26 * t65 * t93 * w3 * 2.0 - t14 * t93 * t101 * (t18 * (Z1 * (t46 - t113 + t114 + t115 - t13 * t14 * w2) - Y1 * t198 + X1 * (t49 + t51 + t52 + t118 - t7 * t10 * t25)) * 2.0 + t23 * (X1 * (-t97 + t112 + t116 + t117 - t13 * t14 * w1) + Y1 * (-t46 + t113 + t114 + t115 - t13 * t14 * w2) - Z1 * t195) * 2.0 + t15 * (t204 + Z1 * (t97 - t112 + t116 + t117 - t13 * t14 * w1) - X1 * (t201 + t202 - t13 * t14 * w3 * 2.0)) * 2.0) * (1.0 / 2.0); + + // = 1/2 * r1 * t65 + // - t14 * t93 + // * t101 * t208 + // = 1/2 * r1 * 1 / sqrt((t1 + r11*X1 + r12*Y1 + r13*Z1)^2 + (t2 + r21*X1 + r22*Y1 + r23*Z1)^2 + (t3 + r31*X1 + r32*Y1 + r33*Z1)^2) + // - 1/theta^2 * ((Y1 * r2 * w2^2) + (Z1 * r3 * w3^2) + (r1 * t1 * w1^2) + (r1 * t1 * w2^2) + // + (r1 * t1 * w3^2) + (r2 * t2 * w1^2) + (r2 * t2 * w2^2) + (r2 * t2 * w3^2) + (r3 * t3 * w1^2) + // + (r3 * t3 * w2^2) + (r3 * t3 * w3^2) + (X1 * r1 * w1^2) + (X1 * r2 * w1 * w2) + (X1 * r3 * w1 * w3) + // + (Y1 * r1 * w1 * w2) + (Y1 * r3 * w2 * w3) + (Z1 * r1 * w1 * w3) + (Z1 * r2 * w2 * w3) + (X1 * r1 * w2^2 * cos(theta)) + // + (X1 * r1 * w3^2 * cos(theta)) + (Y1 * r2 * w1^2 * cos(theta)) + (Y1 * r2 * w3^2 * cos(theta)) + (Z1 * r3 * w1^2 * cos(theta)) + (Z1 * r3 * w2^2 * cos(theta)) + // + (X1 * r2 * theta * sin(theta) * w3) + (Y1 * r3 * theta * sin(theta) * w1) + (Z1 * r1 * theta * sin(theta) * w2) - (X1 * r3 * theta * sin(theta) * w2) - (Y1 * r1 * theta * sin(theta) * w3) + // - (Z1 * r2 * theta * sin(theta) * w1) - (X1 * r2 * cos(theta) * w1 * w2) - (X1 * r3 * cos(theta) * w1 * w3) - (Y1 * r1 * cos(theta) * w1 * w2) - () + // - (Y1 * r3 * cos(theta) * w2 * w3) - (Z1 * r1 * cos(theta) * w1 * w3) - (Z1 * r2 * cos(theta) * w2 * w3)) + // * (pow(((t1 + r11*X1 + r12*Y1 + r13*Z1)^2 + (t2 + r21*X1 + r22*Y1 + r23*Z1)^2 + (t3 + r31*X1 + r32*Y1 + r33*Z1)^2), 3/2)) + // * (2*t1 + 2*r11*X1 + 2*r12*Y1 + 2*r13*Z1) + jacs(0, 3) = r1 * t65 - t14 * t93 * t101 * t208 * (1.0 / 2.0); + jacs(0, 4) = r2 * t65 - t14 * t93 * t101 * t212 * (1.0 / 2.0); + jacs(0, 5) = r3 * t65 - t14 * t93 * t101 * t216 * (1.0 / 2.0); + jacs(1, 0) = t14 * t65 * (X1 * s1 * w1 * 2.0 + X1 * s2 * w2 + X1 * s3 * w3 + Y1 * s1 * w2 + Z1 * s1 * w3 + s1 * t1 * w1 * 2.0 + s2 * t2 * w1 * 2.0 + s3 * t3 * w1 * 2.0 + Y1 * s3 * t5 * t12 + Y1 * s3 * t9 * t10 - Z1 * s2 * t5 * t12 - Z1 * s2 * t9 * t10 - X1 * s2 * t12 * w2 - X1 * s3 * t12 * w3 - Y1 * s1 * t12 * w2 + Y1 * s2 * t12 * w1 * 2.0 - Z1 * s1 * t12 * w3 + Z1 * s3 * t12 * w1 * 2.0 + Y1 * s3 * t5 * t10 * t11 - Z1 * s2 * t5 * t10 * t11 + X1 * s2 * t12 * w1 * w3 - X1 * s3 * t12 * w1 * w2 - Y1 * s1 * t12 * w1 * w3 + Z1 * s1 * t12 * w1 * w2 + X1 * s2 * t10 * t11 * w1 * w3 - X1 * s3 * t10 * t11 * w1 * w2 - Y1 * s1 * t10 * t11 * w1 * w3 + Z1 * s1 * t10 * t11 * w1 * w2 - X1 * s1 * t6 * t10 * t11 * w1 - X1 * s1 * t7 * t10 * t11 * w1 + X1 * s2 * t5 * t10 * t11 * w2 + X1 * s3 * t5 * t10 * t11 * w3 + Y1 * s1 * t5 * t10 * t11 * w2 - Y1 * s2 * t5 * t10 * t11 * w1 - Y1 * s2 * t7 * t10 * t11 * w1 + Z1 * s1 * t5 * t10 * t11 * w3 - Z1 * s3 * t5 * t10 * t11 * w1 - Z1 * s3 * t6 * t10 * t11 * w1 + Y1 * s3 * t10 * t11 * w1 * w2 * w3 + Z1 * s2 * t10 * t11 * w1 * w2 * w3) - t14 * t101 * t167 * (t130 + t15 * (Y1 * (t46 + t47 + t48 - t113 - t138) + Z1 * (t35 + t36 + t37 - t94 - t139) - X1 * t121) * 2.0 + t18 * (t135 + t137 - Y1 * (-t131 + t132 + t133)) * 2.0) * (1.0 / 2.0) - t26 * t65 * t167 * w1 * 2.0; + jacs(1, 1) = t14 * t65 * (X1 * s2 * w1 + Y1 * s1 * w1 + Y1 * s2 * w2 * 2.0 + Y1 * s3 * w3 + Z1 * s2 * w3 + s1 * t1 * w2 * 2.0 + s2 * t2 * w2 * 2.0 + s3 * t3 * w2 * 2.0 - X1 * s3 * t6 * t12 - X1 * s3 * t9 * t10 + Z1 * s1 * t6 * t12 + Z1 * s1 * t9 * t10 + X1 * s1 * t12 * w2 * 2.0 - X1 * s2 * t12 * w1 - Y1 * s1 * t12 * w1 - Y1 * s3 * t12 * w3 - Z1 * s2 * t12 * w3 + Z1 * s3 * t12 * w2 * 2.0 - X1 * s3 * t6 * t10 * t11 + Z1 * s1 * t6 * t10 * t11 + X1 * s2 * t12 * w2 * w3 - Y1 * s1 * t12 * w2 * w3 + Y1 * s3 * t12 * w1 * w2 - Z1 * s2 * t12 * w1 * w2 + X1 * s2 * t10 * t11 * w2 * w3 - Y1 * s1 * t10 * t11 * w2 * w3 + Y1 * s3 * t10 * t11 * w1 * w2 - Z1 * s2 * t10 * t11 * w1 * w2 - X1 * s1 * t6 * t10 * t11 * w2 + X1 * s2 * t6 * t10 * t11 * w1 - X1 * s1 * t7 * t10 * t11 * w2 + Y1 * s1 * t6 * t10 * t11 * w1 - Y1 * s2 * t5 * t10 * t11 * w2 - Y1 * s2 * t7 * t10 * t11 * w2 + Y1 * s3 * t6 * t10 * t11 * w3 - Z1 * s3 * t5 * t10 * t11 * w2 + Z1 * s2 * t6 * t10 * t11 * w3 - Z1 * s3 * t6 * t10 * t11 * w2 + X1 * s3 * t10 * t11 * w1 * w2 * w3 + Z1 * s1 * t10 * t11 * w1 * w2 * w3) - + t26 * t65 * t167 * w2 * 2.0 - t14 * t101 * t167 * (t18 * (X1 * (t97 + t98 + t99 - t112 - t192) + Z1 * (-t35 + t94 + t95 + t96 - t139) - Y1 * t170) * 2.0 + t15 * (t180 + t182 - X1 * (-t176 + t177 + t178)) * 2.0 + t23 * (t175 + Y1 * (t35 - t94 + t95 + t96 - t139) - Z1 * t173) * 2.0) * (1.0 / 2.0); + jacs(1, 2) = t14 * t65 * (X1 * s3 * w1 + Y1 * s3 * w2 + Z1 * s1 * w1 + Z1 * s2 * w2 + Z1 * s3 * w3 * 2.0 + s1 * t1 * w3 * 2.0 + s2 * t2 * w3 * 2.0 + s3 * t3 * w3 * 2.0 + X1 * s2 * t7 * t12 + X1 * s2 * t9 * t10 - Y1 * s1 * t7 * t12 - Y1 * s1 * t9 * t10 + X1 * s1 * t12 * w3 * 2.0 - X1 * s3 * t12 * w1 + Y1 * s2 * t12 * w3 * 2.0 - Y1 * s3 * t12 * w2 - Z1 * s1 * t12 * w1 - Z1 * s2 * t12 * w2 + X1 * s2 * t7 * t10 * t11 - Y1 * s1 * t7 * t10 * t11 - X1 * s3 * t12 * w2 * w3 + Y1 * s3 * t12 * w1 * w3 + Z1 * s1 * t12 * w2 * w3 - Z1 * s2 * t12 * w1 * w3 - X1 * s3 * t10 * t11 * w2 * w3 + Y1 * s3 * t10 * t11 * w1 * w3 + Z1 * s1 * t10 * t11 * w2 * w3 - Z1 * s2 * t10 * t11 * w1 * w3 - X1 * s1 * t6 * t10 * t11 * w3 - X1 * s1 * t7 * t10 * t11 * w3 + X1 * s3 * t7 * t10 * t11 * w1 - Y1 * s2 * t5 * t10 * t11 * w3 - Y1 * s2 * t7 * t10 * t11 * w3 + Y1 * s3 * t7 * t10 * t11 * w2 + Z1 * s1 * t7 * t10 * t11 * w1 + Z1 * s2 * t7 * t10 * t11 * w2 - Z1 * s3 * t5 * t10 * t11 * w3 - Z1 * s3 * t6 * t10 * t11 * w3 + X1 * s2 * t10 * t11 * w1 * w2 * w3 + Y1 * s1 * t10 * t11 * w1 * w2 * w3) - + t26 * t65 * t167 * w3 * 2.0 - t14 * t101 * t167 * (t18 * (Z1 * (t46 - t113 + t114 + t115 - t138) - Y1 * t198 + X1 * (t49 + t51 + t52 + t118 - t199)) * 2.0 + t23 * (X1 * (-t97 + t112 + t116 + t117 - t192) + Y1 * (-t46 + t113 + t114 + t115 - t138) - Z1 * t195) * 2.0 + t15 * (t204 + Z1 * (t97 - t112 + t116 + t117 - t192) - X1 * (-t200 + t201 + t202)) * 2.0) * (1.0 / 2.0); + jacs(1, 3) = s1 * t65 - t14 * t101 * t167 * t208 * (1.0 / 2.0); + jacs(1, 4) = s2 * t65 - t14 * t101 * t167 * t212 * (1.0 / 2.0); + jacs(1, 5) = s3 * t65 - t14 * t101 * t167 * t216 * (1.0 / 2.0); +} +} // End namespace ORB_SLAM3 diff --git a/ORB-SLAM3-UESTC/Workspace/src/Map.cc b/ORB-SLAM3-UESTC/Workspace/src/Map.cc new file mode 100644 index 0000000..34f160a --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/Map.cc @@ -0,0 +1,544 @@ +/** + * This file is part of ORB-SLAM3 + * + * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * + * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public + * License as published by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even + * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with ORB-SLAM3. + * If not, see . + */ + +#include "Map.h" + +#include + +namespace ORB_SLAM3 +{ + +long unsigned int Map::nNextId = 0; + +Map::Map() + : mnMaxKFid(0), mnBigChangeIdx(0), mbImuInitialized(false), mnMapChange(0), mpFirstRegionKF(static_cast(NULL)), + mbFail(false), mIsInUse(false), mHasTumbnail(false), mbBad(false), mnMapChangeNotified(0), mbIsInertial(false), mbIMU_BA1(false), mbIMU_BA2(false) +{ + mnId = nNextId++; + mThumbnail = static_cast(NULL); +} + +Map::Map(int initKFid) + : mnInitKFid(initKFid), mnMaxKFid(initKFid), /*mnLastLoopKFid(initKFid),*/ mnBigChangeIdx(0), mIsInUse(false), + mHasTumbnail(false), mbBad(false), mbImuInitialized(false), mpFirstRegionKF(static_cast(NULL)), + mnMapChange(0), mbFail(false), mnMapChangeNotified(0), mbIsInertial(false), mbIMU_BA1(false), mbIMU_BA2(false) +{ + mnId = nNextId++; + mThumbnail = static_cast(NULL); +} + +Map::~Map() +{ + // TODO: erase all points from memory + mspMapPoints.clear(); + + // TODO: erase all keyframes from memory + mspKeyFrames.clear(); + + if (mThumbnail) + delete mThumbnail; + mThumbnail = static_cast(NULL); + + mvpReferenceMapPoints.clear(); + mvpKeyFrameOrigins.clear(); +} + +// 在地图中插入关键帧,同时更新关键帧的最大id +void Map::AddKeyFrame(KeyFrame *pKF) +{ + unique_lock lock(mMutexMap); + if (mspKeyFrames.empty()) + { + cout << "First KF:" << pKF->mnId << "; Map init KF:" << mnInitKFid << endl; + mnInitKFid = pKF->mnId; + mpKFinitial = pKF; + mpKFlowerID = pKF; + } + mspKeyFrames.insert(pKF); + if (pKF->mnId > mnMaxKFid) + { + mnMaxKFid = pKF->mnId; + } + if (pKF->mnId < mpKFlowerID->mnId) + { + mpKFlowerID = pKF; + } +} + +void Map::AddMapPoint(MapPoint *pMP) +{ + unique_lock lock(mMutexMap); + mspMapPoints.insert(pMP); +} + +void Map::SetImuInitialized() +{ + unique_lock lock(mMutexMap); + mbImuInitialized = true; +} + +bool Map::isImuInitialized() +{ + unique_lock lock(mMutexMap); + return mbImuInitialized; +} + +void Map::EraseMapPoint(MapPoint *pMP) +{ + unique_lock lock(mMutexMap); + mspMapPoints.erase(pMP); + + // 下面是作者加入的注释. 实际上只是从std::set中删除了地图点的指针, 原先地图点 + // 占用的内存区域并没有得到释放 + // TODO: This only erase the pointer. + // Delete the MapPoint +} + +void Map::EraseKeyFrame(KeyFrame *pKF) +{ + unique_lock lock(mMutexMap); + mspKeyFrames.erase(pKF); + if (mspKeyFrames.size() > 0) + { + if (pKF->mnId == mpKFlowerID->mnId) + { + vector vpKFs = vector(mspKeyFrames.begin(), mspKeyFrames.end()); + sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId); + mpKFlowerID = vpKFs[0]; + } + } + else + { + mpKFlowerID = 0; + } + + // TODO: This only erase the pointer. + // Delete the MapPoint +} + +/* + * @brief 设置参考MapPoints，将用于DrawMapPoints函数画图 + * 设置参考地图点用于绘图显示局部地图点（红色） + * @param vpMPs Local MapPoints + */ +void Map::SetReferenceMapPoints(const vector &vpMPs) +{ + unique_lock lock(mMutexMap); + mvpReferenceMapPoints = vpMPs; +} + +void Map::InformNewBigChange() +{ + unique_lock lock(mMutexMap); + mnBigChangeIdx++; +} + +int Map::GetLastBigChangeIdx() +{ + unique_lock lock(mMutexMap); + return mnBigChangeIdx; +} + +// 获取地图中的所有关键帧 +vector Map::GetAllKeyFrames() +{ + unique_lock lock(mMutexMap); + return vector(mspKeyFrames.begin(), mspKeyFrames.end()); +} + +// 获取地图中的所有地图点 +vector Map::GetAllMapPoints() +{ + unique_lock lock(mMutexMap); + return vector(mspMapPoints.begin(), mspMapPoints.end()); +} + +// 获取地图点数目 +long unsigned int Map::MapPointsInMap() +{ + unique_lock lock(mMutexMap); + return mspMapPoints.size(); +} + +// 获取地图中的关键帧数目 +long unsigned int Map::KeyFramesInMap() +{ + unique_lock lock(mMutexMap); + return mspKeyFrames.size(); +} + +// 获取参考地图点 +vector Map::GetReferenceMapPoints() +{ + unique_lock lock(mMutexMap); + return mvpReferenceMapPoints; +} + +long unsigned int Map::GetId() +{ + return mnId; +} +long unsigned int Map::GetInitKFid() +{ + unique_lock lock(mMutexMap); + return mnInitKFid; +} + +void Map::SetInitKFid(long unsigned int initKFif) +{ + unique_lock lock(mMutexMap); + mnInitKFid = initKFif; +} + +long unsigned int Map::GetMaxKFid() +{ + unique_lock lock(mMutexMap); + return mnMaxKFid; +} + +KeyFrame *Map::GetOriginKF() +{ + return mpKFinitial; +} + +void Map::SetCurrentMap() +{ + mIsInUse = true; +} + +void Map::SetStoredMap() +{ + mIsInUse = false; +} + +void Map::clear() +{ + // for(set::iterator sit=mspMapPoints.begin(), send=mspMapPoints.end(); sit!=send; sit++) + // delete *sit; + + for (set::iterator sit = mspKeyFrames.begin(), send = mspKeyFrames.end(); sit != send; sit++) + { + KeyFrame *pKF = *sit; + pKF->UpdateMap(static_cast(NULL)); + // delete *sit; + } + + mspMapPoints.clear(); + mspKeyFrames.clear(); + mnMaxKFid = mnInitKFid; + mbImuInitialized = false; + mvpReferenceMapPoints.clear(); + mvpKeyFrameOrigins.clear(); + mbIMU_BA1 = false; + mbIMU_BA2 = false; +} + +bool Map::IsInUse() +{ + return mIsInUse; +} + +void Map::SetBad() +{ + mbBad = true; +} + +bool Map::IsBad() +{ + return mbBad; +} + +// 恢复尺度及重力方向 +/** imu在localmapping中初始化，LocalMapping::InitializeIMU中使用，误差包含三个残差与两个偏置 + * 地图融合时也会使用 + * @param R 初始化时为Rgw + * @param s 尺度 + * @param bScaledVel 将尺度更新到速度 + * @param t 默认cv::Mat::zeros(cv::Size(1,3),CV_32F) + */ +void Map::ApplyScaledRotation(const Sophus::SE3f &T, const float s, const bool bScaledVel) +{ + unique_lock lock(mMutexMap); + + // Body position (IMU) of first keyframe is fixed to (0,0,0) + Sophus::SE3f Tyw = T; + Eigen::Matrix3f Ryw = Tyw.rotationMatrix(); + Eigen::Vector3f tyw = Tyw.translation(); + + for (set::iterator sit = mspKeyFrames.begin(); sit != mspKeyFrames.end(); sit++) + { + // 更新关键帧位姿 + /** + * | Rw2w1 tw2w1 | * | Rw1c s*tw1c | = | Rw2c s*Rw2w1*tw1c + tw2w1 | + * | 0 1 | | 0 1 | | 0 1 | + * 这么做比正常乘在旋转上少了个s，后面不需要这个s了，因为所有mp在下面已经全部转到了w2坐标系下，不存在尺度变化了 + * + * | s*Rw2w1 tw2w1 | * | Rw1c tw1c | = | s*Rw2c s*Rw2w1*tw1c + tw2w1 | + * | 0 1 | | 0 1 | | 0 1 | + */ + KeyFrame *pKF = *sit; + Sophus::SE3f Twc = pKF->GetPoseInverse(); + Twc.translation() *= s; + + // | Ryc s*Ryw*twc + tyw | + // | 0 1 | + Sophus::SE3f Tyc = Tyw * Twc; + Sophus::SE3f Tcy = Tyc.inverse(); + pKF->SetPose(Tcy); + // 更新关键帧速度 + Eigen::Vector3f Vw = pKF->GetVelocity(); + if (!bScaledVel) + pKF->SetVelocity(Ryw * Vw); + else + pKF->SetVelocity(Ryw * Vw * s); + } + for (set::iterator sit = mspMapPoints.begin(); sit != mspMapPoints.end(); sit++) + { + // 更新每一个mp在世界坐标系下的坐标 + MapPoint *pMP = *sit; + if (!pMP || pMP->isBad()) + continue; + pMP->SetWorldPos(s * Ryw * pMP->GetWorldPos() + tyw); + pMP->UpdateNormalAndDepth(); + } + mnMapChange++; +} + +void Map::SetInertialSensor() +{ + unique_lock lock(mMutexMap); + mbIsInertial = true; +} + +bool Map::IsInertial() +{ + unique_lock lock(mMutexMap); + // 将mbIsInertial设置为true,将其设置为imu属性,以后的跟踪和预积分将和这个标志有关 + return mbIsInertial; +} + +void Map::SetIniertialBA1() +{ + unique_lock lock(mMutexMap); + mbIMU_BA1 = true; +} + +void Map::SetIniertialBA2() +{ + unique_lock lock(mMutexMap); + mbIMU_BA2 = true; +} + +bool Map::GetIniertialBA1() +{ + unique_lock lock(mMutexMap); + return mbIMU_BA1; +} + +bool Map::GetIniertialBA2() +{ + unique_lock lock(mMutexMap); + return mbIMU_BA2; +} + +void Map::ChangeId(long unsigned int nId) +{ + mnId = nId; +} + +unsigned int Map::GetLowerKFID() +{ + unique_lock lock(mMutexMap); + if (mpKFlowerID) + { + return mpKFlowerID->mnId; + } + return 0; +} + +int Map::GetMapChangeIndex() +{ + unique_lock lock(mMutexMap); + return mnMapChange; +} + +void Map::IncreaseChangeIndex() +{ + unique_lock lock(mMutexMap); + mnMapChange++; +} + +int Map::GetLastMapChange() +{ + unique_lock lock(mMutexMap); + return mnMapChangeNotified; +} + +void Map::SetLastMapChange(int currentChangeId) +{ + unique_lock lock(mMutexMap); + mnMapChangeNotified = currentChangeId; +} + +/** 预保存，也就是把想保存的信息保存到备份的变量中 + * @param spCams 相机 + */ +void Map::PreSave(std::set &spCams) +{ + int nMPWithoutObs = 0; // 统计用 + // 1. 剔除一下无效观测 + for (MapPoint *pMPi : mspMapPoints) + { + if (!pMPi || pMPi->isBad()) + continue; + + if (pMPi->GetObservations().size() == 0) + { + nMPWithoutObs++; + } + map> mpObs = pMPi->GetObservations(); + for (map>::iterator it = mpObs.begin(), end = mpObs.end(); it != end; ++it) + { + if (!it->first || it->first->GetMap() != this || it->first->isBad()) + { + pMPi->EraseObservation(it->first, false); + } + } + } + + // Saves the id of KF origins + // 2. 保存最开始的帧的id，貌似一个map的mvpKeyFrameOrigins里面只有一个，可以验证一下 + mvBackupKeyFrameOriginsId.clear(); + mvBackupKeyFrameOriginsId.reserve(mvpKeyFrameOrigins.size()); + for (int i = 0, numEl = mvpKeyFrameOrigins.size(); i < numEl; ++i) + { + mvBackupKeyFrameOriginsId.push_back(mvpKeyFrameOrigins[i]->mnId); + } + + // Backup of MapPoints + // 3. 保存一下对应的mp + mvpBackupMapPoints.clear(); + for (MapPoint *pMPi : mspMapPoints) + { + if (!pMPi || pMPi->isBad()) + continue; + + mvpBackupMapPoints.push_back(pMPi); + pMPi->PreSave(mspKeyFrames, mspMapPoints); + } + + // Backup of KeyFrames + // 4. 保存一下对应的KF + mvpBackupKeyFrames.clear(); + for (KeyFrame *pKFi : mspKeyFrames) + { + if (!pKFi || pKFi->isBad()) + continue; + + mvpBackupKeyFrames.push_back(pKFi); + pKFi->PreSave(mspKeyFrames, mspMapPoints, spCams); + } + + // 保存一些id + mnBackupKFinitialID = -1; + if (mpKFinitial) + { + mnBackupKFinitialID = mpKFinitial->mnId; + } + + mnBackupKFlowerID = -1; + if (mpKFlowerID) + { + mnBackupKFlowerID = mpKFlowerID->mnId; + } +} + +/** 后加载，也就是把备份的变量恢复到正常变量中 + * @param spCams 相机 + */ +void Map::PostLoad(KeyFrameDatabase *pKFDB, ORBVocabulary *pORBVoc /*, map& mpKeyFrameId*/, map &mpCams) +{ + std::copy(mvpBackupMapPoints.begin(), mvpBackupMapPoints.end(), std::inserter(mspMapPoints, mspMapPoints.begin())); + std::copy(mvpBackupKeyFrames.begin(), mvpBackupKeyFrames.end(), std::inserter(mspKeyFrames, mspKeyFrames.begin())); + + // 1. 恢复map中的mp，注意此时mp中只恢复了保存的量 + map mpMapPointId; + for (MapPoint *pMPi : mspMapPoints) + { + if (!pMPi || pMPi->isBad()) + continue; + + pMPi->UpdateMap(this); + mpMapPointId[pMPi->mnId] = pMPi; + } + + // 2. 恢复map中的kf，注意此时kf中只恢复了保存的量 + map mpKeyFrameId; + for (KeyFrame *pKFi : mspKeyFrames) + { + if (!pKFi || pKFi->isBad()) + continue; + + pKFi->UpdateMap(this); + pKFi->SetORBVocabulary(pORBVoc); + pKFi->SetKeyFrameDatabase(pKFDB); + mpKeyFrameId[pKFi->mnId] = pKFi; + } + + // References reconstruction between different instances + // 3. 使用mp中的备份变量恢复正常变量 + for (MapPoint *pMPi : mspMapPoints) + { + if (!pMPi || pMPi->isBad()) + continue; + + pMPi->PostLoad(mpKeyFrameId, mpMapPointId); + } + + // 4. 使用kf中的备份变量恢复正常变量 + for (KeyFrame *pKFi : mspKeyFrames) + { + if (!pKFi || pKFi->isBad()) + continue; + + pKFi->PostLoad(mpKeyFrameId, mpMapPointId, mpCams); + pKFDB->add(pKFi); + } + + // 5. 恢复ID + if (mnBackupKFinitialID != -1) + { + mpKFinitial = mpKeyFrameId[mnBackupKFinitialID]; + } + + if (mnBackupKFlowerID != -1) + { + mpKFlowerID = mpKeyFrameId[mnBackupKFlowerID]; + } + + mvpKeyFrameOrigins.clear(); + mvpKeyFrameOrigins.reserve(mvBackupKeyFrameOriginsId.size()); + for (int i = 0; i < mvBackupKeyFrameOriginsId.size(); ++i) + { + mvpKeyFrameOrigins.push_back(mpKeyFrameId[mvBackupKeyFrameOriginsId[i]]); + } + + mvpBackupMapPoints.clear(); +} + +} // namespace ORB_SLAM3 diff --git a/ORB-SLAM3-UESTC/Workspace/src/MapDrawer.cc b/ORB-SLAM3-UESTC/Workspace/src/MapDrawer.cc new file mode 100644 index 0000000..2ff8fae --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/MapDrawer.cc @@ -0,0 +1,467 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#include "MapDrawer.h" +#include "MapPoint.h" +#include "KeyFrame.h" +#include +#include + +namespace ORB_SLAM3 +{ + + +MapDrawer::MapDrawer(Atlas* pAtlas, const string &strSettingPath, Settings* settings):mpAtlas(pAtlas) +{ + if(settings){ + newParameterLoader(settings); + } + else{ + cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); + bool is_correct = ParseViewerParamFile(fSettings); + + if(!is_correct) + { + std::cerr << "**ERROR in the config file, the format is not correct**" << std::endl; + try + { + throw -1; + } + catch(exception &e) + { + + } + } + } +} + +void MapDrawer::newParameterLoader(Settings *settings) { + mKeyFrameSize = settings->keyFrameSize(); + mKeyFrameLineWidth = settings->keyFrameLineWidth(); + mGraphLineWidth = settings->graphLineWidth(); + mPointSize = settings->pointSize(); + mCameraSize = settings->cameraSize(); + mCameraLineWidth = settings->cameraLineWidth(); +} + +bool MapDrawer::ParseViewerParamFile(cv::FileStorage &fSettings) +{ + bool b_miss_params = false; + + cv::FileNode node = fSettings["Viewer.KeyFrameSize"]; + if(!node.empty()) + { + mKeyFrameSize = node.real(); + } + else + { + std::cerr << "*Viewer.KeyFrameSize parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Viewer.KeyFrameLineWidth"]; + if(!node.empty()) + { + mKeyFrameLineWidth = node.real(); + } + else + { + std::cerr << "*Viewer.KeyFrameLineWidth parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Viewer.GraphLineWidth"]; + if(!node.empty()) + { + mGraphLineWidth = node.real(); + } + else + { + std::cerr << "*Viewer.GraphLineWidth parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Viewer.PointSize"]; + if(!node.empty()) + { + mPointSize = node.real(); + } + else + { + std::cerr << "*Viewer.PointSize parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Viewer.CameraSize"]; + if(!node.empty()) + { + mCameraSize = node.real(); + } + else + { + std::cerr << "*Viewer.CameraSize parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Viewer.CameraLineWidth"]; + if(!node.empty()) + { + mCameraLineWidth = node.real(); + } + else + { + std::cerr << "*Viewer.CameraLineWidth parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + return !b_miss_params; +} + +void MapDrawer::DrawMapPoints() +{ + Map* pActiveMap = mpAtlas->GetCurrentMap(); + if(!pActiveMap) + return; + + const vector &vpMPs = pActiveMap->GetAllMapPoints(); + const vector &vpRefMPs = pActiveMap->GetReferenceMapPoints(); + + set spRefMPs(vpRefMPs.begin(), vpRefMPs.end()); + + if(vpMPs.empty()) + return; + + glPointSize(mPointSize); + glBegin(GL_POINTS); + glColor3f(0.0,0.0,0.0); + + for(size_t i=0, iend=vpMPs.size(); iisBad() || spRefMPs.count(vpMPs[i])) + continue; + Eigen::Matrix pos = vpMPs[i]->GetWorldPos(); + glVertex3f(pos(0),pos(1),pos(2)); + } + glEnd(); + + glPointSize(mPointSize); + glBegin(GL_POINTS); + glColor3f(1.0,0.0,0.0); + + for(set::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++) + { + if((*sit)->isBad()) + continue; + Eigen::Matrix pos = (*sit)->GetWorldPos(); + glVertex3f(pos(0),pos(1),pos(2)); + + } + + glEnd(); +} + +void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const bool bDrawInertialGraph, const bool bDrawOptLba) +{ + const float &w = mKeyFrameSize; + const float h = w*0.75; + const float z = w*0.6; + + Map* pActiveMap = mpAtlas->GetCurrentMap(); + // DEBUG LBA + std::set sOptKFs = pActiveMap->msOptKFs; + std::set sFixedKFs = pActiveMap->msFixedKFs; + + if(!pActiveMap) + return; + + const vector vpKFs = pActiveMap->GetAllKeyFrames(); + + if(bDrawKF) + { + for(size_t i=0; iGetPoseInverse().matrix(); + unsigned int index_color = pKF->mnOriginMapId; + + glPushMatrix(); + + glMultMatrixf((GLfloat*)Twc.data()); + + if(!pKF->GetParent()) // It is the first KF in the map + { + glLineWidth(mKeyFrameLineWidth*5); + glColor3f(1.0f,0.0f,0.0f); + glBegin(GL_LINES); + } + else + { + //cout << "Child KF: " << vpKFs[i]->mnId << endl; + glLineWidth(mKeyFrameLineWidth); + if (bDrawOptLba) { + if(sOptKFs.find(pKF->mnId) != sOptKFs.end()) + { + glColor3f(0.0f,1.0f,0.0f); // Green -> Opt KFs + } + else if(sFixedKFs.find(pKF->mnId) != sFixedKFs.end()) + { + glColor3f(1.0f,0.0f,0.0f); // Red -> Fixed KFs + } + else + { + glColor3f(0.0f,0.0f,1.0f); // Basic color + } + } + else + { + glColor3f(0.0f,0.0f,1.0f); // Basic color + } + glBegin(GL_LINES); + } + + glVertex3f(0,0,0); + glVertex3f(w,h,z); + glVertex3f(0,0,0); + glVertex3f(w,-h,z); + glVertex3f(0,0,0); + glVertex3f(-w,-h,z); + glVertex3f(0,0,0); + glVertex3f(-w,h,z); + + glVertex3f(w,h,z); + glVertex3f(w,-h,z); + + glVertex3f(-w,h,z); + glVertex3f(-w,-h,z); + + glVertex3f(-w,h,z); + glVertex3f(w,h,z); + + glVertex3f(-w,-h,z); + glVertex3f(w,-h,z); + glEnd(); + + glPopMatrix(); + + glEnd(); + } + } + + if(bDrawGraph) + { + glLineWidth(mGraphLineWidth); + glColor4f(0.0f,1.0f,0.0f,0.6f); + glBegin(GL_LINES); + + // cout << "-----------------Draw graph-----------------" << endl; + for(size_t i=0; i vCovKFs = vpKFs[i]->GetCovisiblesByWeight(100); + Eigen::Vector3f Ow = vpKFs[i]->GetCameraCenter(); + if(!vCovKFs.empty()) + { + for(vector::const_iterator vit=vCovKFs.begin(), vend=vCovKFs.end(); vit!=vend; vit++) + { + if((*vit)->mnIdmnId) + continue; + Eigen::Vector3f Ow2 = (*vit)->GetCameraCenter(); + glVertex3f(Ow(0),Ow(1),Ow(2)); + glVertex3f(Ow2(0),Ow2(1),Ow2(2)); + } + } + + // Spanning tree + KeyFrame* pParent = vpKFs[i]->GetParent(); + if(pParent) + { + Eigen::Vector3f Owp = pParent->GetCameraCenter(); + glVertex3f(Ow(0),Ow(1),Ow(2)); + glVertex3f(Owp(0),Owp(1),Owp(2)); + } + + // Loops + set sLoopKFs = vpKFs[i]->GetLoopEdges(); + for(set::iterator sit=sLoopKFs.begin(), send=sLoopKFs.end(); sit!=send; sit++) + { + if((*sit)->mnIdmnId) + continue; + Eigen::Vector3f Owl = (*sit)->GetCameraCenter(); + glVertex3f(Ow(0),Ow(1),Ow(2)); + glVertex3f(Owl(0),Owl(1),Owl(2)); + } + } + + glEnd(); + } + + if(bDrawInertialGraph && pActiveMap->isImuInitialized()) + { + glLineWidth(mGraphLineWidth); + glColor4f(1.0f,0.0f,0.0f,0.6f); + glBegin(GL_LINES); + + //Draw inertial links + for(size_t i=0; iGetCameraCenter(); + KeyFrame* pNext = pKFi->mNextKF; + if(pNext) + { + Eigen::Vector3f Owp = pNext->GetCameraCenter(); + glVertex3f(Ow(0),Ow(1),Ow(2)); + glVertex3f(Owp(0),Owp(1),Owp(2)); + } + } + + glEnd(); + } + + vector vpMaps = mpAtlas->GetAllMaps(); + + if(bDrawKF) + { + for(Map* pMap : vpMaps) + { + if(pMap == pActiveMap) + continue; + + vector vpKFs = pMap->GetAllKeyFrames(); + + for(size_t i=0; iGetPoseInverse().matrix(); + unsigned int index_color = pKF->mnOriginMapId; + + glPushMatrix(); + + glMultMatrixf((GLfloat*)Twc.data()); + + if(!vpKFs[i]->GetParent()) // It is the first KF in the map + { + glLineWidth(mKeyFrameLineWidth*5); + glColor3f(1.0f,0.0f,0.0f); + glBegin(GL_LINES); + } + else + { + glLineWidth(mKeyFrameLineWidth); + glColor3f(mfFrameColors[index_color][0],mfFrameColors[index_color][1],mfFrameColors[index_color][2]); + glBegin(GL_LINES); + } + + glVertex3f(0,0,0); + glVertex3f(w,h,z); + glVertex3f(0,0,0); + glVertex3f(w,-h,z); + glVertex3f(0,0,0); + glVertex3f(-w,-h,z); + glVertex3f(0,0,0); + glVertex3f(-w,h,z); + + glVertex3f(w,h,z); + glVertex3f(w,-h,z); + + glVertex3f(-w,h,z); + glVertex3f(-w,-h,z); + + glVertex3f(-w,h,z); + glVertex3f(w,h,z); + + glVertex3f(-w,-h,z); + glVertex3f(w,-h,z); + glEnd(); + + glPopMatrix(); + } + } + } +} + +void MapDrawer::DrawCurrentCamera(pangolin::OpenGlMatrix &Twc) +{ + const float &w = mCameraSize; + const float h = w*0.75; + const float z = w*0.6; + + glPushMatrix(); + +#ifdef HAVE_GLES + glMultMatrixf(Twc.m); +#else + glMultMatrixd(Twc.m); +#endif + + glLineWidth(mCameraLineWidth); + glColor3f(0.0f,1.0f,0.0f); + glBegin(GL_LINES); + glVertex3f(0,0,0); + glVertex3f(w,h,z); + glVertex3f(0,0,0); + glVertex3f(w,-h,z); + glVertex3f(0,0,0); + glVertex3f(-w,-h,z); + glVertex3f(0,0,0); + glVertex3f(-w,h,z); + + glVertex3f(w,h,z); + glVertex3f(w,-h,z); + + glVertex3f(-w,h,z); + glVertex3f(-w,-h,z); + + glVertex3f(-w,h,z); + glVertex3f(w,h,z); + + glVertex3f(-w,-h,z); + glVertex3f(w,-h,z); + glEnd(); + + glPopMatrix(); +} + + +void MapDrawer::SetCurrentCameraPose(const Sophus::SE3f &Tcw) +{ + unique_lock lock(mMutexCamera); + mCameraPose = Tcw.inverse(); +} + +void MapDrawer::GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw) +{ + Eigen::Matrix4f Twc; + { + unique_lock lock(mMutexCamera); + Twc = mCameraPose.matrix(); + } + + for (int i = 0; i<4; i++) { + M.m[4*i] = Twc(0,i); + M.m[4*i+1] = Twc(1,i); + M.m[4*i+2] = Twc(2,i); + M.m[4*i+3] = Twc(3,i); + } + + MOw.SetIdentity(); + MOw.m[12] = Twc(0,3); + MOw.m[13] = Twc(1,3); + MOw.m[14] = Twc(2,3); +} +} //namespace ORB_SLAM diff --git a/ORB-SLAM3-UESTC/Workspace/src/MapPoint.cc b/ORB-SLAM3-UESTC/Workspace/src/MapPoint.cc new file mode 100644 index 0000000..85a4156 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/MapPoint.cc @@ -0,0 +1,835 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#include "MapPoint.h" +#include "ORBmatcher.h" + +#include + +namespace ORB_SLAM3 +{ + +long unsigned int MapPoint::nNextId=0; +mutex MapPoint::mGlobalMutex; + +/** + * @brief 构造函数 + */ +MapPoint::MapPoint(): + mnFirstKFid(0), mnFirstFrame(0), nObs(0), mnTrackReferenceForFrame(0), + mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0), + mnCorrectedReference(0), mnBAGlobalForKF(0), mnVisible(1), mnFound(1), mbBad(false), + mpReplaced(static_cast(NULL)) +{ + mpReplaced = static_cast(NULL); +} + +/** + * @brief 构造函数 + */ +MapPoint::MapPoint(const Eigen::Vector3f &Pos, KeyFrame *pRefKF, Map* pMap): + mnFirstKFid(pRefKF->mnId), mnFirstFrame(pRefKF->mnFrameId), nObs(0), mnTrackReferenceForFrame(0), + mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0), + mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(pRefKF), mnVisible(1), mnFound(1), mbBad(false), + mpReplaced(static_cast(NULL)), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap), + mnOriginMapId(pMap->GetId()) +{ + SetWorldPos(Pos); + + mNormalVector.setZero(); + + mbTrackInViewR = false; + mbTrackInView = false; + + // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id. + unique_lock lock(mpMap->mMutexPointCreation); + mnId=nNextId++; +} + +/** + * @brief 构造函数 + */ +MapPoint::MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF, KeyFrame* pHostKF, Map* pMap): + mnFirstKFid(pRefKF->mnId), mnFirstFrame(pRefKF->mnFrameId), nObs(0), mnTrackReferenceForFrame(0), + mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0), + mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(pRefKF), mnVisible(1), mnFound(1), mbBad(false), + mpReplaced(static_cast(NULL)), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap), + mnOriginMapId(pMap->GetId()) +{ + mInvDepth=invDepth; + mInitU=(double)uv_init.x; + mInitV=(double)uv_init.y; + mpHostKF = pHostKF; + + mNormalVector.setZero(); + + // Worldpos is not set + // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id. + unique_lock lock(mpMap->mMutexPointCreation); + mnId=nNextId++; +} + +/** + * @brief 构造函数 + */ +MapPoint::MapPoint(const Eigen::Vector3f &Pos, Map* pMap, Frame* pFrame, const int &idxF): + mnFirstKFid(-1), mnFirstFrame(pFrame->mnId), nObs(0), mnTrackReferenceForFrame(0), mnLastFrameSeen(0), + mnBALocalForKF(0), mnFuseCandidateForKF(0),mnLoopPointForKF(0), mnCorrectedByKF(0), + mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(static_cast(NULL)), mnVisible(1), + mnFound(1), mbBad(false), mpReplaced(NULL), mpMap(pMap), mnOriginMapId(pMap->GetId()) +{ + SetWorldPos(Pos); + + Eigen::Vector3f Ow; + if(pFrame -> Nleft == -1 || idxF < pFrame -> Nleft){ + Ow = pFrame->GetCameraCenter(); + } + else{ + Eigen::Matrix3f Rwl = pFrame->GetRwc(); + Eigen::Vector3f tlr = pFrame->GetRelativePoseTlr().translation(); + Eigen::Vector3f twl = pFrame->GetOw(); + + Ow = Rwl * tlr + twl; + } + mNormalVector = mWorldPos - Ow; + mNormalVector = mNormalVector / mNormalVector.norm(); + + Eigen::Vector3f PC = mWorldPos - Ow; + const float dist = PC.norm(); + const int level = (pFrame -> Nleft == -1) ? pFrame->mvKeysUn[idxF].octave + : (idxF < pFrame -> Nleft) ? pFrame->mvKeys[idxF].octave + : pFrame -> mvKeysRight[idxF].octave; + const float levelScaleFactor = pFrame->mvScaleFactors[level]; + const int nLevels = pFrame->mnScaleLevels; + + mfMaxDistance = dist*levelScaleFactor; + mfMinDistance = mfMaxDistance/pFrame->mvScaleFactors[nLevels-1]; + + pFrame->mDescriptors.row(idxF).copyTo(mDescriptor); + + // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id. + unique_lock lock(mpMap->mMutexPointCreation); + mnId=nNextId++; +} + +/** + * @brief 设定该mp的世界坐标 + * @param Pos 坐标值 + */ +void MapPoint::SetWorldPos(const Eigen::Vector3f &Pos) +{ + unique_lock lock2(mGlobalMutex); + unique_lock lock(mMutexPos); + mWorldPos = Pos; +} + +/** + * @brief 返回该mp的世界坐标 + */ +Eigen::Vector3f MapPoint::GetWorldPos() +{ + unique_lock lock(mMutexPos); + return mWorldPos; +} + +/** + * @brief 获取平均观测方向 + */ +Eigen::Vector3f MapPoint::GetNormal() +{ + unique_lock lock(mMutexPos); + return mNormalVector; +} + + +KeyFrame* MapPoint::GetReferenceKeyFrame() +{ + unique_lock lock(mMutexFeatures); + return mpRefKF; +} + +/** + * @brief 给地图点添加观测 + * + * 记录哪些 KeyFrame 的那个特征点能观测到该 地图点 + * 并增加观测的相机数目nObs，单目+1，双目或者rgbd+2 + * 这个函数是建立关键帧共视关系的核心函数，能共同观测到某些地图点的关键帧是共视关键帧 + * @param pKF KeyFrame + * @param idx MapPoint在KeyFrame中的索引 + */ +void MapPoint::AddObservation(KeyFrame* pKF, int idx) +{ + unique_lock lock(mMutexFeatures); + tuple indexes; + + if(mObservations.count(pKF)){ + indexes = mObservations[pKF]; + } + else{ + indexes = tuple(-1,-1); + } + + if(pKF -> NLeft != -1 && idx >= pKF -> NLeft){ + get<1>(indexes) = idx; + } + else{ + get<0>(indexes) = idx; + } + + // 如果没有添加过观测，记录下能观测到该MapPoint的KF和该MapPoint在KF中的索引 + mObservations[pKF]=indexes; + + if(!pKF->mpCamera2 && pKF->mvuRight[idx]>=0) + nObs+=2; + else + nObs++; +} + +// 删除某个关键帧对当前地图点的观测 +void MapPoint::EraseObservation(KeyFrame* pKF, bool erase) +{ + bool bBad=false; + { + unique_lock lock(mMutexFeatures); + // 查找这个要删除的观测,根据单目和双目类型的不同从其中删除当前地图点的被观测次数 + if(mObservations.count(pKF)) + { + tuple indexes = mObservations[pKF]; + int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes); + + if(leftIndex != -1){ + if(!pKF->mpCamera2 && pKF->mvuRight[leftIndex]>=0) + nObs-=2; + else + nObs--; + } + if(rightIndex != -1){ + nObs--; + } + + mObservations.erase(pKF); + + // 如果该keyFrame是参考帧，该Frame被删除后重新指定RefFrame + if(mpRefKF==pKF) + mpRefKF=mObservations.begin()->first; + + // If only 2 observations or less, discard point + // 当观测到该点的相机数目少于2时，丢弃该点 + if(nObs<=2) + bBad=true; + } + } + // 告知可以观测到该MapPoint的Frame，该MapPoint已被删除 + if(bBad) + SetBadFlag(erase); +} + +// 能够观测到当前地图点的所有关键帧及该地图点在KF中的索引 +std::map> MapPoint::GetObservations() +{ + unique_lock lock(mMutexFeatures); + return mObservations; +} + +/** + * @brief 返回被观测次数，双目一帧算两次，左右目各算各的 + * @return nObs + */ +int MapPoint::Observations() +{ + unique_lock lock(mMutexFeatures); + return nObs; +} + +/** + * @brief 告知可以观测到该MapPoint的Frame，该MapPoint已被删除 + * + */ +void MapPoint::SetBadFlag(bool erase) +{ + map> obs; + { + unique_lock lock1(mMutexFeatures); + unique_lock lock2(mMutexPos); + mbBad=true; + // 把mObservations转存到obs，obs和mObservations里存的是指针，赋值过程为浅拷贝 + obs = mObservations; + // 把mObservations指向的内存释放，obs作为局部变量之后自动删除 + mObservations.clear(); + } + for(map>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++) + { + KeyFrame* pKF = mit->first; + int leftIndex = get<0>(mit -> second), rightIndex = get<1>(mit -> second); + if(leftIndex != -1){ + pKF->EraseMapPointMatch(leftIndex); + } + if(rightIndex != -1){ + pKF->EraseMapPointMatch(rightIndex); + } + } + + // 擦除该MapPoint申请的内存 + if (erase) + mpMap->EraseMapPoint(this); +} + +/** + * @brief 判断该点是否已经被替换，因为替换并没有考虑普通帧的替换，不利于下一帧的跟踪，所以要坐下标记 + * @return 替换的新的点 + */ +MapPoint* MapPoint::GetReplaced() +{ + unique_lock lock1(mMutexFeatures); + unique_lock lock2(mMutexPos); + return mpReplaced; +} + +/** + * @brief 替换地图点，更新观测关系 + * + * @param[in] pMP 用该地图点来替换当前地图点 + */ +void MapPoint::Replace(MapPoint* pMP) +{ + // 同一个地图点则跳过 + if(pMP->mnId==this->mnId) + return; + + //要替换当前地图点,有两个工作: + // 1. 将当前地图点的观测数据等其他数据都"叠加"到新的地图点上 + // 2. 将观测到当前地图点的关键帧的信息进行更新 + + + // 清除当前地图点的信息，这一段和SetBadFlag函数相同 + int nvisible, nfound; + map> obs; + { + unique_lock lock1(mMutexFeatures); + unique_lock lock2(mMutexPos); + obs=mObservations; + // 清除当前地图点的原有观测 + mObservations.clear(); + // 当前的地图点被删除了 + mbBad=true; + // 暂存当前地图点的可视次数和被找到的次数 + nvisible = mnVisible; + nfound = mnFound; + // 指明当前地图点已经被指定的地图点替换了 + mpReplaced = pMP; + } + + // 所有能观测到原地图点的关键帧都要复制到替换的地图点上 + for(map>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++) + { + // Replace measurement in keyframe + KeyFrame* pKF = mit->first; + + tuple indexes = mit -> second; + int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes); + + // 2.1 判断新点是否已经在pKF里面 + if(!pMP->IsInKeyFrame(pKF)) + { + // 如果不在，替换特征点与mp的匹配关系 + if(leftIndex != -1) + { + pKF->ReplaceMapPointMatch(leftIndex, pMP); + pMP->AddObservation(pKF,leftIndex); + } + if(rightIndex != -1) + { + pKF->ReplaceMapPointMatch(rightIndex, pMP); + pMP->AddObservation(pKF,rightIndex); + } + } + // 如果新的MP在之前MP对应的关键帧里面，就撞车了。 + // 本来目的想新旧MP融为一个，这样以来一个点有可能对应两个特征点，这样是决不允许的，所以删除旧的，不动新的 + else + { + if(leftIndex != -1){ + pKF->EraseMapPointMatch(leftIndex); + } + if(rightIndex != -1){ + pKF->EraseMapPointMatch(rightIndex); + } + } + } + //- 将当前地图点的观测数据等其他数据都"叠加"到新的地图点上 + pMP->IncreaseFound(nfound); + pMP->IncreaseVisible(nvisible); + // 描述子更新 + pMP->ComputeDistinctiveDescriptors(); + + // 告知地图,删掉我 + mpMap->EraseMapPoint(this); +} + +/** + * @brief 判断这个点是否设置为bad了 + */ +bool MapPoint::isBad() +{ + unique_lock lock1(mMutexFeatures,std::defer_lock); + unique_lock lock2(mMutexPos,std::defer_lock); + lock(lock1, lock2); + + return mbBad; +} + +/** + * @brief Increase Visible + * + * Visible表示： + * 1. 该MapPoint在某些帧的视野范围内，通过Frame::isInFrustum()函数判断 + * 2. 该MapPoint被这些帧观测到，但并不一定能和这些帧的特征点匹配上 + * 例如：有一个MapPoint（记为M），在某一帧F的视野范围内， + * 但并不表明该点M可以和F这一帧的某个特征点能匹配上 + */ +void MapPoint::IncreaseVisible(int n) +{ + unique_lock lock(mMutexFeatures); + mnVisible+=n; +} + +/** + * @brief Increase Found + * + * 能找到该点的帧数+n，n默认为1 + * @see Tracking::TrackLocalMap() + */ +void MapPoint::IncreaseFound(int n) +{ + unique_lock lock(mMutexFeatures); + mnFound+=n; +} + +/** + * @brief 返回被找到/被看到 + * @return 被找到/被看到 + */ +float MapPoint::GetFoundRatio() +{ + unique_lock lock(mMutexFeatures); + return static_cast(mnFound)/mnVisible; +} + +/** + * @brief 计算地图点具有代表性的描述子 + * + * 由于一个MapPoint会被许多相机观测到，因此在插入关键帧后，需要判断是否更新当前点的最适合的描述子 + * 先获得当前点的所有描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值 + */ +void MapPoint::ComputeDistinctiveDescriptors() +{ + // Retrieve all observed descriptors + vector vDescriptors; + + map> observations; + + // Step 1 获取所有观测，跳过坏点 + { + unique_lock lock1(mMutexFeatures); + if(mbBad) + return; + observations=mObservations; + } + + if(observations.empty()) + return; + + vDescriptors.reserve(observations.size()); + + // Step 2 遍历观测到3d点的所有关键帧，获得orb描述子，并插入到vDescriptors中 + for(map>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) + { + // mit->first取观测到该地图点的关键帧 + // mit->second取该地图点在关键帧中的索引 + KeyFrame* pKF = mit->first; + + if(!pKF->isBad()) + { + tuple indexes = mit -> second; + int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes); + + if(leftIndex != -1) + { + vDescriptors.push_back(pKF->mDescriptors.row(leftIndex)); + } + if(rightIndex != -1) + { + vDescriptors.push_back(pKF->mDescriptors.row(rightIndex)); + } + } + } + + if(vDescriptors.empty()) + return; + + // Compute distances between them + // Step 3 获得这些描述子两两之间的距离 + // N表示为一共多少个描述子 + const size_t N = vDescriptors.size(); + + float Distances[N][N]; + for(size_t i=0;i vDists(Distances[i],Distances[i]+N); + sort(vDists.begin(),vDists.end()); + // 获得中值 + int median = vDists[0.5*(N-1)]; + + // 寻找最小的中值 + if(median lock(mMutexFeatures); + // 最好的描述子，该描述子相对于其他描述子有最小的距离中值 + // 简化来讲，中值代表了这个描述子到其它描述子的平均距离 + // 最好的描述子就是和其它描述子的平均距离最小 + mDescriptor = vDescriptors[BestIdx].clone(); + } +} + +/** + * @brief 返回描述子 + */ +cv::Mat MapPoint::GetDescriptor() +{ + unique_lock lock(mMutexFeatures); + return mDescriptor.clone(); +} + +/** + * @brief 返回这个点在关键帧中对应的特征点id + */ +tuple MapPoint::GetIndexInKeyFrame(KeyFrame *pKF) +{ + unique_lock lock(mMutexFeatures); + if(mObservations.count(pKF)) + return mObservations[pKF]; + else + return tuple(-1,-1); +} + +/** + * @brief return (mObservations.count(pKF)); + */ +bool MapPoint::IsInKeyFrame(KeyFrame *pKF) +{ + unique_lock lock(mMutexFeatures); + return (mObservations.count(pKF)); +} + +/** + * @brief 更新平均观测方向以及观测距离范围 + * + * 由于一个MapPoint会被许多相机观测到，因此在插入关键帧后，需要更新相应变量 + * 创建新的关键帧的时候会调用 + */ +void MapPoint::UpdateNormalAndDepth() +{ + map> observations; + KeyFrame* pRefKF; + Eigen::Vector3f Pos; + { + unique_lock lock1(mMutexFeatures); + unique_lock lock2(mMutexPos); + if(mbBad) + return; + observations = mObservations; // 获得观测到该地图点的所有关键帧 + pRefKF = mpRefKF; // 观测到该点的参考关键帧（第一次创建时的关键帧） + Pos = mWorldPos; // 地图点在世界坐标系中的位置 + } + + if(observations.empty()) + return; + + // Step 2 计算该地图点的法线方向，也就是朝向等信息。 + // 能观测到该地图点的所有关键帧，对该点的观测方向归一化为单位向量，然后进行求和得到该地图点的朝向 + // 初始值为0向量，累加为归一化向量，最后除以总数n + Eigen::Vector3f normal; + normal.setZero(); + int n = 0; + for(map>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) + { + KeyFrame* pKF = mit->first; + + tuple indexes = mit -> second; + int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes); + + if(leftIndex != -1) + { + Eigen::Vector3f Owi = pKF->GetCameraCenter(); + Eigen::Vector3f normali = Pos - Owi; + normal = normal + normali / normali.norm(); + n++; + } + if(rightIndex != -1) + { + Eigen::Vector3f Owi = pKF->GetRightCameraCenter(); + Eigen::Vector3f normali = Pos - Owi; + normal = normal + normali / normali.norm(); + n++; + } + } + + Eigen::Vector3f PC = Pos - pRefKF->GetCameraCenter(); // 参考关键帧相机指向地图点的向量（在世界坐标系下的表示） + const float dist = PC.norm(); // 该点到参考关键帧相机的距离 + + tuple indexes = observations[pRefKF]; + int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes); + int level; + if(pRefKF -> NLeft == -1) + { + level = pRefKF->mvKeysUn[leftIndex].octave; + } + else if(leftIndex != -1) + { + level = pRefKF -> mvKeys[leftIndex].octave; + } + else{ + level = pRefKF -> mvKeysRight[rightIndex - pRefKF -> NLeft].octave; + } + + //const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave; + const float levelScaleFactor = pRefKF->mvScaleFactors[level]; // 当前金字塔层对应的缩放倍数 + const int nLevels = pRefKF->mnScaleLevels; // 金字塔层数 + + { + unique_lock lock3(mMutexPos); + // 使用方法见PredictScale函数前的注释 + mfMaxDistance = dist*levelScaleFactor; // 观测到该点的距离上限 + mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1]; // 观测到该点的距离下限 + mNormalVector = normal/n; // 获得地图点平均的观测方向 + } +} + +/** + * @brief 设置平均观测方向 + * @param normal 观测方向 + */ +void MapPoint::SetNormalVector(const Eigen::Vector3f& normal) +{ + unique_lock lock3(mMutexPos); + mNormalVector = normal; +} + +/** + * @brief 返回最近距离 + */ +float MapPoint::GetMinDistanceInvariance() +{ + unique_lock lock(mMutexPos); + return 0.8f * mfMinDistance; +} + +/** + * @brief 返回最远距离 + */ +float MapPoint::GetMaxDistanceInvariance() +{ + unique_lock lock(mMutexPos); + return 1.2f * mfMaxDistance; +} + +// 下图中横线的大小表示不同图层图像上的一个像素表示的真实物理空间中的大小 +// ____ +// Nearer /____\ level:n-1 --> dmin +// /______\ d/dmin = 1.2^(n-1-m) +// /________\ level:m --> d +// /__________\ dmax/d = 1.2^m +// Farther /____________\ level:0 --> dmax +// +// log(dmax/d) +// m = ceil(------------) +// log(1.2) +// 这个函数的作用: +// 在进行投影匹配的时候会给定特征点的搜索范围,考虑到处于不同尺度(也就是距离相机远近,位于图像金字塔中不同图层)的特征点受到相机旋转的影响不同, +// 因此会希望距离相机近的点的搜索范围更大一点,距离相机更远的点的搜索范围更小一点,所以要在这里,根据点到关键帧/帧的距离来估计它在当前的关键帧/帧中, +// 会大概处于哪个尺度 +int MapPoint::PredictScale(const float ¤tDist, KeyFrame* pKF) +{ + float ratio; + { + unique_lock lock(mMutexPos); + // mfMaxDistance = ref_dist*levelScaleFactor 为参考帧考虑上尺度后的距离 + // ratio = mfMaxDistance/currentDist = ref_dist/cur_dist + ratio = mfMaxDistance/currentDist; + } + + // 同时取log线性化 + int nScale = ceil(log(ratio)/pKF->mfLogScaleFactor); + if(nScale<0) + nScale = 0; + else if(nScale>=pKF->mnScaleLevels) + nScale = pKF->mnScaleLevels-1; + + return nScale; +} + +/** + * @brief 根据地图点到光心的距离来预测一个类似特征金字塔的尺度 + * + * @param[in] currentDist 地图点到光心的距离 + * @param[in] pF 当前帧 + * @return int 尺度 + */ +int MapPoint::PredictScale(const float ¤tDist, Frame* pF) +{ + float ratio; + { + unique_lock lock(mMutexPos); + ratio = mfMaxDistance/currentDist; + } + + // 同时取log线性化 + int nScale = ceil(log(ratio)/pF->mfLogScaleFactor); + if(nScale<0) + nScale = 0; + else if(nScale>=pF->mnScaleLevels) + nScale = pF->mnScaleLevels-1; + + return nScale; +} + +void MapPoint::PrintObservations() +{ + cout << "MP_OBS: MP " << mnId << endl; + for(map>::iterator mit=mObservations.begin(), mend=mObservations.end(); mit!=mend; mit++) + { + KeyFrame* pKFi = mit->first; + tuple indexes = mit->second; + int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes); + cout << "--OBS in KF " << pKFi->mnId << " in map " << pKFi->GetMap()->GetId() << endl; + } +} + +Map* MapPoint::GetMap() +{ + unique_lock lock(mMutexMap); + return mpMap; +} + +void MapPoint::UpdateMap(Map* pMap) +{ + unique_lock lock(mMutexMap); + mpMap = pMap; +} + +/** + * @brief 预保存 + * @param spKF 地图中所有关键帧 + * @param spMP 地图中所有三维点 + */ +void MapPoint::PreSave(set& spKF,set& spMP) +{ + // 1. 备份替代的MP id + mBackupReplacedId = -1; + if(mpReplaced && spMP.find(mpReplaced) != spMP.end()) + mBackupReplacedId = mpReplaced->mnId; + + // 2. 备份观测 + mBackupObservationsId1.clear(); + mBackupObservationsId2.clear(); + // Save the id and position in each KF who view it + std::vector erase_kfs; + for(std::map >::const_iterator it = mObservations.begin(), + end = mObservations.end(); it != end; ++it) + { + KeyFrame* pKFi = it->first; + if(spKF.find(pKFi) != spKF.end()) + { + mBackupObservationsId1[it->first->mnId] = get<0>(it->second); + mBackupObservationsId2[it->first->mnId] = get<1>(it->second); + } + else + { + erase_kfs.push_back(pKFi); + } + } + for (auto pKFi : erase_kfs) + EraseObservation(pKFi, false); + // Save the id of the reference KF + // 3. 备份参考关键帧ID + if(spKF.find(mpRefKF) != spKF.end()) + { + mBackupRefKFId = mpRefKF->mnId; + } +} + +/** + * @brief 后加载 + */ +void MapPoint::PostLoad(map& mpKFid, map& mpMPid) +{ + // 1. 根据保存的ID加载参考关键帧与替代点 + mpRefKF = mpKFid[mBackupRefKFId]; + if(!mpRefKF) + { + cout << "ERROR: MP without KF reference " << mBackupRefKFId << "; Num obs: " << nObs << endl; + } + mpReplaced = static_cast(NULL); + if(mBackupReplacedId>=0) + { + map::iterator it = mpMPid.find(mBackupReplacedId); + if (it != mpMPid.end()) + mpReplaced = it->second; + } + + // 2. 加载观测 + mObservations.clear(); + + for(map::const_iterator it = mBackupObservationsId1.begin(), end = mBackupObservationsId1.end(); it != end; ++it) + { + KeyFrame* pKFi = mpKFid[it->first]; + map::const_iterator it2 = mBackupObservationsId2.find(it->first); + std::tuple indexes = tuple(it->second,it2->second); + if(pKFi) + { + mObservations[pKFi] = indexes; + } + } + + mBackupObservationsId1.clear(); + mBackupObservationsId2.clear(); +} + +} //namespace ORB_SLAM diff --git a/ORB-SLAM3-UESTC/Workspace/src/ORBextractor.cc b/ORB-SLAM3-UESTC/Workspace/src/ORBextractor.cc new file mode 100644 index 0000000..5a80457 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/ORBextractor.cc @@ -0,0 +1,1742 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ +/** +* Software License Agreement (BSD License) +* +* Copyright (c) 2009, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +*/ + + +#include +#include +#include +#include +#include +#include + +#include "ORBextractor.h" + + +using namespace cv; +using namespace std; + +namespace ORB_SLAM3 +{ +//理解下面代码的过程中需要用到一些知识： +//高斯图像金字塔，参考[https://blog.csdn.net/xbcreal/article/details/52629465] +// +//另外有一篇对这个部分进行简介的帖子，具有很大的参考价值：[https://blog.csdn.net/saber_altolia/article/details/52623513] +// + + +const int PATCH_SIZE = 31; ///<使用灰度质心法计算特征点的方向信息时，图像块的大小,或者说是直径 +const int HALF_PATCH_SIZE = 15; ///<上面这个大小的一半，或者说是半径 +const int EDGE_THRESHOLD = 19; ///<算法生成的图像边 +//生成这个边的目的是进行图像金子塔的生成时，需要对图像进行高斯滤波处理，为了考虑到使滤波后的图像边界处的像素也能够携带有正确的图像信息， +//这里作者就将原图像扩大了一个边。 + +/** + * @brief 这个函数用于计算特征点的方向，这里是返回角度作为方向。 + * 计算特征点方向是为了使得提取的特征点具有旋转不变性。 + * 方法是灰度质心法：以几何中心和灰度质心的连线作为该特征点方向 + * @param[in] image 要进行操作的某层金字塔图像 + * @param[in] pt 当前特征点的坐标 + * @param[in] u_max 图像块的每一行的坐标边界 u_max + * @return float 返回特征点的角度，范围为[0,360)角度，精度为0.3° + */ +static float IC_Angle(const Mat& image, Point2f pt, const vector & u_max) +{ + //图像的矩，前者是按照图像块的y坐标加权，后者是按照图像块的x坐标加权 + int m_01 = 0, m_10 = 0; + + //获得这个特征点所在的图像块的中心点坐标灰度值的指针center + const uchar* center = &image.at (cvRound(pt.y), cvRound(pt.x)); + + // Treat the center line differently, v=0 + //这条v=0中心线的计算需要特殊对待 + //由于是中心行+若干行对，所以PATCH_SIZE应该是个奇数 + for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u) + //注意这里的center下标u可以是负的！中心水平线上的像素按x坐标（也就是u坐标）加权 + m_10 += u * center[u]; + + // Go line by line in the circular patch + //这里的step1表示这个图像一行包含的字节总数。参考[https://blog.csdn.net/qianqing13579/article/details/45318279] + int step = (int)image.step1(); + //注意这里是以v=0中心线为对称轴，然后对称地每成对的两行之间进行遍历，这样处理加快了计算速度 + for (int v = 1; v <= HALF_PATCH_SIZE; ++v) + { + // Proceed over the two lines + //本来m_01应该是一列一列地计算的，但是由于对称以及坐标x,y正负的原因，可以一次计算两行 + int v_sum = 0; + // 获取某行像素横坐标的最大范围，注意这里的图像块是圆形的！ + int d = u_max[v]; + //在坐标范围内挨个像素遍历，实际是一次遍历2个 + // 假设每次处理的两个点坐标，中心线下方为(x,y),中心线上方为(x,-y) + // 对于某次待处理的两个点：m_10 = Σ x*I(x,y) = x*I(x,y) + x*I(x,-y) = x*(I(x,y) + I(x,-y)) + // 对于某次待处理的两个点：m_01 = Σ y*I(x,y) = y*I(x,y) - y*I(x,-y) = y*(I(x,y) - I(x,-y)) + for (int u = -d; u <= d; ++u) + { + //得到需要进行加运算和减运算的像素灰度值 + //val_plus：在中心线下方x=u时的的像素灰度值 + //val_minus：在中心线上方x=u时的像素灰度值 + int val_plus = center[u + v*step], val_minus = center[u - v*step]; + //在v（y轴）上，2行所有像素灰度值之差 + v_sum += (val_plus - val_minus); + //u轴（也就是x轴）方向上用u坐标加权和（u坐标也有正负符号），相当于同时计算两行 + m_10 += u * (val_plus + val_minus); + } + //将这一行上的和按照y坐标加权 + m_01 += v * v_sum; + } + + //为了加快速度还使用了fastAtan2()函数，输出为[0,360)角度，精度为0.3° + return fastAtan2((float)m_01, (float)m_10); +} + +///乘数因子，一度对应着多少弧度 +const float factorPI = (float)(CV_PI/180.f); + +/** + * @brief 计算ORB特征点的描述子。注意这个是全局的静态函数，只能是在本文件内被调用 + * @param[in] kpt 特征点对象 + * @param[in] img 提取出特征点的图像 + * @param[in] pattern 预定义好的随机采样点集 + * @param[out] desc 用作输出变量，保存计算好的描述子，长度为32*8bit + */ +static void computeOrbDescriptor(const KeyPoint& kpt, + const Mat& img, const Point* pattern, + uchar* desc) +{ + //得到特征点的角度，用弧度制表示。kpt.angle是角度制，范围为[0,360)度 + float angle = (float)kpt.angle*factorPI; + //然后计算这个角度的余弦值和正弦值 + float a = (float)cos(angle), b = (float)sin(angle); + + //获得图像中心指针 + const uchar* center = &img.at(cvRound(kpt.pt.y), cvRound(kpt.pt.x)); + //获得图像的每行的字节数 + const int step = (int)img.step; + + //原始的BRIEF描述子不具有方向信息，通过加入特征点的方向来计算描述子，称之为Steer BRIEF，具有较好旋转不变特性 + //具体地，在计算的时候需要将这里选取的随机点点集的x轴方向旋转到特征点的方向。 + //获得随机“相对点集”中某个idx所对应的点的灰度,这里旋转前坐标为(x,y), 旋转后坐标(x',y')推导: + // x'= xcos(θ) - ysin(θ), y'= xsin(θ) + ycos(θ) + #define GET_VALUE(idx) center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + cvRound(pattern[idx].x*a - pattern[idx].y*b)] + // y'* step + // x' + //brief描述子由32*8位组成 + //其中每一位是来自于两个像素点灰度的直接比较，所以每比较出8bit结果，需要16个随机点，这也就是为什么pattern需要+=16的原因 + for (int i = 0; i < 32; ++i, pattern += 16) + { + + int t0, //参与比较的一个特征点的灰度值 + t1, //参与比较的另一个特征点的灰度值 + val; //描述子这个字节的比较结果 + + t0 = GET_VALUE(0); t1 = GET_VALUE(1); + val = t0 < t1; //描述子本字节的bit0 + t0 = GET_VALUE(2); t1 = GET_VALUE(3); + val |= (t0 < t1) << 1; //描述子本字节的bit1 + t0 = GET_VALUE(4); t1 = GET_VALUE(5); + val |= (t0 < t1) << 2; //描述子本字节的bit2 + t0 = GET_VALUE(6); t1 = GET_VALUE(7); + val |= (t0 < t1) << 3; //描述子本字节的bit3 + t0 = GET_VALUE(8); t1 = GET_VALUE(9); + val |= (t0 < t1) << 4; //描述子本字节的bit4 + t0 = GET_VALUE(10); t1 = GET_VALUE(11); + val |= (t0 < t1) << 5; //描述子本字节的bit5 + t0 = GET_VALUE(12); t1 = GET_VALUE(13); + val |= (t0 < t1) << 6; //描述子本字节的bit6 + t0 = GET_VALUE(14); t1 = GET_VALUE(15); + val |= (t0 < t1) << 7; //描述子本字节的bit7 + + //保存当前比较的出来的描述子的这个字节 + desc[i] = (uchar)val; + }//通过对随机点像素灰度的比较，得出BRIEF描述子，一共是32*8=256位 + + //为了避免和程序中的其他部分冲突在，在使用完成之后就取消这个宏定义 + #undef GET_VALUE +} + +//下面就是预先定义好的随机点集，256是指可以提取出256bit的描述子信息，每个bit由一对点比较得来；4=2*2，前面的2是需要两个点（一对点）进行比较，后面的2是一个点有两个坐标 + static int bit_pattern_31_[256*4] = + { + 8,-3, 9,5/*mean (0), correlation (0)*/, + 4,2, 7,-12/*mean (1.12461e-05), correlation (0.0437584)*/, + -11,9, -8,2/*mean (3.37382e-05), correlation (0.0617409)*/, + 7,-12, 12,-13/*mean (5.62303e-05), correlation (0.0636977)*/, + 2,-13, 2,12/*mean (0.000134953), correlation (0.085099)*/, + 1,-7, 1,6/*mean (0.000528565), correlation (0.0857175)*/, + -2,-10, -2,-4/*mean (0.0188821), correlation (0.0985774)*/, + -13,-13, -11,-8/*mean (0.0363135), correlation (0.0899616)*/, + -13,-3, -12,-9/*mean (0.121806), correlation (0.099849)*/, + 10,4, 11,9/*mean (0.122065), correlation (0.093285)*/, + -13,-8, -8,-9/*mean (0.162787), correlation (0.0942748)*/, + -11,7, -9,12/*mean (0.21561), correlation (0.0974438)*/, + 7,7, 12,6/*mean (0.160583), correlation (0.130064)*/, + -4,-5, -3,0/*mean (0.228171), correlation (0.132998)*/, + -13,2, -12,-3/*mean (0.00997526), correlation (0.145926)*/, + -9,0, -7,5/*mean (0.198234), correlation (0.143636)*/, + 12,-6, 12,-1/*mean (0.0676226), correlation (0.16689)*/, + -3,6, -2,12/*mean (0.166847), correlation (0.171682)*/, + -6,-13, -4,-8/*mean (0.101215), correlation (0.179716)*/, + 11,-13, 12,-8/*mean (0.200641), correlation (0.192279)*/, + 4,7, 5,1/*mean (0.205106), correlation (0.186848)*/, + 5,-3, 10,-3/*mean (0.234908), correlation (0.192319)*/, + 3,-7, 6,12/*mean (0.0709964), correlation (0.210872)*/, + -8,-7, -6,-2/*mean (0.0939834), correlation (0.212589)*/, + -2,11, -1,-10/*mean (0.127778), correlation (0.20866)*/, + -13,12, -8,10/*mean (0.14783), correlation (0.206356)*/, + -7,3, -5,-3/*mean (0.182141), correlation (0.198942)*/, + -4,2, -3,7/*mean (0.188237), correlation (0.21384)*/, + -10,-12, -6,11/*mean (0.14865), correlation (0.23571)*/, + 5,-12, 6,-7/*mean (0.222312), correlation (0.23324)*/, + 5,-6, 7,-1/*mean (0.229082), correlation (0.23389)*/, + 1,0, 4,-5/*mean (0.241577), correlation (0.215286)*/, + 9,11, 11,-13/*mean (0.00338507), correlation (0.251373)*/, + 4,7, 4,12/*mean (0.131005), correlation (0.257622)*/, + 2,-1, 4,4/*mean (0.152755), correlation (0.255205)*/, + -4,-12, -2,7/*mean (0.182771), correlation (0.244867)*/, + -8,-5, -7,-10/*mean (0.186898), correlation (0.23901)*/, + 4,11, 9,12/*mean (0.226226), correlation (0.258255)*/, + 0,-8, 1,-13/*mean (0.0897886), correlation (0.274827)*/, + -13,-2, -8,2/*mean (0.148774), correlation (0.28065)*/, + -3,-2, -2,3/*mean (0.153048), correlation (0.283063)*/, + -6,9, -4,-9/*mean (0.169523), correlation (0.278248)*/, + 8,12, 10,7/*mean (0.225337), correlation (0.282851)*/, + 0,9, 1,3/*mean (0.226687), correlation (0.278734)*/, + 7,-5, 11,-10/*mean (0.00693882), correlation (0.305161)*/, + -13,-6, -11,0/*mean (0.0227283), correlation (0.300181)*/, + 10,7, 12,1/*mean (0.125517), correlation (0.31089)*/, + -6,-3, -6,12/*mean (0.131748), correlation (0.312779)*/, + 10,-9, 12,-4/*mean (0.144827), correlation (0.292797)*/, + -13,8, -8,-12/*mean (0.149202), correlation (0.308918)*/, + -13,0, -8,-4/*mean (0.160909), correlation (0.310013)*/, + 3,3, 7,8/*mean (0.177755), correlation (0.309394)*/, + 5,7, 10,-7/*mean (0.212337), correlation (0.310315)*/, + -1,7, 1,-12/*mean (0.214429), correlation (0.311933)*/, + 3,-10, 5,6/*mean (0.235807), correlation (0.313104)*/, + 2,-4, 3,-10/*mean (0.00494827), correlation (0.344948)*/, + -13,0, -13,5/*mean (0.0549145), correlation (0.344675)*/, + -13,-7, -12,12/*mean (0.103385), correlation (0.342715)*/, + -13,3, -11,8/*mean (0.134222), correlation (0.322922)*/, + -7,12, -4,7/*mean (0.153284), correlation (0.337061)*/, + 6,-10, 12,8/*mean (0.154881), correlation (0.329257)*/, + -9,-1, -7,-6/*mean (0.200967), correlation (0.33312)*/, + -2,-5, 0,12/*mean (0.201518), correlation (0.340635)*/, + -12,5, -7,5/*mean (0.207805), correlation (0.335631)*/, + 3,-10, 8,-13/*mean (0.224438), correlation (0.34504)*/, + -7,-7, -4,5/*mean (0.239361), correlation (0.338053)*/, + -3,-2, -1,-7/*mean (0.240744), correlation (0.344322)*/, + 2,9, 5,-11/*mean (0.242949), correlation (0.34145)*/, + -11,-13, -5,-13/*mean (0.244028), correlation (0.336861)*/, + -1,6, 0,-1/*mean (0.247571), correlation (0.343684)*/, + 5,-3, 5,2/*mean (0.000697256), correlation (0.357265)*/, + -4,-13, -4,12/*mean (0.00213675), correlation (0.373827)*/, + -9,-6, -9,6/*mean (0.0126856), correlation (0.373938)*/, + -12,-10, -8,-4/*mean (0.0152497), correlation (0.364237)*/, + 10,2, 12,-3/*mean (0.0299933), correlation (0.345292)*/, + 7,12, 12,12/*mean (0.0307242), correlation (0.366299)*/, + -7,-13, -6,5/*mean (0.0534975), correlation (0.368357)*/, + -4,9, -3,4/*mean (0.099865), correlation (0.372276)*/, + 7,-1, 12,2/*mean (0.117083), correlation (0.364529)*/, + -7,6, -5,1/*mean (0.126125), correlation (0.369606)*/, + -13,11, -12,5/*mean (0.130364), correlation (0.358502)*/, + -3,7, -2,-6/*mean (0.131691), correlation (0.375531)*/, + 7,-8, 12,-7/*mean (0.160166), correlation (0.379508)*/, + -13,-7, -11,-12/*mean (0.167848), correlation (0.353343)*/, + 1,-3, 12,12/*mean (0.183378), correlation (0.371916)*/, + 2,-6, 3,0/*mean (0.228711), correlation (0.371761)*/, + -4,3, -2,-13/*mean (0.247211), correlation (0.364063)*/, + -1,-13, 1,9/*mean (0.249325), correlation (0.378139)*/, + 7,1, 8,-6/*mean (0.000652272), correlation (0.411682)*/, + 1,-1, 3,12/*mean (0.00248538), correlation (0.392988)*/, + 9,1, 12,6/*mean (0.0206815), correlation (0.386106)*/, + -1,-9, -1,3/*mean (0.0364485), correlation (0.410752)*/, + -13,-13, -10,5/*mean (0.0376068), correlation (0.398374)*/, + 7,7, 10,12/*mean (0.0424202), correlation (0.405663)*/, + 12,-5, 12,9/*mean (0.0942645), correlation (0.410422)*/, + 6,3, 7,11/*mean (0.1074), correlation (0.413224)*/, + 5,-13, 6,10/*mean (0.109256), correlation (0.408646)*/, + 2,-12, 2,3/*mean (0.131691), correlation (0.416076)*/, + 3,8, 4,-6/*mean (0.165081), correlation (0.417569)*/, + 2,6, 12,-13/*mean (0.171874), correlation (0.408471)*/, + 9,-12, 10,3/*mean (0.175146), correlation (0.41296)*/, + -8,4, -7,9/*mean (0.183682), correlation (0.402956)*/, + -11,12, -4,-6/*mean (0.184672), correlation (0.416125)*/, + 1,12, 2,-8/*mean (0.191487), correlation (0.386696)*/, + 6,-9, 7,-4/*mean (0.192668), correlation (0.394771)*/, + 2,3, 3,-2/*mean (0.200157), correlation (0.408303)*/, + 6,3, 11,0/*mean (0.204588), correlation (0.411762)*/, + 3,-3, 8,-8/*mean (0.205904), correlation (0.416294)*/, + 7,8, 9,3/*mean (0.213237), correlation (0.409306)*/, + -11,-5, -6,-4/*mean (0.243444), correlation (0.395069)*/, + -10,11, -5,10/*mean (0.247672), correlation (0.413392)*/, + -5,-8, -3,12/*mean (0.24774), correlation (0.411416)*/, + -10,5, -9,0/*mean (0.00213675), correlation (0.454003)*/, + 8,-1, 12,-6/*mean (0.0293635), correlation (0.455368)*/, + 4,-6, 6,-11/*mean (0.0404971), correlation (0.457393)*/, + -10,12, -8,7/*mean (0.0481107), correlation (0.448364)*/, + 4,-2, 6,7/*mean (0.050641), correlation (0.455019)*/, + -2,0, -2,12/*mean (0.0525978), correlation (0.44338)*/, + -5,-8, -5,2/*mean (0.0629667), correlation (0.457096)*/, + 7,-6, 10,12/*mean (0.0653846), correlation (0.445623)*/, + -9,-13, -8,-8/*mean (0.0858749), correlation (0.449789)*/, + -5,-13, -5,-2/*mean (0.122402), correlation (0.450201)*/, + 8,-8, 9,-13/*mean (0.125416), correlation (0.453224)*/, + -9,-11, -9,0/*mean (0.130128), correlation (0.458724)*/, + 1,-8, 1,-2/*mean (0.132467), correlation (0.440133)*/, + 7,-4, 9,1/*mean (0.132692), correlation (0.454)*/, + -2,1, -1,-4/*mean (0.135695), correlation (0.455739)*/, + 11,-6, 12,-11/*mean (0.142904), correlation (0.446114)*/, + -12,-9, -6,4/*mean (0.146165), correlation (0.451473)*/, + 3,7, 7,12/*mean (0.147627), correlation (0.456643)*/, + 5,5, 10,8/*mean (0.152901), correlation (0.455036)*/, + 0,-4, 2,8/*mean (0.167083), correlation (0.459315)*/, + -9,12, -5,-13/*mean (0.173234), correlation (0.454706)*/, + 0,7, 2,12/*mean (0.18312), correlation (0.433855)*/, + -1,2, 1,7/*mean (0.185504), correlation (0.443838)*/, + 5,11, 7,-9/*mean (0.185706), correlation (0.451123)*/, + 3,5, 6,-8/*mean (0.188968), correlation (0.455808)*/, + -13,-4, -8,9/*mean (0.191667), correlation (0.459128)*/, + -5,9, -3,-3/*mean (0.193196), correlation (0.458364)*/, + -4,-7, -3,-12/*mean (0.196536), correlation (0.455782)*/, + 6,5, 8,0/*mean (0.1972), correlation (0.450481)*/, + -7,6, -6,12/*mean (0.199438), correlation (0.458156)*/, + -13,6, -5,-2/*mean (0.211224), correlation (0.449548)*/, + 1,-10, 3,10/*mean (0.211718), correlation (0.440606)*/, + 4,1, 8,-4/*mean (0.213034), correlation (0.443177)*/, + -2,-2, 2,-13/*mean (0.234334), correlation (0.455304)*/, + 2,-12, 12,12/*mean (0.235684), correlation (0.443436)*/, + -2,-13, 0,-6/*mean (0.237674), correlation (0.452525)*/, + 4,1, 9,3/*mean (0.23962), correlation (0.444824)*/, + -6,-10, -3,-5/*mean (0.248459), correlation (0.439621)*/, + -3,-13, -1,1/*mean (0.249505), correlation (0.456666)*/, + 7,5, 12,-11/*mean (0.00119208), correlation (0.495466)*/, + 4,-2, 5,-7/*mean (0.00372245), correlation (0.484214)*/, + -13,9, -9,-5/*mean (0.00741116), correlation (0.499854)*/, + 7,1, 8,6/*mean (0.0208952), correlation (0.499773)*/, + 7,-8, 7,6/*mean (0.0220085), correlation (0.501609)*/, + -7,-4, -7,1/*mean (0.0233806), correlation (0.496568)*/, + -8,11, -7,-8/*mean (0.0236505), correlation (0.489719)*/, + -13,6, -12,-8/*mean (0.0268781), correlation (0.503487)*/, + 2,4, 3,9/*mean (0.0323324), correlation (0.501938)*/, + 10,-5, 12,3/*mean (0.0399235), correlation (0.494029)*/, + -6,-5, -6,7/*mean (0.0420153), correlation (0.486579)*/, + 8,-3, 9,-8/*mean (0.0548021), correlation (0.484237)*/, + 2,-12, 2,8/*mean (0.0616622), correlation (0.496642)*/, + -11,-2, -10,3/*mean (0.0627755), correlation (0.498563)*/, + -12,-13, -7,-9/*mean (0.0829622), correlation (0.495491)*/, + -11,0, -10,-5/*mean (0.0843342), correlation (0.487146)*/, + 5,-3, 11,8/*mean (0.0929937), correlation (0.502315)*/, + -2,-13, -1,12/*mean (0.113327), correlation (0.48941)*/, + -1,-8, 0,9/*mean (0.132119), correlation (0.467268)*/, + -13,-11, -12,-5/*mean (0.136269), correlation (0.498771)*/, + -10,-2, -10,11/*mean (0.142173), correlation (0.498714)*/, + -3,9, -2,-13/*mean (0.144141), correlation (0.491973)*/, + 2,-3, 3,2/*mean (0.14892), correlation (0.500782)*/, + -9,-13, -4,0/*mean (0.150371), correlation (0.498211)*/, + -4,6, -3,-10/*mean (0.152159), correlation (0.495547)*/, + -4,12, -2,-7/*mean (0.156152), correlation (0.496925)*/, + -6,-11, -4,9/*mean (0.15749), correlation (0.499222)*/, + 6,-3, 6,11/*mean (0.159211), correlation (0.503821)*/, + -13,11, -5,5/*mean (0.162427), correlation (0.501907)*/, + 11,11, 12,6/*mean (0.16652), correlation (0.497632)*/, + 7,-5, 12,-2/*mean (0.169141), correlation (0.484474)*/, + -1,12, 0,7/*mean (0.169456), correlation (0.495339)*/, + -4,-8, -3,-2/*mean (0.171457), correlation (0.487251)*/, + -7,1, -6,7/*mean (0.175), correlation (0.500024)*/, + -13,-12, -8,-13/*mean (0.175866), correlation (0.497523)*/, + -7,-2, -6,-8/*mean (0.178273), correlation (0.501854)*/, + -8,5, -6,-9/*mean (0.181107), correlation (0.494888)*/, + -5,-1, -4,5/*mean (0.190227), correlation (0.482557)*/, + -13,7, -8,10/*mean (0.196739), correlation (0.496503)*/, + 1,5, 5,-13/*mean (0.19973), correlation (0.499759)*/, + 1,0, 10,-13/*mean (0.204465), correlation (0.49873)*/, + 9,12, 10,-1/*mean (0.209334), correlation (0.49063)*/, + 5,-8, 10,-9/*mean (0.211134), correlation (0.503011)*/, + -1,11, 1,-13/*mean (0.212), correlation (0.499414)*/, + -9,-3, -6,2/*mean (0.212168), correlation (0.480739)*/, + -1,-10, 1,12/*mean (0.212731), correlation (0.502523)*/, + -13,1, -8,-10/*mean (0.21327), correlation (0.489786)*/, + 8,-11, 10,-6/*mean (0.214159), correlation (0.488246)*/, + 2,-13, 3,-6/*mean (0.216993), correlation (0.50287)*/, + 7,-13, 12,-9/*mean (0.223639), correlation (0.470502)*/, + -10,-10, -5,-7/*mean (0.224089), correlation (0.500852)*/, + -10,-8, -8,-13/*mean (0.228666), correlation (0.502629)*/, + 4,-6, 8,5/*mean (0.22906), correlation (0.498305)*/, + 3,12, 8,-13/*mean (0.233378), correlation (0.503825)*/, + -4,2, -3,-3/*mean (0.234323), correlation (0.476692)*/, + 5,-13, 10,-12/*mean (0.236392), correlation (0.475462)*/, + 4,-13, 5,-1/*mean (0.236842), correlation (0.504132)*/, + -9,9, -4,3/*mean (0.236977), correlation (0.497739)*/, + 0,3, 3,-9/*mean (0.24314), correlation (0.499398)*/, + -12,1, -6,1/*mean (0.243297), correlation (0.489447)*/, + 3,2, 4,-8/*mean (0.00155196), correlation (0.553496)*/, + -10,-10, -10,9/*mean (0.00239541), correlation (0.54297)*/, + 8,-13, 12,12/*mean (0.0034413), correlation (0.544361)*/, + -8,-12, -6,-5/*mean (0.003565), correlation (0.551225)*/, + 2,2, 3,7/*mean (0.00835583), correlation (0.55285)*/, + 10,6, 11,-8/*mean (0.00885065), correlation (0.540913)*/, + 6,8, 8,-12/*mean (0.0101552), correlation (0.551085)*/, + -7,10, -6,5/*mean (0.0102227), correlation (0.533635)*/, + -3,-9, -3,9/*mean (0.0110211), correlation (0.543121)*/, + -1,-13, -1,5/*mean (0.0113473), correlation (0.550173)*/, + -3,-7, -3,4/*mean (0.0140913), correlation (0.554774)*/, + -8,-2, -8,3/*mean (0.017049), correlation (0.55461)*/, + 4,2, 12,12/*mean (0.01778), correlation (0.546921)*/, + 2,-5, 3,11/*mean (0.0224022), correlation (0.549667)*/, + 6,-9, 11,-13/*mean (0.029161), correlation (0.546295)*/, + 3,-1, 7,12/*mean (0.0303081), correlation (0.548599)*/, + 11,-1, 12,4/*mean (0.0355151), correlation (0.523943)*/, + -3,0, -3,6/*mean (0.0417904), correlation (0.543395)*/, + 4,-11, 4,12/*mean (0.0487292), correlation (0.542818)*/, + 2,-4, 2,1/*mean (0.0575124), correlation (0.554888)*/, + -10,-6, -8,1/*mean (0.0594242), correlation (0.544026)*/, + -13,7, -11,1/*mean (0.0597391), correlation (0.550524)*/, + -13,12, -11,-13/*mean (0.0608974), correlation (0.55383)*/, + 6,0, 11,-13/*mean (0.065126), correlation (0.552006)*/, + 0,-1, 1,4/*mean (0.074224), correlation (0.546372)*/, + -13,3, -9,-2/*mean (0.0808592), correlation (0.554875)*/, + -9,8, -6,-3/*mean (0.0883378), correlation (0.551178)*/, + -13,-6, -8,-2/*mean (0.0901035), correlation (0.548446)*/, + 5,-9, 8,10/*mean (0.0949843), correlation (0.554694)*/, + 2,7, 3,-9/*mean (0.0994152), correlation (0.550979)*/, + -1,-6, -1,-1/*mean (0.10045), correlation (0.552714)*/, + 9,5, 11,-2/*mean (0.100686), correlation (0.552594)*/, + 11,-3, 12,-8/*mean (0.101091), correlation (0.532394)*/, + 3,0, 3,5/*mean (0.101147), correlation (0.525576)*/, + -1,4, 0,10/*mean (0.105263), correlation (0.531498)*/, + 3,-6, 4,5/*mean (0.110785), correlation (0.540491)*/, + -13,0, -10,5/*mean (0.112798), correlation (0.536582)*/, + 5,8, 12,11/*mean (0.114181), correlation (0.555793)*/, + 8,9, 9,-6/*mean (0.117431), correlation (0.553763)*/, + 7,-4, 8,-12/*mean (0.118522), correlation (0.553452)*/, + -10,4, -10,9/*mean (0.12094), correlation (0.554785)*/, + 7,3, 12,4/*mean (0.122582), correlation (0.555825)*/, + 9,-7, 10,-2/*mean (0.124978), correlation (0.549846)*/, + 7,0, 12,-2/*mean (0.127002), correlation (0.537452)*/, + -1,-6, 0,-11/*mean (0.127148), correlation (0.547401)*/ + }; + +//特征点提取器的构造函数 + +ORBextractor::ORBextractor(int _nfeatures, //指定要提取的特征点数目 + float _scaleFactor, //指定图像金字塔的缩放系数 + int _nlevels, //指定图像金字塔的层数 + int _iniThFAST, //指定初始的FAST特征点提取参数，可以提取出最明显的角点 + int _minThFAST): //如果因为图像纹理不丰富提取出的特征点不多，为了达到想要的特征点数目， + //就使用这个参数提取出不是那么明显的角点 + nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels), + iniThFAST(_iniThFAST), minThFAST(_minThFAST)//设置这些参数 +{ + //存储每层图像缩放系数的vector调整为符合图层数目的大小 + mvScaleFactor.resize(nlevels); + //存储这个sigma^2，其实就是每层图像相对初始图像缩放因子的平方 + mvLevelSigma2.resize(nlevels); + //对于初始图像，这两个参数都是1 + mvScaleFactor[0]=1.0f; + mvLevelSigma2[0]=1.0f; + //然后逐层计算图像金字塔中图像相当于初始图像的缩放系数 + for(int i=1; i= vmin; --v) + { + while (umax[v0] == umax[v0 + 1]) + ++v0; + umax[v] = v0; + ++v0; + } +} + + +/** + * @brief 计算特征点的方向 + * @param[in] image 特征点所在当前金字塔的图像 + * @param[in & out] keypoints 特征点向量 + * @param[in] umax 每个特征点所在图像区块的每行的边界 u_max 组成的vector + */ +static void computeOrientation(const Mat& image, vector& keypoints, const vector& umax) +{ + // 遍历所有的特征点 + for (vector::iterator keypoint = keypoints.begin(), + keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint) + { + // 调用IC_Angle 函数计算这个特征点的方向 + keypoint->angle = IC_Angle(image, //特征点所在的图层的图像 + keypoint->pt, //特征点在这张图像中的坐标 + umax); //每个特征点所在图像区块的每行的边界 u_max 组成的vector + } +} + + +/** + * @brief 将提取器节点分成4个子节点，同时也完成图像区域的划分、特征点归属的划分，以及相关标志位的置位 + * + * @param[in & out] n1 提取器节点1：左上 + * @param[in & out] n2 提取器节点1：右上 + * @param[in & out] n3 提取器节点1：左下 + * @param[in & out] n4 提取器节点1：右下 + */ +void ExtractorNode::DivideNode(ExtractorNode &n1, + ExtractorNode &n2, + ExtractorNode &n3, + ExtractorNode &n4) +{ + //得到当前提取器节点所在图像区域的一半长宽，当然结果需要取整 + const int halfX = ceil(static_cast(UR.x-UL.x)/2); + const int halfY = ceil(static_cast(BR.y-UL.y)/2); + + //Define boundaries of childs + //下面的操作大同小异，将一个图像区域再细分成为四个小图像区块 + //n1 存储左上区域的边界 + n1.UL = UL; + n1.UR = cv::Point2i(UL.x+halfX,UL.y); + n1.BL = cv::Point2i(UL.x,UL.y+halfY); + n1.BR = cv::Point2i(UL.x+halfX,UL.y+halfY); + //用来存储在该节点对应的图像网格中提取出来的特征点的vector + n1.vKeys.reserve(vKeys.size()); + + //n2 存储右上区域的边界 + n2.UL = n1.UR; + n2.UR = UR; + n2.BL = n1.BR; + n2.BR = cv::Point2i(UR.x,UL.y+halfY); + n2.vKeys.reserve(vKeys.size()); + + //n3 存储左下区域的边界 + n3.UL = n1.BL; + n3.UR = n1.BR; + n3.BL = BL; + n3.BR = cv::Point2i(n1.BR.x,BL.y); + n3.vKeys.reserve(vKeys.size()); + + //n4 存储右下区域的边界 + n4.UL = n3.UR; + n4.UR = n2.BR; + n4.BL = n3.BR; + n4.BR = BR; + n4.vKeys.reserve(vKeys.size()); + + //Associate points to childs + //遍历当前提取器节点的vkeys中存储的特征点 + for(size_t i=0;i& e1, pair& e2) +{ + if(e1.first < e2.first) + { + return true; + } + else if(e1.first > e2.first) + { + return false; + } + else + { + if(e1.second->UL.x < e2.second->UL.x) + { + return true; + } + else + { + return false; + } + } +} + +/** + * @brief 使用四叉树法对一个图像金字塔图层中的特征点进行平均和分发 + * + * @param[in] vToDistributeKeys 等待进行分配到四叉树中的特征点 + * @param[in] minX 当前图层的图像的边界，坐标都是在“半径扩充图像”坐标系下的坐标 + * @param[in] maxX + * @param[in] minY + * @param[in] maxY + * @param[in] N 希望提取出的特征点个数 + * @param[in] level 指定的金字塔图层，并未使用 + * @return vector 已经均匀分散好的特征点vector容器 + */ +vector ORBextractor::DistributeOctTree(const vector& vToDistributeKeys, const int &minX, + const int &maxX, const int &minY, const int &maxY, const int &N, const int &level) +{ + // Compute how many initial nodes + // Step 1 根据宽高比确定初始节点数目 + //计算应该生成的初始节点个数，根节点的数量nIni是根据边界的宽高比值确定的，一般是1或者2 + // ! bug: 如果宽高比小于0.5，nIni=0, 后面hx会报错 + const int nIni = round(static_cast(maxX-minX)/(maxY-minY)); + + //一个初始的节点的x方向有多少个像素 + const float hX = static_cast(maxX-minX)/nIni; + + //存储有提取器节点的列表 + list lNodes; + + //存储初始提取器节点指针的vector + vector vpIniNodes; + + //然后重新设置其大小 + vpIniNodes.resize(nIni); + + // Step 2 生成初始提取器节点 + for(int i=0; i(i),0); //UpLeft + ni.UR = cv::Point2i(hX*static_cast(i+1),0); //UpRight + ni.BL = cv::Point2i(ni.UL.x,maxY-minY); //BottomLeft + ni.BR = cv::Point2i(ni.UR.x,maxY-minY); //BottomRight + + //重设vkeys大小 + ni.vKeys.reserve(vToDistributeKeys.size()); + + //将刚才生成的提取节点添加到列表中 + //虽然这里的ni是局部变量，但是由于这里的push_back()是拷贝参数的内容到一个新的对象中然后再添加到列表中 + //所以当本函数退出之后这里的内存不会成为“野指针” + lNodes.push_back(ni); + //存储这个初始的提取器节点句柄 + vpIniNodes[i] = &lNodes.back(); + } + + //Associate points to childs + // Step 3 将特征点分配到子提取器节点中 + for(size_t i=0;ivKeys.push_back(kp); + } + + // Step 4 遍历此提取器节点列表，标记那些不可再分裂的节点，删除那些没有分配到特征点的节点 + // ? 这个步骤是必要的吗？感觉可以省略，通过判断nIni个数和vKeys.size() 就可以吧 + list::iterator lit = lNodes.begin(); + while(lit!=lNodes.end()) + { + //如果初始的提取器节点所分配到的特征点个数为1 + if(lit->vKeys.size()==1) + { + //那么就标志位置位，表示此节点不可再分 + lit->bNoMore=true; + //更新迭代器 + lit++; + } + ///如果一个提取器节点没有被分配到特征点，那么就从列表中直接删除它 + else if(lit->vKeys.empty()) + //注意，由于是直接删除了它，所以这里的迭代器没有必要更新；否则反而会造成跳过元素的情况 + lit = lNodes.erase(lit); + else + //如果上面的这些情况和当前的特征点提取器节点无关，那么就只是更新迭代器 + lit++; + } + + //结束标志位清空 + bool bFinish = false; + + //记录迭代次数，只是记录，并未起到作用 + int iteration = 0; + + //声明一个vector用于存储节点的vSize和句柄对 + //这个变量记录了在一次分裂循环中，那些可以再继续进行分裂的节点中包含的特征点数目和其句柄 + vector > vSizeAndPointerToNode; + + //调整大小，这里的意思是一个初始化节点将“分裂”成为四个，当然实际上不会有那么多，这里多分配了一些只是预防万一 + vSizeAndPointerToNode.reserve(lNodes.size()*4); + + // Step 5 根据兴趣点分布,利用4叉树方法对图像进行划分区域 + while(!bFinish) + { + //更新迭代次数计数器，只是记录，并未起到作用 + iteration++; + + //保存当前节点个数，prev在这里理解为“保留”比较好 + int prevSize = lNodes.size(); + + //重新定位迭代器指向列表头部 + lit = lNodes.begin(); + + //需要展开的节点计数，这个一直保持累计，不清零 + int nToExpand = 0; + + //因为是在循环中，前面的循环体中可能污染了这个变量，so清空这个vector + //这个变量也只是统计了某一个循环中的点 + //这个变量记录了在一次分裂循环中，那些可以再继续进行分裂的节点中包含的特征点数目和其句柄 + vSizeAndPointerToNode.clear(); + + // 将目前的子区域进行划分 + //开始遍历列表中所有的提取器节点，并进行分解或者保留 + while(lit!=lNodes.end()) + { + //如果提取器节点只有一个特征点， + if(lit->bNoMore) + { + // If node only contains one point do not subdivide and continue + //那么就没有必要再进行细分了 + lit++; + //跳过当前节点，继续下一个 + continue; + } + else + { + // If more than one point, subdivide + //如果当前的提取器节点具有超过一个的特征点，那么就要进行继续细分 + ExtractorNode n1,n2,n3,n4; + + //再细分成四个子区域 + lit->DivideNode(n1,n2,n3,n4); + + // Add childs if they contain points + //如果这里分出来的子区域中有特征点，那么就将这个子区域的节点添加到提取器节点的列表中 + //注意这里的条件是，有特征点即可 + if(n1.vKeys.size()>0) + { + //注意这里也是添加到列表前面的 + lNodes.push_front(n1); + + //再判断其中子提取器节点中的特征点数目是否大于1 + if(n1.vKeys.size()>1) + { + //如果有超过一个的特征点，那么“待展开的节点计数++” + nToExpand++; + + //保存这个特征点数目和节点指针的信息 + vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front())); + + //?这个访问用的句柄貌似并没有用到？ + // lNodes.front().lit 和前面的迭代的lit 不同，只是名字相同而已 + // lNodes.front().lit是node结构体里的一个指针用来记录节点的位置 + // 迭代的lit 是while循环里作者命名的遍历的指针名称 + lNodes.front().lit = lNodes.begin(); + } + } + //后面的操作都是相同的，这里不再赘述 + if(n2.vKeys.size()>0) + { + lNodes.push_front(n2); + if(n2.vKeys.size()>1) + { + nToExpand++; + vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front())); + lNodes.front().lit = lNodes.begin(); + } + } + if(n3.vKeys.size()>0) + { + lNodes.push_front(n3); + if(n3.vKeys.size()>1) + { + nToExpand++; + vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front())); + lNodes.front().lit = lNodes.begin(); + } + } + if(n4.vKeys.size()>0) + { + lNodes.push_front(n4); + if(n4.vKeys.size()>1) + { + nToExpand++; + vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front())); + lNodes.front().lit = lNodes.begin(); + } + } + + //当这个母节点expand之后就从列表中删除它了，能够进行分裂操作说明至少有一个子节点的区域中特征点的数量是>1的 + //? 分裂方式是后加的先分裂，先加的后分裂。 + lit=lNodes.erase(lit); + + //继续下一次循环，其实这里加不加这句话的作用都是一样的 + continue; + }//判断当前遍历到的节点中是否有超过一个的特征点 + }//遍历列表中的所有提取器节点 + + // Finish if there are more nodes than required features or all nodes contain just one point + //停止这个过程的条件有两个，满足其中一个即可： + //1、当前的节点数已经超过了要求的特征点数 + //2、当前所有的节点中都只包含一个特征点 + if((int)lNodes.size()>=N //判断是否超过了要求的特征点数 + || (int)lNodes.size()==prevSize) //prevSize中保存的是分裂之前的节点个数，如果分裂之前和分裂之后的总节点个数一样，说明当前所有的 + //节点区域中只有一个特征点，已经不能够再细分了 + { + //停止标志置位 + bFinish = true; + } + + // Step 6 当再划分之后所有的Node数大于要求数目时,就慢慢划分直到使其刚刚达到或者超过要求的特征点个数 + //可以展开的子节点个数nToExpand x3，是因为一分四之后，会删除原来的主节点，所以乘以3 + /** + * //?BUG 但是我觉得这里有BUG，虽然最终作者也给误打误撞、稀里糊涂地修复了 + * 注意到，这里的nToExpand变量在前面的执行过程中是一直处于累计状态的，如果因为特征点个数太少，跳过了下面的else-if，又进行了一次上面的遍历 + * list的操作之后，lNodes.size()增加了，但是nToExpand也增加了，尤其是在很多次操作之后，下面的表达式： + * ((int)lNodes.size()+nToExpand*3)>N + * 会很快就被满足，但是此时只进行一次对vSizeAndPointerToNode中点进行分裂的操作是肯定不够的； + * 理想中，作者下面的for理论上只要执行一次就能满足，不过作者所考虑的“不理想情况”应该是分裂后出现的节点所在区域可能没有特征点，因此将for + * 循环放在了一个while循环里面，通过再次进行for循环、再分裂一次解决这个问题。而我所考虑的“不理想情况”则是因为前面的一次对vSizeAndPointerToNode + * 中的特征点进行for循环不够，需要将其放在另外一个循环（也就是作者所写的while循环）中不断尝试直到达到退出条件。 + * */ + else if(((int)lNodes.size()+nToExpand*3)>N) + { + //如果再分裂一次那么数目就要超了，这里想办法尽可能使其刚刚达到或者超过要求的特征点个数时就退出 + //这里的nToExpand和vSizeAndPointerToNode不是一次循环对一次循环的关系，而是前者是累计计数，后者只保存某一个循环的 + //一直循环，直到结束标志位被置位 + while(!bFinish) + { + //获取当前的list中的节点个数 + prevSize = lNodes.size(); + + //Prev这里是应该是保留的意思吧，保留那些还可以分裂的节点的信息, 这里是深拷贝 + vector > vPrevSizeAndPointerToNode = vSizeAndPointerToNode; + //清空 + vSizeAndPointerToNode.clear(); + + // 对需要划分的节点进行排序，对pair对的第一个元素进行排序，默认是从小到大排序 + // 优先分裂特征点多的节点，使得特征点密集的区域保留更少的特征点 + //! 注意这里的排序规则非常重要！会导致每次最后产生的特征点都不一样。建议使用 stable_sort + sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end(),compareNodes); + + //遍历这个存储了pair对的vector，注意是从后往前遍历 + for(int j=vPrevSizeAndPointerToNode.size()-1;j>=0;j--) + { + ExtractorNode n1,n2,n3,n4; + //对每个需要进行分裂的节点进行分裂 + vPrevSizeAndPointerToNode[j].second->DivideNode(n1,n2,n3,n4); + + // Add childs if they contain points + //其实这里的节点可以说是二级子节点了，执行和前面一样的操作 + if(n1.vKeys.size()>0) + { + lNodes.push_front(n1); + if(n1.vKeys.size()>1) + { + //因为这里还有对于vSizeAndPointerToNode的操作，所以前面才会备份vSizeAndPointerToNode中的数据 + //为可能的、后续的又一次for循环做准备 + vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front())); + lNodes.front().lit = lNodes.begin(); + } + } + if(n2.vKeys.size()>0) + { + lNodes.push_front(n2); + if(n2.vKeys.size()>1) + { + vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front())); + lNodes.front().lit = lNodes.begin(); + } + } + if(n3.vKeys.size()>0) + { + lNodes.push_front(n3); + if(n3.vKeys.size()>1) + { + vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front())); + lNodes.front().lit = lNodes.begin(); + } + } + if(n4.vKeys.size()>0) + { + lNodes.push_front(n4); + if(n4.vKeys.size()>1) + { + vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front())); + lNodes.front().lit = lNodes.begin(); + } + } + + //删除母节点，在这里其实应该是一级子节点 + lNodes.erase(vPrevSizeAndPointerToNode[j].second->lit); + + //判断是是否超过了需要的特征点数？是的话就退出，不是的话就继续这个分裂过程，直到刚刚达到或者超过要求的特征点个数 + //作者的思想其实就是这样的，再分裂了一次之后判断下一次分裂是否会超过N，如果不是那么就放心大胆地全部进行分裂（因为少了一个判断因此 + //其运算速度会稍微快一些），如果会那么就引导到这里进行最后一次分裂 + if((int)lNodes.size()>=N) + break; + }//遍历vPrevSizeAndPointerToNode并对其中指定的node进行分裂，直到刚刚达到或者超过要求的特征点个数 + + //这里理想中应该是一个for循环就能够达成结束条件了，但是作者想的可能是，有些子节点所在的区域会没有特征点，因此很有可能一次for循环之后 + //的数目还是不能够满足要求，所以还是需要判断结束条件并且再来一次 + //判断是否达到了停止条件 + if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize) + bFinish = true; + }//一直进行不进行nToExpand累加的节点分裂过程，直到分裂后的nodes数目刚刚达到或者超过要求的特征点数目 + }//当本次分裂后达不到结束条件但是再进行一次完整的分裂之后就可以达到结束条件时 + }// 根据兴趣点分布,利用4叉树方法对图像进行划分区域 + + // Retain the best point in each node + // Step 7 保留每个区域响应值最大的一个兴趣点 + //使用这个vector来存储我们感兴趣的特征点的过滤结果 + vector vResultKeys; + + //调整大小为要提取的特征点数目 + vResultKeys.reserve(nfeatures); + + //遍历这个节点列表 + for(list::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++) + { + //得到这个节点区域中的特征点容器句柄 + vector &vNodeKeys = lit->vKeys; + + //得到指向第一个特征点的指针，后面作为最大响应值对应的关键点 + cv::KeyPoint* pKP = &vNodeKeys[0]; + + //用第1个关键点响应值初始化最大响应值 + float maxResponse = pKP->response; + + //开始遍历这个节点区域中的特征点容器中的特征点，注意是从1开始哟，0已经用过了 + for(size_t k=1;kmaxResponse) + { + //更新pKP指向具有最大响应值的keypoints + pKP = &vNodeKeys[k]; + maxResponse = vNodeKeys[k].response; + } + } + + //将这个节点区域中的响应值最大的特征点加入最终结果容器 + vResultKeys.push_back(*pKP); + } + + //返回最终结果容器，其中保存有分裂出来的区域中，我们最感兴趣、响应值最大的特征点 + return vResultKeys; +} + + +//计算四叉树的特征点，函数名字后面的OctTree只是说明了在过滤和分配特征点时所使用的方式 +void ORBextractor::ComputeKeyPointsOctTree( + vector >& allKeypoints) //所有的特征点，这里第一层vector存储的是某图层里面的所有特征点， + //第二层存储的是整个图像金字塔中的所有图层里面的所有特征点 +{ + //重新调整图像层数 + allKeypoints.resize(nlevels); + + //图像cell的尺寸，是个正方形，可以理解为边长in像素坐标 + const float W = 35; + + // 对每一层图像做处理 + //遍历所有图像 + for (int level = 0; level < nlevels; ++level) + { + //计算这层图像的坐标边界， NOTICE 注意这里是坐标边界，EDGE_THRESHOLD指的应该是可以提取特征点的有效图像边界，后面会一直使用“有效图像边界“这个自创名词 + const int minBorderX = EDGE_THRESHOLD-3; //这里的3是因为在计算FAST特征点的时候，需要建立一个半径为3的圆 + const int minBorderY = minBorderX; //minY的计算就可以直接拷贝上面的计算结果了 + const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3; + const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3; + + //存储需要进行平均分配的特征点 + vector vToDistributeKeys; + //一般地都是过量采集，所以这里预分配的空间大小是nfeatures*10 + vToDistributeKeys.reserve(nfeatures*10); + + //计算进行特征点提取的图像区域尺寸 + const float width = (maxBorderX-minBorderX); + const float height = (maxBorderY-minBorderY); + + //计算网格在当前层的图像有的行数和列数 + const int nCols = width/W; + const int nRows = height/W; + //计算每个图像网格所占的像素行数和列数 + const int wCell = ceil(width/nCols); + const int hCell = ceil(height/nRows); + + //开始遍历图像网格，还是以行开始遍历的 + for(int i=0; i=maxBorderY-3) + //那么就跳过这一行 + continue; + //如果图像的大小导致不能够正好划分出来整齐的图像网格，那么就要委屈最后一行了 + if(maxY>maxBorderY) + maxY = maxBorderY; + + //开始列的遍历 + for(int j=0; j=maxBorderX-6) + continue; + //如果最大坐标越界那么委屈一下 + if(maxX>maxBorderX) + maxX = maxBorderX; + + // FAST提取兴趣点, 自适应阈值 + //这个向量存储这个cell中的特征点 + vector vKeysCell; + //调用opencv的库函数来检测FAST角点 + FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), //待检测的图像，这里就是当前遍历到的图像块 + vKeysCell, //存储角点位置的容器 + iniThFAST, //检测阈值 + true); //使能非极大值抑制 + + //如果这个图像块中使用默认的FAST检测阈值没有能够检测到角点 + if(vKeysCell.empty()) + { + //那么就使用更低的阈值来进行重新检测 + FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), //待检测的图像 + vKeysCell, //存储角点位置的容器 + minThFAST, //更低的检测阈值 + true); //使能非极大值抑制 + } + + //当图像cell中检测到FAST角点的时候执行下面的语句 + if(!vKeysCell.empty()) + { + //遍历其中的所有FAST角点 + for(vector::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++) + { + //NOTICE 到目前为止，这些角点的坐标都是基于图像cell的，现在我们要先将其恢复到当前的【坐标边界】下的坐标 + //这样做是因为在下面使用八叉树法整理特征点的时候将会使用得到这个坐标 + //在后面将会被继续转换成为在当前图层的扩充图像坐标系下的坐标 + (*vit).pt.x+=j*wCell; + (*vit).pt.y+=i*hCell; + //然后将其加入到”等待被分配“的特征点容器中 + vToDistributeKeys.push_back(*vit); + }//遍历图像cell中的所有的提取出来的FAST角点，并且恢复其在整个金字塔当前层图像下的坐标 + }//当图像cell中检测到FAST角点的时候执行下面的语句 + }//开始遍历图像cell的列 + }//开始遍历图像cell的行 + + //声明一个对当前图层的特征点的容器的引用 + vector & keypoints = allKeypoints[level]; + //并且调整其大小为欲提取出来的特征点个数（当然这里也是扩大了的，因为不可能所有的特征点都是在这一个图层中提取出来的） + keypoints.reserve(nfeatures); + + // 根据mnFeatuvector & keypoints = allKeypoints[level];resPerLevel,即该层的兴趣点数,对特征点进行剔除 + //返回值是一个保存有特征点的vector容器，含有剔除后的保留下来的特征点 + //得到的特征点的坐标，依旧是在当前图层下来讲的 + keypoints = DistributeOctTree(vToDistributeKeys, //当前图层提取出来的特征点，也即是等待剔除的特征点 + //NOTICE 注意此时特征点所使用的坐标都是在“半径扩充图像”下的 + minBorderX, maxBorderX, //当前图层图像的边界，而这里的坐标却都是在“边缘扩充图像”下的 + minBorderY, maxBorderY, + mnFeaturesPerLevel[level], //希望保留下来的当前层图像的特征点个数 + level); //当前层图像所在的图层 + + //PATCH_SIZE是对于底层的初始图像来说的，现在要根据当前图层的尺度缩放倍数进行缩放得到缩放后的PATCH大小 和特征点的方向计算有关 + const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level]; + + // Add border to coordinates and scale information + //获取剔除过程后保留下来的特征点数目 + const int nkps = keypoints.size(); + //然后开始遍历这些特征点，恢复其在当前图层图像坐标系下的坐标 + for(int i=0; i > &allKeypoints) //输出，所有图层上的所有特征点 +{ + //根据图像金字塔的图层数调整这个变量中的图层数 + allKeypoints.resize(nlevels); + + //计算底层图像的长宽比，其实也就是所有图像的长宽比 + float imageRatio = (float)mvImagePyramid[0].cols/mvImagePyramid[0].rows; + + //开始遍历所有图层的图像 + for (int level = 0; level < nlevels; ++level) + { + //获取每层图像希望提取出来的特征点 + const int nDesiredFeatures = mnFeaturesPerLevel[level]; + + //计算当前图层中应该有几行cell和几行row + //NOTICE 其实下面都应该注意的是最好是使用显式强制转换或者直接使用取整函数。经过查阅资料，发现下面的浮点型数据到整型数据的转换规则 + //是直接保留浮点数据的整数部分 + //TODO 不解，为什么是这样子计算本层图像中图像cell的行数和列数。而且这个数目和希望提取的特征点个数有关 + const int levelCols = sqrt((float)nDesiredFeatures/(5*imageRatio)); + //按照图像比例计算，省事 + const int levelRows = imageRatio*levelCols; + + //这里指的应该是FAST角点可以存在的坐标位置范围，其实就是原始图像的坐标范围 + //注意这里没有提前进行+3的操作，而是在后面计算每个网格的区域的时候使用-3的操作来处理FAST角点半径问题 + //本质上和前面的思想是一样的 + const int minBorderX = EDGE_THRESHOLD; + const int minBorderY = minBorderX; + const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD; + const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD; + + //计算这个容许坐标区域的宽度和高度 + const int W = maxBorderX - minBorderX; + const int H = maxBorderY - minBorderY; + //同时计算每个图像cell的宽度和高度 + const int cellW = ceil((float)W/levelCols); + const int cellH = ceil((float)H/levelRows); + + //计算本层图像中的总cell个数 + const int nCells = levelRows*levelCols; + //ceil:返回大于或者等于表达式的最小整数，向上取整 + //这里计算了每个cell中需要提取出来的特征点数量，由于存在小数取整问题，所以都是往多了取整 + const int nfeaturesCell = ceil((float)nDesiredFeatures/nCells); + + //以方便查找的格式存储从图像cell中提取出来的特征点， + //第三层vector-当前cell中的特征点向量 + //第二层vector-包含了一行cell中，每个cell的上面的特征点容器 + //第一层vector-包含了所有行的，存储“第二层vector”的容器 + vector > > cellKeyPoints(levelRows, vector >(levelCols)); + + //每个cell中应该保留的特征点数量 + vector > nToRetain(levelRows,vector(levelCols,0)); + //每个cell中实际提取出来的特征点的数量 + vector > nTotal(levelRows,vector(levelCols,0)); + //每个cell中是否只提取出来了一个特征点的标记 + vector > bNoMore(levelRows,vector(levelCols,false)); + //保存每一个cell图像的x起始坐标和y起始坐标 + vector iniXCol(levelCols); + vector iniYRow(levelRows); + + //TODO 那些只有一个特征点的图像cell的计数？ + int nNoMore = 0; + //存储需要进行分裂的图像cell计数 + int nToDistribute = 0; + + //考虑到提取FAST特征点的图像半径时，计算cell边界所使用的增量 + //+6=+3+3，代表这起始侧和终止侧提取FAST特征点时的那个3个像素的圆半径 + //也就是说，这里的cellH指的是有效的、FAST角点可以存在的图像长度 + float hY = cellH + 6; + + //开始逐行遍历网格 + for(int i=0; i0,即 + * maxBorderY+3-iniY>0 + * 而前面计算iniY时，实际上考虑到了提取FAST角点所用到的半径为3pixels的半径，所以已经减去了3，这里为了方便分析，不妨 + * 设原始的图像网格的起始坐标为borderY，那么就有： + * iniY=boderY-3 + * 整理可得，使得最后一行计算有意义的要求为： + * borderY < maxBorderY+6 + * 然而根据程序前面的计算，这里的borderY是不应该超过maxBorderY的，所以，这里的这个比较是否是没有必要呢？ + */ + + if(hY<=0) + continue; + } + + //计算从起始的行位置到终止的行位置的坐标增量 + //+6=+3+3,前面的+3是弥补iniY相对于minBorderY + i*cellH的-3，后面的+3则是在minBorderY + （i+1）*cellH + //的基础上，又+的3来表示考虑到提取FAST特征点的半径为3的圆 + float hX = cellW + 6; + + //接下来开始遍历一行中每个列的图像cell + for(int j=0; jnfeaturesCell) + { + //那么就将需要保留的特征点数目设置为我们想要这个cell具有的特征点数目 + nToRetain[i][j] = nfeaturesCell; + //TODO 不太明白这里的用意，是想说明这个数目已经超了吗？ + //目前来看这里的意思是认为这个网格中已经不再需要更多的点来补充了 + bNoMore[i][j] = false; + } + else + { + //如果没有满足，那么也只能保留目前已经有的特征点数目 + nToRetain[i][j] = nKeys; + //需要进行分配到这里的特征点数目就是当前提取到的特征点数目和应该提取的数目之间差值，可以理解为特征点个数的缺口 + //累加 + nToDistribute += nfeaturesCell-nKeys; + //置位表示这个cell真正提取到的特征点数目没有被满足，即需要更多的点 + bNoMore[i][j] = true; + //计数++ + nNoMore++; + }//判断从当前cell中提取出来的特征点数目是否符合我们的预期要求 + }//接下来开始遍历一行中每个列的图像cell + }//接下来开始遍历一行中每个列的图像cell + + + // Retain by score + //根据评分数值决定哪个特征点将会被留下 + //进行的条件： + //1、有需要匀给少特征点的cell的特征点数 + //2、不是所有的cell都nomore了 + //反着说，这个过程停止的条件一是没有需要在进行均匀的特征点个数，二是当前次循环时所有的特征点数目都不足以补充特征点个数的缺口了 + while(nToDistribute>0 && nNoMorenNewFeaturesCell) + { + //如果满足的话，那么就重新设置要保存的特征点数量 + nToRetain[i][j] = nNewFeaturesCell; + //因为减去在本轮要求的特征点数量之后还有剩余，所以这里设置为false + bNoMore[i][j] = false; + } + else + { + //如果不幸不能够满足在本轮中的特征点数目要求，那么也只好将就了 + //只保存能够提取出来的特征点数目 + nToRetain[i][j] = nTotal[i][j]; + //这部分特征点数目的缺口也只好由其他的部分来补充了，因此还是累加到这个变量中 + nToDistribute += nNewFeaturesCell-nTotal[i][j]; + //这个cell中确实没有能够满足本轮，以及可能的、后面的几轮的特征点数目要求了 + bNoMore[i][j] = true; + //这个计数++ + nNoMore++; + }//如果当前遍历到的这个点不能够满足在本轮中的特征点数目要求 + }//判断上一轮时这个cell是否满足了要提取的特征点数目 + }//遍历一行中每个列中的图像cell + }//遍历每行cell + }//判断是否达到了停止条件 + + //请注意，执行到这里，只是完成了每个图像cell中特征点的提取+每个cell中应该保留的特征点数量，下面才是正式地开始对每个 + //cell中vector中的特征点进行删减——或者说是过滤 + + //声明一个对当前图层中特征点vector容器的引用 + vector & keypoints = allKeypoints[level]; + //预分配2倍的期望特征点空间（因为实际的提取过程中，我们都是按照稍微超过期望值的特征点数目进行操作的） + keypoints.reserve(nDesiredFeatures*2); + + //计算在本层图像的时候，图像patch块经过尺度缩放之后的大小（这里的缩放因子就是自己正常认为的缩放因子） + //TODO 这里的patch用于进行什么操作？又为什么一定要在当前的图层上进行而不是在底层图像上进行？怀疑这个操作是和计算特征点方向有关 + //的变量，但是目前还是没有在本文件中发现对这个变量的使用 + const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level]; + + // Retain by score and transform coordinates + //根据响应值保留符合要求的特征点，并且进行坐标的转换 + //开始遍历每行的cell + for(int i=0; i &keysCell = cellKeyPoints[i][j]; + //这里是调用了opencv函数，根据特征点的评分（也就是响应值），保留一个cell中指定数量的特征点 + KeyPointsFilter::retainBest(keysCell, //输入输出，用于提供待保留的特征点vector，操作完成后将保留的特征点存放在里面 + //其实就是把那些响应值低的点从vector中给删除了 + nToRetain[i][j]); //指定要保留的特征点数目 + //如果由于小数取整等原因（前面都是向多了取整的），【对于当前的cell】，经过去除之后的特征点数目还是大于之前设定的要保留的特征点数目 + if((int)keysCell.size()>nToRetain[i][j]) + //那么就强制丢弃vector后面的特征点以满足保留的特征点数目要求 + //TODO 注意，目前所找到的opencv的相关资料中，并没有介绍是否是调用retainBest完成了特征点的过滤之后， + //这里vector中的特征点就已经按照响应值的大小进行了排序.不过不管是否是已经经过了排序，这里最终的结果都是 + //一样的，也就是都将特征点个数缩减到了所希望的数目，不过就是删除的点的是否是“不太那么好的点”而已 + keysCell.resize(nToRetain[i][j]); + + //遍历剔除后的特征点vector，进行坐标的转换，以及添加相关的信息 + //NOTICE 这里的特征点还只是一个cell中的，其坐标也是在一个cell下的坐标 + for(size_t k=0, kend=keysCell.size(); knDesiredFeatures) + { + //那么就再调用opencv的库函数对这组特征点按照其响应值进行一次剔除 + KeyPointsFilter::retainBest(keypoints,nDesiredFeatures); + //并且强制丢弃还是超过期望结果的特征点 + keypoints.resize(nDesiredFeatures); + }//如果最终结果中，本层图像的的特征点数目还是大于希望的，那么就再剔除一次 + }//遍历每个图层 + + // and compute orientations + //最后计算这些特征点的方向信息 + //遍历图像金字塔中的每个图层 + for (int level = 0; level < nlevels; ++level) + //计算这个图层所有特征点的方向信息 + computeOrientation(mvImagePyramid[level], //这个图层的图像 + allKeypoints[level], //这个图层的特征点对象vector容器 + umax); //patch区域的边界 +} + +//注意这是一个不属于任何类的全局静态函数，static修饰符限定其只能够被本文件中的函数调用 +/** + * @brief 计算某层金字塔图像上特征点的描述子 + * + * @param[in] image 某层金字塔图像 + * @param[in] keypoints 特征点vector容器 + * @param[out] descriptors 描述子 + * @param[in] pattern 计算描述子使用的固定随机点集 + */ +static void computeDescriptors(const Mat& image, vector& keypoints, Mat& descriptors, + const vector& pattern) +{ + //清空保存描述子信息的容器 + descriptors = Mat::zeros((int)keypoints.size(), 32, CV_8UC1); + + //开始遍历特征点 + for (size_t i = 0; i < keypoints.size(); i++) + //计算这个特征点的描述子 + computeOrbDescriptor(keypoints[i], //要计算描述子的特征点 + image, //以及其图像 + &pattern[0], //随机点集的首地址 + descriptors.ptr((int)i)); //提取出来的描述子的保存位置 +} + +/** + * @brief 用仿函数（重载括号运算符）方法来计算图像特征点 + * + * @param[in] _image 输入原始图的图像 + * @param[in] _mask 掩膜mask + * @param[in & out] _keypoints 存储特征点关键点的向量 + * @param[in & out] _descriptors 存储特征点描述子的矩阵 + */ + int ORBextractor::operator()( InputArray _image, InputArray _mask, vector& _keypoints, + OutputArray _descriptors, std::vector &vLappingArea) + { + // Step 1 检查图像有效性。如果图像为空，那么就直接返回 + if(_image.empty()) + return -1; + + //获取图像的大小 + Mat image = _image.getMat(); + //判断图像的格式是否正确，要求是单通道灰度值 + assert(image.type() == CV_8UC1 ); + + // Pre-compute the scale pyramid + // Step 2 构建图像金字塔 + ComputePyramid(image); + + // Step 3 计算图像的特征点，并且将特征点进行均匀化。均匀的特征点可以提高位姿计算精度 + // 存储所有的特征点，注意此处为二维的vector，第一维存储的是金字塔的层数，第二维存储的是那一层金字塔图像里提取的所有特征点 + vector < vector > allKeypoints; + //使用四叉树的方式计算每层图像的特征点并进行分配 + ComputeKeyPointsOctTree(allKeypoints); + + //使用传统的方法提取并平均分配图像的特征点，作者并未使用 + //ComputeKeyPointsOld(allKeypoints); + + + // Step 4 拷贝图像描述子到新的矩阵descriptors + Mat descriptors; + + //统计整个图像金字塔中的特征点 + int nkeypoints = 0; + //开始遍历每层图像金字塔，并且累加每层的特征点个数 + for (int level = 0; level < nlevels; ++level) + nkeypoints += (int)allKeypoints[level].size(); + + //如果本图像金字塔中没有任何的特征点 + if( nkeypoints == 0 ) + //通过调用cv::mat类的.realse方法，强制清空矩阵的引用计数，这样就可以强制释放矩阵的数据了 + //参考[https://blog.csdn.net/giantchen547792075/article/details/9107877] + _descriptors.release(); + else + { + //如果图像金字塔中有特征点，那么就创建这个存储描述子的矩阵，注意这个矩阵是存储整个图像金字塔中特征点的描述子的 + _descriptors.create(nkeypoints, //矩阵的行数，对应为特征点的总个数 + 32, //矩阵的列数，对应为使用32*8=256位描述子 + CV_8U); //矩阵元素的格式 + //获取这个描述子的矩阵信息 + // ?为什么不是直接在参数_descriptors上对矩阵内容进行修改，而是重新新建了一个变量，复制矩阵后，在这个新建变量的基础上进行修改？ + descriptors = _descriptors.getMat(); + } + + //_keypoints.clear(); + //_keypoints.reserve(nkeypoints); + _keypoints = vector(nkeypoints); + //因为遍历是一层一层进行的，但是描述子那个矩阵是存储整个图像金字塔中特征点的描述子，所以在这里设置了Offset变量来保存“寻址”时的偏移量， + //辅助进行在总描述子mat中的定位 + int offset = 0; + //Modified for speeding up stereo fisheye matching + int monoIndex = 0, stereoIndex = nkeypoints-1; + for (int level = 0; level < nlevels; ++level) + { + //获取在allKeypoints中当前层特征点容器的句柄 + vector& keypoints = allKeypoints[level]; + //本层的特征点数 + int nkeypointsLevel = (int)keypoints.size(); + + if(nkeypointsLevel==0) + continue; + + // preprocess the resized image + // Step 5 对图像进行高斯模糊 + // 深拷贝当前金字塔所在层级的图像 + Mat workingMat = mvImagePyramid[level].clone(); + + // 注意：提取特征点的时候，使用的是清晰的原图像；这里计算描述子的时候，为了避免图像噪声的影响，使用了高斯模糊 + GaussianBlur(workingMat, //源图像 + workingMat, //输出图像 + Size(7, 7), //高斯滤波器kernel大小，必须为正的奇数 + 2, //高斯滤波在x方向的标准差 + 2, //高斯滤波在y方向的标准差 + BORDER_REFLECT_101);//边缘拓展点插值类型 + + // Compute the descriptors + // desc存储当前图层的描述子 + //Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel); + Mat desc = cv::Mat(nkeypointsLevel, 32, CV_8U); + // Step 6 计算高斯模糊后图像的描述子 + computeDescriptors(workingMat, //高斯模糊之后的图层图像 + keypoints, //当前图层中的特征点集合 + desc, //存储计算之后的描述子 + pattern); //随机采样点集 + + // 更新偏移量的值 + offset += nkeypointsLevel; + + // Scale keypoint coordinates + // Step 6 对非第0层图像中的特征点的坐标恢复到第0层图像（原图像）的坐标系下 + // ? 得到所有层特征点在第0层里的坐标放到_keypoints里面 + // 对于第0层的图像特征点，他们的坐标就不需要再进行恢复了 + float scale = mvScaleFactor[level]; //getScale(level, firstLevel, scaleFactor); + int i = 0; + for (vector::iterator keypoint = keypoints.begin(), + keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint){ + + // Scale keypoint coordinates + if (level != 0){ + // 特征点本身直接乘缩放倍数就可以了 + keypoint->pt *= scale; + } + // ?TODO vLappingArea + if(keypoint->pt.x >= vLappingArea[0] && keypoint->pt.x <= vLappingArea[1]){ + _keypoints.at(stereoIndex) = (*keypoint); + desc.row(i).copyTo(descriptors.row(stereoIndex)); + stereoIndex--; + } + else{ + _keypoints.at(monoIndex) = (*keypoint); + desc.row(i).copyTo(descriptors.row(monoIndex)); + monoIndex++; + } + i++; + } + } + //cout << "[ORBextractor]: extracted " << _keypoints.size() << " KeyPoints" << endl; + return monoIndex; + } + /** + * 构建图像金字塔 + * @param image 输入原图像，这个输入图像所有像素都是有效的，也就是说都是可以在其上提取出FAST角点的 + */ + void ORBextractor::ComputePyramid(cv::Mat image) + { + for (int level = 0; level < nlevels; ++level) + { + float scale = mvInvScaleFactor[level]; + Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale)); + Size wholeSize(sz.width + EDGE_THRESHOLD*2, sz.height + EDGE_THRESHOLD*2); + Mat temp(wholeSize, image.type()), masktemp; + mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height)); + + // Compute the resized image + //计算第0层以上resize后的图像 + if( level != 0 ) + { + //将上一层金字塔图像根据设定sz缩放到当前层级 + resize(mvImagePyramid[level-1], //输入图像 + mvImagePyramid[level], //输出图像 + sz, //输出图像的尺寸 + 0, //水平方向上的缩放系数，留0表示自动计算 + 0, //垂直方向上的缩放系数，留0表示自动计算 + cv::INTER_LINEAR); //图像缩放的差值算法类型，这里的是线性插值算法 + + //把源图像拷贝到目的图像的中央，四面填充指定的像素。图片如果已经拷贝到中间，只填充边界 + //TODO 貌似这样做是因为在计算描述子前，进行高斯滤波的时候，图像边界会导致一些问题，说不明白 + //EDGE_THRESHOLD指的这个边界的宽度，由于这个边界之外的像素不是原图像素而是算法生成出来的，所以不能够在EDGE_THRESHOLD之外提取特征点 + copyMakeBorder(mvImagePyramid[level], //源图像 + temp, //目标图像（此时其实就已经有大了一圈的尺寸了） + EDGE_THRESHOLD, EDGE_THRESHOLD, //top & bottom 需要扩展的border大小 + EDGE_THRESHOLD, EDGE_THRESHOLD, //left & right 需要扩展的border大小 + BORDER_REFLECT_101+BORDER_ISOLATED); //扩充方式，opencv给出的解释： + + /*Various border types, image boundaries are denoted with '|' + * BORDER_REPLICATE: aaaaaa|abcdefgh|hhhhhhh + * BORDER_REFLECT: fedcba|abcdefgh|hgfedcb + * BORDER_REFLECT_101: gfedcb|abcdefgh|gfedcba + * BORDER_WRAP: cdefgh|abcdefgh|abcdefg + * BORDER_CONSTANT: iiiiii|abcdefgh|iiiiiii with some specified 'i' + */ + + //BORDER_ISOLATED 表示对整个图像进行操作 + // https://docs.opencv.org/3.4.4/d2/de8/group__core__array.html#ga2ac1049c2c3dd25c2b41bffe17658a36 + + } + else + { + //对于底层图像，直接就扩充边界了 + //?temp 是在循环内部新定义的，在该函数里又作为输出，并没有使用啊！ + copyMakeBorder(image, //这里是原图像 + temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, + BORDER_REFLECT_101); + } + } + + } + +} //namespace ORB_SLAM diff --git a/ORB-SLAM3-UESTC/Workspace/src/ORBmatcher.cc b/ORB-SLAM3-UESTC/Workspace/src/ORBmatcher.cc new file mode 100644 index 0000000..16daa97 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/ORBmatcher.cc @@ -0,0 +1,2405 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#include "ORBmatcher.h" + +#include + +#include + +#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" + +#include + +using namespace std; + +namespace ORB_SLAM3 +{ + + const int ORBmatcher::TH_HIGH = 100; + const int ORBmatcher::TH_LOW = 50; + const int ORBmatcher::HISTO_LENGTH = 30; + + // 构造函数,参数默认值为0.6,true + ORBmatcher::ORBmatcher(float nnratio, bool checkOri): mfNNratio(nnratio), mbCheckOrientation(checkOri) + { + } + + // 用于Tracking::SearchLocalPoints中匹配更多地图点 + int ORBmatcher::SearchByProjection(Frame &F, const vector &vpMapPoints, const float th, const bool bFarPoints, const float thFarPoints) + { + int nmatches=0, left = 0, right = 0; + + // 如果 th！=1 (RGBD 相机或者刚刚进行过重定位), 需要扩大范围搜索 + const bool bFactor = th!=1.0; + + // Step 1 遍历有效的局部地图点 + for(size_t iMP=0; iMPmbTrackInView && !pMP->mbTrackInViewR) + continue; + + if(bFarPoints && pMP->mTrackDepth>thFarPoints) + continue; + + if(pMP->isBad()) + continue; + + if(pMP->mbTrackInView) + { + // 通过距离预测的金字塔层数，该层数相对于当前的帧 + const int &nPredictedLevel = pMP->mnTrackScaleLevel; + + // The size of the window will depend on the viewing direction + // Step 2 设定搜索搜索窗口的大小。取决于视角, 若当前视角和平均视角夹角较小时, r取一个较小的值 + float r = RadiusByViewingCos(pMP->mTrackViewCos); + + // 如果需要扩大范围搜索，则乘以阈值th + if(bFactor) + r*=th; + + // Step 3 通过投影点以及搜索窗口和预测的尺度进行搜索, 找出搜索半径内的候选匹配点索引 + const vector vIndices = + F.GetFeaturesInArea(pMP->mTrackProjX,pMP->mTrackProjY, // 该地图点投影到一帧上的坐标 + r*F.mvScaleFactors[nPredictedLevel], // 认为搜索窗口的大小和该特征点被追踪到时所处的尺度也有关系 + nPredictedLevel-1,nPredictedLevel); // 搜索的图层范围 + + // 没找到候选的,就放弃对当前点的匹配 + if(!vIndices.empty()){ + const cv::Mat MPdescriptor = pMP->GetDescriptor(); + + // 最优的次优的描述子距离和index + int bestDist=256; + int bestLevel= -1; + int bestDist2=256; + int bestLevel2 = -1; + int bestIdx =-1 ; + + // Get best and second matches with near keypoints + // Step 4 寻找候选匹配点中的最佳和次佳匹配点 + for(vector::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++) + { + const size_t idx = *vit; + + // 如果Frame中的该兴趣点已经有对应的MapPoint了,则退出该次循环 + if(F.mvpMapPoints[idx]) + if(F.mvpMapPoints[idx]->Observations()>0) + continue; + + //如果是双目数据 + if(F.Nleft == -1 && F.mvuRight[idx]>0) + { + //计算在X轴上的投影误差 + const float er = fabs(pMP->mTrackProjXR-F.mvuRight[idx]); + //超过阈值,说明这个点不行,丢掉. + //这里的阈值定义是以给定的搜索范围r为参考,然后考虑到越近的点(nPredictedLevel越大), 相机运动时对其产生的影响也就越大, + //因此需要扩大其搜索空间. + //当给定缩放倍率为1.2的时候, mvScaleFactors 中的数据是: 1 1.2 1.2^2 1.2^3 ... + if(er>r*F.mvScaleFactors[nPredictedLevel]) + continue; + } + + const cv::Mat &d = F.mDescriptors.row(idx); + + // 计算地图点和候选投影点的描述子距离 + const int dist = DescriptorDistance(MPdescriptor,d); + + // 寻找描述子距离最小和次小的特征点和索引 + if(distmfNNratio*bestDist2 表示最佳和次佳距离不满足阈值比例。理论来说 bestDist/bestDist2 越小越好 + if(bestLevel==bestLevel2 && bestDist>mfNNratio*bestDist2) + continue; + + if(bestLevel!=bestLevel2 || bestDist<=mfNNratio*bestDist2){ + F.mvpMapPoints[bestIdx]=pMP; + + if(F.Nleft != -1 && F.mvLeftToRightMatch[bestIdx] != -1){ //Also match with the stereo observation at right camera + F.mvpMapPoints[F.mvLeftToRightMatch[bestIdx] + F.Nleft] = pMP; + nmatches++; + right++; + } + + nmatches++; + left++; + } + } + } + } + + if(F.Nleft != -1 && pMP->mbTrackInViewR){ + const int &nPredictedLevel = pMP->mnTrackScaleLevelR; + if(nPredictedLevel != -1){ + float r = RadiusByViewingCos(pMP->mTrackViewCosR); + + const vector vIndices = + F.GetFeaturesInArea(pMP->mTrackProjXR,pMP->mTrackProjYR,r*F.mvScaleFactors[nPredictedLevel],nPredictedLevel-1,nPredictedLevel,true); + + if(vIndices.empty()) + continue; + + const cv::Mat MPdescriptor = pMP->GetDescriptor(); + + int bestDist=256; + int bestLevel= -1; + int bestDist2=256; + int bestLevel2 = -1; + int bestIdx =-1 ; + + // Get best and second matches with near keypoints + for(vector::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++) + { + const size_t idx = *vit; + + if(F.mvpMapPoints[idx + F.Nleft]) + if(F.mvpMapPoints[idx + F.Nleft]->Observations()>0) + continue; + + + const cv::Mat &d = F.mDescriptors.row(idx + F.Nleft); + + const int dist = DescriptorDistance(MPdescriptor,d); + + if(distmfNNratio*bestDist2) + continue; + + if(F.Nleft != -1 && F.mvRightToLeftMatch[bestIdx] != -1){ //Also match with the stereo observation at right camera + F.mvpMapPoints[F.mvRightToLeftMatch[bestIdx]] = pMP; + nmatches++; + left++; + } + + + F.mvpMapPoints[bestIdx + F.Nleft]=pMP; + nmatches++; + right++; + } + } + } + } + return nmatches; + } + + // 根据观察的视角来计算匹配的时的搜索窗口大小 + float ORBmatcher::RadiusByViewingCos(const float &viewCos) + { + // 当视角相差小于3.6°，对应cos(3.6°)=0.998，搜索范围是2.5，否则是4 + if(viewCos>0.998) + return 2.5; + else + return 4.0; + } + + /** + * @brief 通过词袋，对关键帧的特征点进行跟踪 + * + * @param[in] pKF 关键帧 + * @param[in] F 当前普通帧 + * @param[in] vpMapPointMatches F中地图点对应的匹配，NULL表示未匹配 + * @return int 成功匹配的数量 + */ + int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector &vpMapPointMatches) + { + // 获取该关键帧的地图点 + const vector vpMapPointsKF = pKF->GetMapPointMatches(); + + // 和普通帧F特征点的索引一致 + vpMapPointMatches = vector(F.N,static_cast(NULL)); + + // 取出关键帧的词袋特征向量 + const DBoW2::FeatureVector &vFeatVecKF = pKF->mFeatVec; + + int nmatches=0; + + // 特征点角度旋转差统计用的直方图 + vector rotHist[HISTO_LENGTH]; + for(int i=0;ifirst == Fit->first) + { + // second 是该node内存储的feature index + const vector vIndicesKF = KFit->second; + const vector vIndicesF = Fit->second; + + // Step 2：遍历KF中属于该node的特征点 + for(size_t iKF=0; iKFisBad()) + continue; + // 取出关键帧KF中该特征对应的描述子 + const cv::Mat &dKF= pKF->mDescriptors.row(realIdxKF); + + int bestDist1=256; // 最好的距离（最小距离） + int bestIdxF =-1 ; + int bestDist2=256; // 次好距离（倒数第二小距离） + + int bestDist1R=256; + int bestIdxFR =-1 ; + int bestDist2R=256; + // Step 3：遍历F中属于该node的特征点，寻找最佳匹配点 + for(size_t iF=0; iF= F.Nleft && dist= F.Nleft && dist(bestDist1)(bestDist2)) + { + // Step 4.3：记录成功匹配特征点的对应的地图点(来自关键帧) + vpMapPointMatches[bestIdxF]=pMP; + + // 这里的realIdxKF是当前遍历到的关键帧的特征点id + const cv::KeyPoint &kp = + (!pKF->mpCamera2) ? pKF->mvKeysUn[realIdxKF] : + (realIdxKF >= pKF -> NLeft) ? pKF -> mvKeysRight[realIdxKF - pKF -> NLeft] + : pKF -> mvKeys[realIdxKF]; + // Step 4.4：计算匹配点旋转角度差所在的直方图 + if(mbCheckOrientation) + { + cv::KeyPoint &Fkp = + (!pKF->mpCamera2 || F.Nleft == -1) ? F.mvKeys[bestIdxF] : + (bestIdxF >= F.Nleft) ? F.mvKeysRight[bestIdxF - F.Nleft] + : F.mvKeys[bestIdxF]; + // 所有的特征点的角度变化应该是一致的，通过直方图统计得到最准确的角度变化值 + float rot = kp.angle-Fkp.angle; + if(rot<0.0) + rot+=360.0f; + int bin = round(rot*factor);// 将rot分配到bin组, 四舍五入, 其实就是离散到对应的直方图组中 + if(bin==HISTO_LENGTH) + bin=0; + assert(bin>=0 && bin(bestDist1R)(bestDist2R) || true) + { + vpMapPointMatches[bestIdxFR]=pMP; + + const cv::KeyPoint &kp = + (!pKF->mpCamera2) ? pKF->mvKeysUn[realIdxKF] : + (realIdxKF >= pKF -> NLeft) ? pKF -> mvKeysRight[realIdxKF - pKF -> NLeft] + : pKF -> mvKeys[realIdxKF]; + + if(mbCheckOrientation) + { + cv::KeyPoint &Fkp = + (!F.mpCamera2) ? F.mvKeys[bestIdxFR] : + (bestIdxFR >= F.Nleft) ? F.mvKeysRight[bestIdxFR - F.Nleft] + : F.mvKeys[bestIdxFR]; + + float rot = kp.angle-Fkp.angle; + if(rot<0.0) + rot+=360.0f; + int bin = round(rot*factor); + if(bin==HISTO_LENGTH) + bin=0; + assert(bin>=0 && binfirst < Fit->first) + { + // 对齐 + KFit = vFeatVecKF.lower_bound(Fit->first); + } + else + { + // 对齐 + Fit = F.mFeatVec.lower_bound(KFit->first); + } + } + + // Step 5 根据方向剔除误匹配的点 + if(mbCheckOrientation) + { + // index + int ind1=-1; + int ind2=-1; + int ind3=-1; + + // 筛选出在旋转角度差落在在直方图区间内数量最多的前三个bin的索引 + ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3); + + for(int i=0; i(NULL); + nmatches--; + } + } + } + + return nmatches; + } + + int ORBmatcher::SearchByProjection(KeyFrame* pKF, Sophus::Sim3f &Scw, const vector &vpPoints, + vector &vpMatched, int th, float ratioHamming) + { + // Get Calibration Parameters for later projection + const float &fx = pKF->fx; + const float &fy = pKF->fy; + const float &cx = pKF->cx; + const float &cy = pKF->cy; + + // Decompose Scw + // Step 1 分解Sim变换矩阵 + //? 为什么要剥离尺度信息？ + Sophus::SE3f Tcw = Sophus::SE3f(Scw.rotationMatrix(),Scw.translation()/Scw.scale()); + Eigen::Vector3f Ow = Tcw.inverse().translation(); // 世界坐标系下相机光心坐标 + + // Set of MapPoints already found in the KeyFrame + // 使用set类型，记录前面已经成功的匹配关系，避免重复匹配。并去除其中无效匹配关系（NULL） + set spAlreadyFound(vpMatched.begin(), vpMatched.end()); + spAlreadyFound.erase(static_cast(NULL)); + + int nmatches=0; + + // For each Candidate MapPoint Project and Match + // Step 2 遍历闭环KF及其共视KF的所有地图点（不考虑当前KF已经匹配的地图点）投影到当前KF + for(int iMP=0, iendMP=vpPoints.size(); iMPisBad() || spAlreadyFound.count(pMP)) + continue; + + // Get 3D Coords. + // Step 2.2 投影到当前KF的图像坐标并判断是否有效 + Eigen::Vector3f p3Dw = pMP->GetWorldPos(); + + // Transform into Camera Coords. + Eigen::Vector3f p3Dc = Tcw * p3Dw; + + // Depth must be positive + // 深度值必须为正 + if(p3Dc(2)<0.0) + continue; + + // Project into Image + const Eigen::Vector2f uv = pKF->mpCamera->project(p3Dc); + + // Point must be inside the image + // 在图像范围内 + if(!pKF->IsInImage(uv(0),uv(1))) + continue; + + // Depth must be inside the scale invariance region of the point + // 判断距离是否在有效距离内 + const float maxDistance = pMP->GetMaxDistanceInvariance(); + const float minDistance = pMP->GetMinDistanceInvariance(); + // 地图点到相机光心的向量 + Eigen::Vector3f PO = p3Dw-Ow; + const float dist = PO.norm(); + + if(distmaxDistance) + continue; + + // Viewing angle must be less than 60 deg + // 观察角度小于60° + Eigen::Vector3f Pn = pMP->GetNormal(); + + if(PO.dot(Pn)<0.5*dist) + continue; + + // 根据当前这个地图点距离当前KF光心的距离,预测该点在当前KF中的尺度(图层) + int nPredictedLevel = pMP->PredictScale(dist,pKF); + + // Search in a radius + // 根据尺度确定搜索半径 + const float radius = th*pKF->mvScaleFactors[nPredictedLevel]; + + // Step 2.3 搜索候选匹配点 + const vector vIndices = pKF->GetFeaturesInArea(uv(0),uv(1),radius); + + if(vIndices.empty()) + continue; + + // Match to the most similar keypoint in the radius + const cv::Mat dMP = pMP->GetDescriptor(); + + int bestDist = 256; + int bestIdx = -1; + // Step 2.4 遍历候选匹配点，找到最佳匹配点 + for(vector::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++) + { + const size_t idx = *vit; + if(vpMatched[idx]) + continue; + + const int &kpLevel= pKF->mvKeysUn[idx].octave; + + // 不在一个尺度也不行 + if(kpLevelnPredictedLevel) + continue; + + const cv::Mat &dKF = pKF->mDescriptors.row(idx); + + const int dist = DescriptorDistance(dMP,dKF); + + if(dist &Scw, const std::vector &vpPoints, const std::vector &vpPointsKFs, + std::vector &vpMatched, std::vector &vpMatchedKF, int th, float ratioHamming) + { + // Get Calibration Parameters for later projection + const float &fx = pKF->fx; + const float &fy = pKF->fy; + const float &cx = pKF->cx; + const float &cy = pKF->cy; + + Sophus::SE3f Tcw = Sophus::SE3f(Scw.rotationMatrix(),Scw.translation()/Scw.scale()); + Eigen::Vector3f Ow = Tcw.inverse().translation(); + + // Set of MapPoints already found in the KeyFrame + set spAlreadyFound(vpMatched.begin(), vpMatched.end()); + spAlreadyFound.erase(static_cast(NULL)); + + int nmatches=0; + + // For each Candidate MapPoint Project and Match + for(int iMP=0, iendMP=vpPoints.size(); iMPisBad() || spAlreadyFound.count(pMP)) + continue; + + // Get 3D Coords. + Eigen::Vector3f p3Dw = pMP->GetWorldPos(); + + // Transform into Camera Coords. + Eigen::Vector3f p3Dc = Tcw * p3Dw; + + // Depth must be positive + if(p3Dc(2)<0.0) + continue; + + // Project into Image + const float invz = 1/p3Dc(2); + const float x = p3Dc(0)*invz; + const float y = p3Dc(1)*invz; + + const float u = fx*x+cx; + const float v = fy*y+cy; + + // Point must be inside the image + if(!pKF->IsInImage(u,v)) + continue; + + // Depth must be inside the scale invariance region of the point + const float maxDistance = pMP->GetMaxDistanceInvariance(); + const float minDistance = pMP->GetMinDistanceInvariance(); + Eigen::Vector3f PO = p3Dw-Ow; + const float dist = PO.norm(); + + if(distmaxDistance) + continue; + + // Viewing angle must be less than 60 deg + Eigen::Vector3f Pn = pMP->GetNormal(); + + if(PO.dot(Pn)<0.5*dist) + continue; + + int nPredictedLevel = pMP->PredictScale(dist,pKF); + + // Search in a radius + const float radius = th*pKF->mvScaleFactors[nPredictedLevel]; + + const vector vIndices = pKF->GetFeaturesInArea(u,v,radius); + + if(vIndices.empty()) + continue; + + // Match to the most similar keypoint in the radius + const cv::Mat dMP = pMP->GetDescriptor(); + + int bestDist = 256; + int bestIdx = -1; + for(vector::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++) + { + const size_t idx = *vit; + if(vpMatched[idx]) + continue; + + const int &kpLevel= pKF->mvKeysUn[idx].octave; + + if(kpLevelnPredictedLevel) + continue; + + const cv::Mat &dKF = pKF->mDescriptors.row(idx); + + const int dist = DescriptorDistance(dMP,dKF); + + if(dist &vbPrevMatched, vector &vnMatches12, int windowSize) + { + int nmatches=0; + // F1中特征点和F2中匹配关系，注意是按照F1特征点数目分配空间 + vnMatches12 = vector(F1.mvKeysUn.size(),-1); + + // Step 1 构建旋转直方图，HISTO_LENGTH = 30 + vector rotHist[HISTO_LENGTH]; + // 每个bin里预分配500个，因为使用的是vector不够的话可以自动扩展容量 + for(int i=0;i vMatchedDistance(F2.mvKeysUn.size(),INT_MAX); + // 从帧2到帧1的反向匹配，注意是按照F2特征点数目分配空间 + vector vnMatches21(F2.mvKeysUn.size(),-1); + + // 遍历帧1中的所有特征点 + for(size_t i1=0, iend1=F1.mvKeysUn.size(); i10) + continue; + + // Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 + // vbPrevMatched 输入的是参考帧 F1的特征点 + // windowSize = 100，输入最大最小金字塔层级 均为0 + vector vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1); + + // 没有候选特征点，跳过 + if(vIndices2.empty()) + continue; + + // 取出参考帧F1中当前遍历特征点对应的描述子 + cv::Mat d1 = F1.mDescriptors.row(i1); + + int bestDist = INT_MAX; //最佳描述子匹配距离，越小越好 + int bestDist2 = INT_MAX; //次佳描述子匹配距离 + int bestIdx2 = -1; //最佳候选特征点在F2中的index + + // Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的 + for(vector::iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++) + { + size_t i2 = *vit; + // 取出候选特征点对应的描述子 + cv::Mat d2 = F2.mDescriptors.row(i2); + // 计算两个特征点描述子距离 + int dist = DescriptorDistance(d1,d2); + + if(vMatchedDistance[i2]<=dist) + continue; + // 如果当前匹配距离更小，更新最佳次佳距离 + if(dist=0) + { + vnMatches12[vnMatches21[bestIdx2]]=-1; + nmatches--; + } + // 次优的匹配关系，双向建立 + // vnMatches12保存参考帧F1和F2匹配关系，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引 + vnMatches12[i1]=bestIdx2; + vnMatches21[bestIdx2]=i1; + vMatchedDistance[bestIdx2]=bestDist; + nmatches++; + + // Step 5 计算匹配点旋转角度差所在的直方图 + if(mbCheckOrientation) + { + // 计算匹配特征点的角度差，这里单位是角度°，不是弧度 + float rot = F1.mvKeysUn[i1].angle-F2.mvKeysUn[bestIdx2].angle; + if(rot<0.0) + rot+=360.0f; + // 前面factor = HISTO_LENGTH/360.0f + // bin = rot / 360.of * HISTO_LENGTH 表示当前rot被分配在第几个直方图bin + int bin = round(rot*factor); + // 如果bin 满了又是一个轮回 + if(bin==HISTO_LENGTH) + bin=0; + assert(bin>=0 && bin=0) + { + vnMatches12[idx1]=-1; + nmatches--; + } + } + } + + } + + //Update prev matched + // Step 7 将最后通过筛选的匹配好的特征点保存到vbPrevMatched + for(size_t i1=0, iend1=vnMatches12.size(); i1=0) + vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt; + + return nmatches; + } + + /* + * @brief 通过词袋，对关键帧的特征点进行跟踪，该函数用于闭环检测时两个关键帧间的特征点匹配 + * @details 通过bow对pKF和F中的特征点进行快速匹配（不属于同一node的特征点直接跳过匹配） + * 对属于同一node的特征点通过描述子距离进行匹配 + * 通过距离阈值、比例阈值和角度投票进行剔除误匹配 + * @param pKF1 KeyFrame1 + * @param pKF2 KeyFrame2 + * @param vpMatches12 pKF2中与pKF1匹配的MapPoint，vpMatches12[i]表示匹配的地图点，null表示没有匹配，i表示匹配的pKF1 特征点索引 + * @return 成功匹配的数量 + */ + int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector &vpMatches12) + { + // Step 1 分别取出两个关键帧的特征点、BoW 向量、地图点、描述子 + const vector &vKeysUn1 = pKF1->mvKeysUn; + const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec; + const vector vpMapPoints1 = pKF1->GetMapPointMatches(); + const cv::Mat &Descriptors1 = pKF1->mDescriptors; + + const vector &vKeysUn2 = pKF2->mvKeysUn; + const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec; + const vector vpMapPoints2 = pKF2->GetMapPointMatches(); + const cv::Mat &Descriptors2 = pKF2->mDescriptors; + + // 保存匹配结果 + vpMatches12 = vector(vpMapPoints1.size(),static_cast(NULL)); + vector vbMatched2(vpMapPoints2.size(),false); + + // Step 2 构建旋转直方图，HISTO_LENGTH = 30 + vector rotHist[HISTO_LENGTH]; + for(int i=0;ifirst == f2it->first) + { + // 遍历KF中属于该node的特征点 + for(size_t i1=0, iend1=f1it->second.size(); i1second[i1]; + if(pKF1 -> NLeft != -1 && idx1 >= pKF1 -> mvKeysUn.size()){ + continue; + } + + MapPoint* pMP1 = vpMapPoints1[idx1]; + if(!pMP1) + continue; + if(pMP1->isBad()) + continue; + + const cv::Mat &d1 = Descriptors1.row(idx1); + + int bestDist1=256; + int bestIdx2 =-1 ; + int bestDist2=256; + + // Step 4 遍历KF2中属于该node的特征点，找到了最优及次优匹配点 + for(size_t i2=0, iend2=f2it->second.size(); i2second[i2]; + + if(pKF2 -> NLeft != -1 && idx2 >= pKF2 -> mvKeysUn.size()){ + continue; + } + + MapPoint* pMP2 = vpMapPoints2[idx2]; + + // 如果已经有匹配的点，或者遍历到的特征点对应的地图点无效 + if(vbMatched2[idx2] || !pMP2) + continue; + + if(pMP2->isBad()) + continue; + + const cv::Mat &d2 = Descriptors2.row(idx2); + + int dist = DescriptorDistance(d1,d2); + + if(dist(bestDist1)(bestDist2)) + { + vpMatches12[idx1]=vpMapPoints2[bestIdx2]; + vbMatched2[bestIdx2]=true; + + if(mbCheckOrientation) + { + float rot = vKeysUn1[idx1].angle-vKeysUn2[bestIdx2].angle; + if(rot<0.0) + rot+=360.0f; + int bin = round(rot*factor); + if(bin==HISTO_LENGTH) + bin=0; + assert(bin>=0 && binfirst < f2it->first) + { + f1it = vFeatVec1.lower_bound(f2it->first); + } + else + { + f2it = vFeatVec2.lower_bound(f1it->first); + } + } + + // Step 6 检查旋转直方图分布，剔除差异较大的匹配 + if(mbCheckOrientation) + { + int ind1=-1; + int ind2=-1; + int ind3=-1; + + ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3); + + for(int i=0; i(NULL); + nmatches--; + } + } + } + + return nmatches; + } + + int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, + vector > &vMatchedPairs, const bool bOnlyStereo, const bool bCoarse) + { + const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec; + const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec; + + //Compute epipole in second image + // Step 1 计算KF1的相机中心在KF2图像平面的二维像素坐标 + Sophus::SE3f T1w = pKF1->GetPose(); + Sophus::SE3f T2w = pKF2->GetPose(); + Sophus::SE3f Tw2 = pKF2->GetPoseInverse(); // for convenience + Eigen::Vector3f Cw = pKF1->GetCameraCenter(); + Eigen::Vector3f C2 = T2w * Cw; + + Eigen::Vector2f ep = pKF2->mpCamera->project(C2); + Sophus::SE3f T12; + Sophus::SE3f Tll, Tlr, Trl, Trr; + Eigen::Matrix3f R12; // for fastest computation + Eigen::Vector3f t12; // for fastest computation + + GeometricCamera* pCamera1 = pKF1->mpCamera, *pCamera2 = pKF2->mpCamera; + + if(!pKF1->mpCamera2 && !pKF2->mpCamera2){ + T12 = T1w * Tw2; + R12 = T12.rotationMatrix(); + t12 = T12.translation(); + } + else{ + Sophus::SE3f Tr1w = pKF1->GetRightPose(); + Sophus::SE3f Twr2 = pKF2->GetRightPoseInverse(); + Tll = T1w * Tw2; + Tlr = T1w * Twr2; + Trl = Tr1w * Tw2; + Trr = Tr1w * Twr2; + } + Eigen::Matrix3f Rll = Tll.rotationMatrix(), Rlr = Tlr.rotationMatrix(), Rrl = Trl.rotationMatrix(), Rrr = Trr.rotationMatrix(); + Eigen::Vector3f tll = Tll.translation(), tlr = Tlr.translation(), trl = Trl.translation(), trr = Trr.translation(); + + // Find matches between not tracked keypoints + // Matching speed-up by ORB Vocabulary + // Compare only ORB that share the same node + + int nmatches=0; + // 记录匹配是否成功，避免重复匹配 + vector vbMatched2(pKF2->N,false); + vector vMatches12(pKF1->N,-1); + // 用于统计匹配点对旋转差的直方图 + vector rotHist[HISTO_LENGTH]; + for(int i=0;ifirst对应node编号，f1it->second对应属于该node的所有特特征点编号 + DBoW2::FeatureVector::const_iterator f1it = vFeatVec1.begin(); + DBoW2::FeatureVector::const_iterator f2it = vFeatVec2.begin(); + DBoW2::FeatureVector::const_iterator f1end = vFeatVec1.end(); + DBoW2::FeatureVector::const_iterator f2end = vFeatVec2.end(); + + // Step 2.1：遍历pKF1和pKF2中的node节点 + while(f1it!=f1end && f2it!=f2end) + { + // 如果f1it和f2it属于同一个node节点才会进行匹配，这就是BoW加速匹配原理 + if(f1it->first == f2it->first) + { + // Step 2.2：遍历属于同一node节点(id：f1it->first)下的所有特征点 + for(size_t i1=0, iend1=f1it->second.size(); i1second[i1]; + + // Step 2.3：通过特征点索引idx1在pKF1中取出对应的MapPoint + MapPoint* pMP1 = pKF1->GetMapPoint(idx1); + + // If there is already a MapPoint skip + // 由于寻找的是未匹配的特征点，所以pMP1应该为NULL + if(pMP1) + { + continue; + } + // 如果mvuRight中的值大于0，表示是双目，且该特征点有深度值 + const bool bStereo1 = (!pKF1->mpCamera2 && pKF1->mvuRight[idx1]>=0); + + if(bOnlyStereo) + if(!bStereo1) + continue; + + // Step 2.4：通过特征点索引idx1在pKF1中取出对应的特征点 + const cv::KeyPoint &kp1 = (pKF1 -> NLeft == -1) ? pKF1->mvKeysUn[idx1] + : (idx1 < pKF1 -> NLeft) ? pKF1 -> mvKeys[idx1] + : pKF1 -> mvKeysRight[idx1 - pKF1 -> NLeft]; + + const bool bRight1 = (pKF1 -> NLeft == -1 || idx1 < pKF1 -> NLeft) ? false + : true; + //if(bRight1) continue; + // 通过特征点索引idx1在pKF1中取出对应的特征点的描述子 + const cv::Mat &d1 = pKF1->mDescriptors.row(idx1); + + int bestDist = TH_LOW; + int bestIdx2 = -1; + + // Step 2.5：遍历该node节点下(f2it->first)对应KF2中的所有特征点 + for(size_t i2=0, iend2=f2it->second.size(); i2second[i2]; + + // 通过特征点索引idx2在pKF2中取出对应的MapPoint + MapPoint* pMP2 = pKF2->GetMapPoint(idx2); + + // If we have already matched or there is a MapPoint skip + // 如果pKF2当前特征点索引idx2已经被匹配过或者对应的3d点非空，那么跳过这个索引idx2 + if(vbMatched2[idx2] || pMP2) + continue; + + const bool bStereo2 = (!pKF2->mpCamera2 && pKF2->mvuRight[idx2]>=0); + + if(bOnlyStereo) + if(!bStereo2) + continue; + + // 通过特征点索引idx2在pKF2中取出对应的特征点的描述子 + const cv::Mat &d2 = pKF2->mDescriptors.row(idx2); + + // Step 2.6 计算idx1与idx2在两个关键帧中对应特征点的描述子距离 + const int dist = DescriptorDistance(d1,d2); + + if(dist>TH_LOW || dist>bestDist) + continue; + + // 通过特征点索引idx2在pKF2中取出对应的特征点 + const cv::KeyPoint &kp2 = (pKF2 -> NLeft == -1) ? pKF2->mvKeysUn[idx2] + : (idx2 < pKF2 -> NLeft) ? pKF2 -> mvKeys[idx2] + : pKF2 -> mvKeysRight[idx2 - pKF2 -> NLeft]; + const bool bRight2 = (pKF2 -> NLeft == -1 || idx2 < pKF2 -> NLeft) ? false + : true; + //? 为什么双目就不需要判断像素点到极点的距离的判断？ + // 因为双目模式下可以左右互匹配恢复三维点 + if(!bStereo1 && !bStereo2 && !pKF1->mpCamera2) + { + const float distex = ep(0)-kp2.pt.x; + const float distey = ep(1)-kp2.pt.y; + // Step 2.7 极点e2到kp2的像素距离如果小于阈值th,认为kp2对应的MapPoint距离pKF1相机太近，跳过该匹配点对 + // 作者根据kp2金字塔尺度因子(scale^n，scale=1.2，n为层数)定义阈值th + // 金字塔层数从0到7，对应距离 sqrt(100*pKF2->mvScaleFactors[kp2.octave]) 是10-20个像素 + //? 对这个阈值的有效性持怀疑态度 + if(distex*distex+distey*distey<100*pKF2->mvScaleFactors[kp2.octave]) + { + continue; + } + } + + if(pKF1->mpCamera2 && pKF2->mpCamera2){ + if(bRight1 && bRight2){ + R12 = Rrr; + t12 = trr; + T12 = Trr; + + pCamera1 = pKF1->mpCamera2; + pCamera2 = pKF2->mpCamera2; + } + else if(bRight1 && !bRight2){ + R12 = Rrl; + t12 = trl; + T12 = Trl; + + pCamera1 = pKF1->mpCamera2; + pCamera2 = pKF2->mpCamera; + } + else if(!bRight1 && bRight2){ + R12 = Rlr; + t12 = tlr; + T12 = Tlr; + + pCamera1 = pKF1->mpCamera; + pCamera2 = pKF2->mpCamera2; + } + else{ + R12 = Rll; + t12 = tll; + T12 = Tll; + + pCamera1 = pKF1->mpCamera; + pCamera2 = pKF2->mpCamera; + } + + } + + // Step 2.8 计算特征点kp2到kp1对应极线的距离是否小于阈值 + if(bCoarse || pCamera1->epipolarConstrain(pCamera2,kp1,kp2,R12,t12,pKF1->mvLevelSigma2[kp1.octave],pKF2->mvLevelSigma2[kp2.octave])) // MODIFICATION_2 + { + // bestIdx2，bestDist 是 kp1 对应 KF2中的最佳匹配点 index及匹配距离 + bestIdx2 = idx2; + bestDist = dist; + } + } + + if(bestIdx2>=0) + { + const cv::KeyPoint &kp2 = (pKF2 -> NLeft == -1) ? pKF2->mvKeysUn[bestIdx2] + : (bestIdx2 < pKF2 -> NLeft) ? pKF2 -> mvKeys[bestIdx2] + : pKF2 -> mvKeysRight[bestIdx2 - pKF2 -> NLeft]; + // 记录匹配结果 + vMatches12[idx1]=bestIdx2; + // !记录已经匹配，避免重复匹配。原作者漏掉！可以添加下面代码 + // vbMatched2[bestIdx2]=true; + nmatches++; + + // 记录旋转差直方图信息 + if(mbCheckOrientation) + { + // angle：角度，表示匹配点对的方向差。 + float rot = kp1.angle-kp2.angle; + if(rot<0.0) + rot+=360.0f; + int bin = round(rot*factor); + if(bin==HISTO_LENGTH) + bin=0; + assert(bin>=0 && binfirst < f2it->first) + { + f1it = vFeatVec1.lower_bound(f2it->first); + } + else + { + f2it = vFeatVec2.lower_bound(f1it->first); + } + } + + // Step 3 用旋转差直方图来筛掉错误匹配对 + if(mbCheckOrientation) + { + int ind1=-1; + int ind2=-1; + int ind3=-1; + + ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3); + + for(int i=0; i &vpMapPoints, const float th, const bool bRight) + { + GeometricCamera* pCamera; + Sophus::SE3f Tcw; + Eigen::Vector3f Ow; + + if(bRight){ + Tcw = pKF->GetRightPose(); + Ow = pKF->GetRightCameraCenter(); + pCamera = pKF->mpCamera2; + } + else{ + Tcw = pKF->GetPose(); + Ow = pKF->GetCameraCenter(); + pCamera = pKF->mpCamera; + } + + const float &fx = pKF->fx; + const float &fy = pKF->fy; + const float &cx = pKF->cx; + const float &cy = pKF->cy; + const float &bf = pKF->mbf; + + int nFused=0; + + const int nMPs = vpMapPoints.size(); + + // For debbuging + int count_notMP = 0, count_bad=0, count_isinKF = 0, count_negdepth = 0, count_notinim = 0, count_dist = 0, count_normal=0, count_notidx = 0, count_thcheck = 0; + // 遍历所有的待投影地图点 + for(int i=0; iisBad()) + { + count_bad++; + continue; + } + else if(pMP->IsInKeyFrame(pKF)) + { + count_isinKF++; + continue; + } + + // 将地图点变换到关键帧的相机坐标系下 + Eigen::Vector3f p3Dw = pMP->GetWorldPos(); + Eigen::Vector3f p3Dc = Tcw * p3Dw; + + // Depth must be positive + // 深度值为负，跳过 + if(p3Dc(2)<0.0f) + { + count_negdepth++; + continue; + } + // Step 2 得到地图点投影到关键帧的图像坐标 + const float invz = 1/p3Dc(2); + + const Eigen::Vector2f uv = pCamera->project(p3Dc); + + // Point must be inside the image + // 投影点需要在有效范围内 + if(!pKF->IsInImage(uv(0),uv(1))) + { + count_notinim++; + continue; + } + + const float ur = uv(0)-bf*invz; + + const float maxDistance = pMP->GetMaxDistanceInvariance(); + const float minDistance = pMP->GetMinDistanceInvariance(); + Eigen::Vector3f PO = p3Dw-Ow; + const float dist3D = PO.norm(); + + // Depth must be inside the scale pyramid of the image + // Step 3 地图点到关键帧相机光心距离需满足在有效范围内 + if(dist3DmaxDistance) { + count_dist++; + continue; + } + + // Viewing angle must be less than 60 deg + // Step 4 地图点的平均观测方向（正视程度）要小于60° + Eigen::Vector3f Pn = pMP->GetNormal(); + + if(PO.dot(Pn)<0.5*dist3D) + { + count_normal++; + continue; + } + + int nPredictedLevel = pMP->PredictScale(dist3D,pKF); + + // Search in a radius + // 根据MapPoint的深度确定尺度，从而确定搜索范围 + const float radius = th*pKF->mvScaleFactors[nPredictedLevel]; + // Step 5 在投影点附近搜索窗口内找到候选匹配点的索引 + const vector vIndices = pKF->GetFeaturesInArea(uv(0),uv(1),radius,bRight); + + if(vIndices.empty()) + { + count_notidx++; + continue; + } + + // Match to the most similar keypoint in the radius + // Step 6 遍历寻找最佳匹配点（最小描述子距离） + const cv::Mat dMP = pMP->GetDescriptor(); + + int bestDist = 256; + int bestIdx = -1; + for(vector::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++) + { + size_t idx = *vit; + const cv::KeyPoint &kp = (pKF -> NLeft == -1) ? pKF->mvKeysUn[idx] + : (!bRight) ? pKF -> mvKeys[idx] + : pKF -> mvKeysRight[idx]; + + const int &kpLevel= kp.octave; + // 金字塔层级要接近（同一层或小一层），否则跳过 + if(kpLevelnPredictedLevel) + continue; + + // 计算投影点与候选匹配特征点的距离，如果偏差很大，直接跳过 + if(pKF->mvuRight[idx]>=0) + { + // Check reprojection error in stereo + // 双目情况 + const float &kpx = kp.pt.x; + const float &kpy = kp.pt.y; + const float &kpr = pKF->mvuRight[idx]; + const float ex = uv(0)-kpx; + const float ey = uv(1)-kpy; + // 右目数据的偏差也要考虑进去 + const float er = ur-kpr; + const float e2 = ex*ex+ey*ey+er*er; + + //自由度为3, 误差小于1个像素,这种事情95%发生的概率对应卡方检验阈值为7.82 + if(e2*pKF->mvInvLevelSigma2[kpLevel]>7.8) + continue; + } + else + { + // 单目情况 + const float &kpx = kp.pt.x; + const float &kpy = kp.pt.y; + const float ex = uv(0)-kpx; + const float ey = uv(1)-kpy; + const float e2 = ex*ex+ey*ey; + + // 自由度为2的，卡方检验阈值5.99（假设测量有一个像素的偏差） + if(e2*pKF->mvInvLevelSigma2[kpLevel]>5.99) + continue; + } + + if(bRight) idx += pKF->NLeft; + + const cv::Mat &dKF = pKF->mDescriptors.row(idx); + + const int dist = DescriptorDistance(dMP,dKF); + + if(distGetMapPoint(bestIdx); + if(pMPinKF) + { + // 如果最佳匹配点有对应有效地图点，选择被观测次数最多的那个替换 + if(!pMPinKF->isBad()) + { + if(pMPinKF->Observations()>pMP->Observations()) + pMP->Replace(pMPinKF); + else + pMPinKF->Replace(pMP); + } + } + else + { + // 如果最佳匹配点没有对应地图点，添加观测信息 + pMP->AddObservation(pKF,bestIdx); + pKF->AddMapPoint(pMP,bestIdx); + } + nFused++; + } + else + count_thcheck++; + + } + + return nFused; + } + + + /** + * @brief 闭环矫正中使用。将当前关键帧闭环匹配上的关键帧及其共视关键帧组成的地图点投影到当前关键帧，融合地图点 + * + * @param[in] pKF 当前关键帧 + * @param[in] Scw 当前关键帧经过闭环Sim3 后的世界到相机坐标系的Sim变换 + * @param[in] vpPoints 与当前关键帧闭环匹配上的关键帧及其共视关键帧组成的地图点 + * @param[in] th 搜索范围系数 + * @param[out] vpReplacePoint 替换的地图点 + * @return int 融合（替换和新增）的地图点数目 + */ + int ORBmatcher::Fuse(KeyFrame *pKF, Sophus::Sim3f &Scw, const vector &vpPoints, float th, vector &vpReplacePoint) + { + // Get Calibration Parameters for later projection + const float &fx = pKF->fx; + const float &fy = pKF->fy; + const float &cx = pKF->cx; + const float &cy = pKF->cy; + + // Decompose Scw + // Step 1 将Sim3转化为SE3并分解 + Sophus::SE3f Tcw = Sophus::SE3f(Scw.rotationMatrix(),Scw.translation()/Scw.scale()); + Eigen::Vector3f Ow = Tcw.inverse().translation(); + + // Set of MapPoints already found in the KeyFrame + // 当前帧已有的匹配地图点 + const set spAlreadyFound = pKF->GetMapPoints(); + + int nFused=0; + // 与当前帧闭环匹配上的关键帧及其共视关键帧组成的地图点 + const int nPoints = vpPoints.size(); + + // For each candidate MapPoint project and match + // 遍历所有的地图点 + for(int iMP=0; iMPisBad() || spAlreadyFound.count(pMP)) + continue; + + // Get 3D Coords. + // Step 2 地图点变换到当前相机坐标系下 + Eigen::Vector3f p3Dw = pMP->GetWorldPos(); + + // Transform into Camera Coords. + Eigen::Vector3f p3Dc = Tcw * p3Dw; + + // Depth must be positive + if(p3Dc(2)<0.0f) + continue; + + // Project into Image + // Step 3 得到地图点投影到当前帧的图像坐标 + const Eigen::Vector2f uv = pKF->mpCamera->project(p3Dc); + + // Point must be inside the image + // 投影点必须在图像范围内 + if(!pKF->IsInImage(uv(0),uv(1))) + continue; + + // Depth must be inside the scale pyramid of the image + // Step 4 根据距离是否在图像合理金字塔尺度范围内和观测角度是否小于60度判断该地图点是否有效 + const float maxDistance = pMP->GetMaxDistanceInvariance(); + const float minDistance = pMP->GetMinDistanceInvariance(); + Eigen::Vector3f PO = p3Dw-Ow; + const float dist3D = PO.norm(); + + if(dist3DmaxDistance) + continue; + + // Viewing angle must be less than 60 deg + Eigen::Vector3f Pn = pMP->GetNormal(); + + if(PO.dot(Pn)<0.5*dist3D) + continue; + + // Compute predicted scale level + const int nPredictedLevel = pMP->PredictScale(dist3D,pKF); + + // Search in a radius + // 计算搜索范围 + const float radius = th*pKF->mvScaleFactors[nPredictedLevel]; + + // Step 5 在当前帧内搜索匹配候选点 + const vector vIndices = pKF->GetFeaturesInArea(uv(0),uv(1),radius); + + if(vIndices.empty()) + continue; + + // Match to the most similar keypoint in the radius + // Step 6 寻找最佳匹配点（没有用到次佳匹配的比例） + const cv::Mat dMP = pMP->GetDescriptor(); + + int bestDist = INT_MAX; + int bestIdx = -1; + for(vector::const_iterator vit=vIndices.begin(); vit!=vIndices.end(); vit++) + { + const size_t idx = *vit; + const int &kpLevel = pKF->mvKeysUn[idx].octave; + + if(kpLevelnPredictedLevel) + continue; + + const cv::Mat &dKF = pKF->mDescriptors.row(idx); + + int dist = DescriptorDistance(dMP,dKF); + + if(distGetMapPoint(bestIdx); + if(pMPinKF) + { + // 如果这个地图点已经存在，则记录要替换信息 + // 这里不能直接替换，原因是需要对地图点加锁后才能替换，否则可能会crash。所以先记录，在加锁后替换 + if(!pMPinKF->isBad()) + vpReplacePoint[iMP] = pMPinKF; + } + else + { + // 如果这个地图点不存在，直接添加 + pMP->AddObservation(pKF,bestIdx); + pKF->AddMapPoint(pMP,bestIdx); + } + nFused++; + } + } + // 融合（替换和新增）的地图点数目 + return nFused; + } + + /** + * @brief 通过Sim3变换，搜索两个关键帧中更多的匹配点对 + * （之前使用SearchByBoW进行特征点匹配时会有漏匹配） + * @param[in] pKF1 当前帧 + * @param[in] pKF2 闭环候选帧 + * @param[in] vpMatches12 i表示匹配的pKF1 特征点索引，vpMatches12[i]表示匹配的地图点，null表示没有匹配 + * @param[in] s12 pKF2 到 pKF1 的Sim 变换中的尺度 + * @param[in] R12 pKF2 到 pKF1 的Sim 变换中的旋转矩阵 + * @param[in] t12 pKF2 到 pKF1 的Sim 变换中的平移向量 + * @param[in] th 搜索窗口的倍数 + * @return int 新增的匹配点对数目 + */ + int ORBmatcher::SearchBySim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches12, const Sophus::Sim3f &S12, const float th) + { + // Step 1： 准备工作 + const float &fx = pKF1->fx; + const float &fy = pKF1->fy; + const float &cx = pKF1->cx; + const float &cy = pKF1->cy; + + // 从world到camera1的变换 + // 从world到camera2的变换 + // Camera 1 & 2 from world + Sophus::SE3f T1w = pKF1->GetPose(); + Sophus::SE3f T2w = pKF2->GetPose(); + + //Transformation between cameras + // Sim3 的逆 + Sophus::Sim3f S21 = S12.inverse(); + + // 取出关键帧中的地图点 + const vector vpMapPoints1 = pKF1->GetMapPointMatches(); + const int N1 = vpMapPoints1.size(); + + const vector vpMapPoints2 = pKF2->GetMapPointMatches(); + const int N2 = vpMapPoints2.size(); + + // pKF1中特征点的匹配情况，有匹配为true，否则false + vector vbAlreadyMatched1(N1,false); + // pKF2中特征点的匹配情况，有匹配为true，否则false + vector vbAlreadyMatched2(N2,false); + + // Step 2：记录已经匹配的特征点 + for(int i=0; i(pMP->GetIndexInKeyFrame(pKF2)); + if(idx2>=0 && idx2 vnMatch1(N1,-1); + vector vnMatch2(N2,-1); + + // Transform from KF1 to KF2 and search + // Step 3：通过Sim变换，寻找 pKF1 中特征点和 pKF2 中的新的匹配 + // 之前使用SearchByBoW进行特征点匹配时会有漏匹配 + for(int i1=0; i1isBad()) + continue; + + // Step 3.1：通过Sim变换，将pKF1的地图点投影到pKF2中的图像坐标 + Eigen::Vector3f p3Dw = pMP->GetWorldPos(); + // 把pKF1的地图点从world坐标系变换到camera1坐标系 + Eigen::Vector3f p3Dc1 = T1w * p3Dw; + // 再通过Sim3将该地图点从camera1变换到camera2坐标系 + Eigen::Vector3f p3Dc2 = S21 * p3Dc1; + + // Depth must be positive + if(p3Dc2(2)<0.0) + continue; + + // 投影到camera2图像坐标 (u,v) + const float invz = 1.0/p3Dc2(2); + const float x = p3Dc2(0)*invz; + const float y = p3Dc2(1)*invz; + + const float u = fx*x+cx; + const float v = fy*y+cy; + + // Point must be inside the image + if(!pKF2->IsInImage(u,v)) + continue; + + const float maxDistance = pMP->GetMaxDistanceInvariance(); + const float minDistance = pMP->GetMinDistanceInvariance(); + const float dist3D = p3Dc2.norm(); + + // Depth must be inside the scale invariance region + if(dist3DmaxDistance ) + continue; + + // Compute predicted octave + // Step 3.2：预测投影的点在图像金字塔哪一层 + const int nPredictedLevel = pMP->PredictScale(dist3D,pKF2); + + // Search in a radius + // 计算特征点搜索半径 + const float radius = th*pKF2->mvScaleFactors[nPredictedLevel]; + + // Step 3.3：搜索该区域内的所有候选匹配特征点 + const vector vIndices = pKF2->GetFeaturesInArea(u,v,radius); + + if(vIndices.empty()) + continue; + + // Match to the most similar keypoint in the radius + const cv::Mat dMP = pMP->GetDescriptor(); + + int bestDist = INT_MAX; + int bestIdx = -1; + // Step 3.4：遍历所有候选特征点，寻找最佳匹配点（并未使用次佳最佳比例约束） + for(vector::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++) + { + const size_t idx = *vit; + + const cv::KeyPoint &kp = pKF2->mvKeysUn[idx]; + + if(kp.octavenPredictedLevel) + continue; + + const cv::Mat &dKF = pKF2->mDescriptors.row(idx); + + const int dist = DescriptorDistance(dMP,dKF); + + if(distisBad()) + continue; + + Eigen::Vector3f p3Dw = pMP->GetWorldPos(); + Eigen::Vector3f p3Dc2 = T2w * p3Dw; + Eigen::Vector3f p3Dc1 = S12 * p3Dc2; + + // Depth must be positive + if(p3Dc1(2)<0.0) + continue; + + const float invz = 1.0/p3Dc1(2); + const float x = p3Dc1(0)*invz; + const float y = p3Dc1(1)*invz; + + const float u = fx*x+cx; + const float v = fy*y+cy; + + // Point must be inside the image + if(!pKF1->IsInImage(u,v)) + continue; + + const float maxDistance = pMP->GetMaxDistanceInvariance(); + const float minDistance = pMP->GetMinDistanceInvariance(); + const float dist3D = p3Dc1.norm(); + + // Depth must be inside the scale pyramid of the image + if(dist3DmaxDistance) + continue; + + // Compute predicted octave + const int nPredictedLevel = pMP->PredictScale(dist3D,pKF1); + + // Search in a radius of 2.5*sigma(ScaleLevel) + const float radius = th*pKF1->mvScaleFactors[nPredictedLevel]; + + const vector vIndices = pKF1->GetFeaturesInArea(u,v,radius); + + if(vIndices.empty()) + continue; + + // Match to the most similar keypoint in the radius + const cv::Mat dMP = pMP->GetDescriptor(); + + int bestDist = INT_MAX; + int bestIdx = -1; + for(vector::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++) + { + const size_t idx = *vit; + + const cv::KeyPoint &kp = pKF1->mvKeysUn[idx]; + + if(kp.octavenPredictedLevel) + continue; + + const cv::Mat &dKF = pKF1->mDescriptors.row(idx); + + const int dist = DescriptorDistance(dMP,dKF); + + if(dist=0) + { + int idx1 = vnMatch2[idx2]; + if(idx1==i1) + { + // 更新匹配的地图点 + vpMatches12[i1] = vpMapPoints2[idx2]; + nFound++; + } + } + } + + return nFound; + } + + /** + * @brief 将上一帧跟踪的地图点投影到当前帧，并且搜索匹配点。用于跟踪前一帧 + * 步骤 + * Step 1 建立旋转直方图，用于检测旋转一致性 + * Step 2 计算当前帧和前一帧的平移向量 + * Step 3 对于前一帧的每一个地图点，通过相机投影模型，得到投影到当前帧的像素坐标 + * Step 4 根据相机的前后前进方向来判断搜索尺度范围 + * Step 5 遍历候选匹配点，寻找距离最小的最佳匹配点 + * Step 6 计算匹配点旋转角度差所在的直方图 + * Step 7 进行旋转一致检测，剔除不一致的匹配 + * @param[in] CurrentFrame 当前帧 + * @param[in] LastFrame 上一帧 + * @param[in] th 搜索范围阈值，默认单目为7，双目15 + * @param[in] bMono 是否为单目 + * @return int 成功匹配的数量 + */ + int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono) + { + int nmatches = 0; + + // Rotation Histogram (to check rotation consistency) + // Step 1 建立旋转直方图，用于检测旋转一致性 + vector rotHist[HISTO_LENGTH]; + for(int i=0;iCurrentFrame.mb && !bMono; // 非单目情况，如果Z大于基线，则表示相机明显前进 + const bool bBackward = -tlc(2)>CurrentFrame.mb && !bMono; // 非单目情况，如果-Z小于基线，则表示相机明显后退 + + // Step 3 对于前一帧的每一个地图点，通过相机投影模型，得到投影到当前帧的像素坐标 + for(int i=0; iGetWorldPos(); + Eigen::Vector3f x3Dc = Tcw * x3Dw; + + const float xc = x3Dc(0); + const float yc = x3Dc(1); + const float invzc = 1.0/x3Dc(2); + + if(invzc<0) + continue; + + // 投影到当前帧中 + Eigen::Vector2f uv = CurrentFrame.mpCamera->project(x3Dc); + + if(uv(0)CurrentFrame.mnMaxX) + continue; + if(uv(1)CurrentFrame.mnMaxY) + continue; + // 认为投影前后地图点的尺度信息不变 + int nLastOctave = (LastFrame.Nleft == -1 || i < LastFrame.Nleft) ? LastFrame.mvKeys[i].octave + : LastFrame.mvKeysRight[i - LastFrame.Nleft].octave; + + // Search in a window. Size depends on scale + // 单目：th = 7，双目：th = 15 + float radius = th*CurrentFrame.mvScaleFactors[nLastOctave]; // 尺度越大，搜索范围越大 + + // 记录候选匹配点的id + vector vIndices2; + + // Step 4 根据相机的前后前进方向来判断搜索尺度范围。 + // 以下可以这么理解，例如一个有一定面积的圆点，在某个尺度n下它是一个特征点 + // 当相机前进时，圆点的面积增大，在某个尺度m下它是一个特征点，由于面积增大，则需要在更高的尺度下才能检测出来 + // 当相机后退时，圆点的面积减小，在某个尺度m下它是一个特征点，由于面积减小，则需要在更低的尺度下才能检测出来 + if(bForward) // 前进,则上一帧兴趣点在所在的尺度nLastOctave<=nCurOctave + vIndices2 = CurrentFrame.GetFeaturesInArea(uv(0),uv(1), radius, nLastOctave); + else if(bBackward) // 后退,则上一帧兴趣点在所在的尺度0<=nCurOctave<=nLastOctave + vIndices2 = CurrentFrame.GetFeaturesInArea(uv(0),uv(1), radius, 0, nLastOctave); + else // 在[nLastOctave-1, nLastOctave+1]中搜索 + vIndices2 = CurrentFrame.GetFeaturesInArea(uv(0),uv(1), radius, nLastOctave-1, nLastOctave+1); + + if(vIndices2.empty()) + continue; + + const cv::Mat dMP = pMP->GetDescriptor(); + + int bestDist = 256; + int bestIdx2 = -1; + + // Step 5 遍历候选匹配点，寻找距离最小的最佳匹配点 + for(vector::const_iterator vit=vIndices2.begin(), vend=vIndices2.end(); vit!=vend; vit++) + { + const size_t i2 = *vit; + + // 如果该特征点已经有对应的MapPoint了,则退出该次循环 + if(CurrentFrame.mvpMapPoints[i2]) + if(CurrentFrame.mvpMapPoints[i2]->Observations()>0) + continue; + + if(CurrentFrame.Nleft == -1 && CurrentFrame.mvuRight[i2]>0) + { + // 双目和rgbd的情况，需要保证右图的点也在搜索半径以内 + const float ur = uv(0) - CurrentFrame.mbf*invzc; + const float er = fabs(ur - CurrentFrame.mvuRight[i2]); + if(er>radius) + continue; + } + + const cv::Mat &d = CurrentFrame.mDescriptors.row(i2); + + const int dist = DescriptorDistance(dMP,d); + + if(dist=0 && binproject(x3Dr); + + int nLastOctave = (LastFrame.Nleft == -1 || i < LastFrame.Nleft) ? LastFrame.mvKeys[i].octave + : LastFrame.mvKeysRight[i - LastFrame.Nleft].octave; + + // Search in a window. Size depends on scale + float radius = th*CurrentFrame.mvScaleFactors[nLastOctave]; + + vector vIndices2; + + if(bForward) + vIndices2 = CurrentFrame.GetFeaturesInArea(uv(0),uv(1), radius, nLastOctave, -1,true); + else if(bBackward) + vIndices2 = CurrentFrame.GetFeaturesInArea(uv(0),uv(1), radius, 0, nLastOctave, true); + else + vIndices2 = CurrentFrame.GetFeaturesInArea(uv(0),uv(1), radius, nLastOctave-1, nLastOctave+1, true); + + const cv::Mat dMP = pMP->GetDescriptor(); + + int bestDist = 256; + int bestIdx2 = -1; + + for(vector::const_iterator vit=vIndices2.begin(), vend=vIndices2.end(); vit!=vend; vit++) + { + const size_t i2 = *vit; + if(CurrentFrame.mvpMapPoints[i2 + CurrentFrame.Nleft]) + if(CurrentFrame.mvpMapPoints[i2 + CurrentFrame.Nleft]->Observations()>0) + continue; + + const cv::Mat &d = CurrentFrame.mDescriptors.row(i2 + CurrentFrame.Nleft); + + const int dist = DescriptorDistance(dMP,d); + + if(dist=0 && bin(NULL); + nmatches--; + } + } + } + } + + return nmatches; + } + + /** + * @brief 通过投影的方式将关键帧中未匹配的地图点投影到当前帧中,进行匹配，并通过旋转直方图进行筛选 + * + * @param[in] CurrentFrame 当前帧 + * @param[in] pKF 参考关键帧 + * @param[in] sAlreadyFound 已经找到的地图点集合，不会用于PNP + * @param[in] th 匹配时搜索范围，会乘以金字塔尺度 + * @param[in] ORBdist 匹配的ORB描述子距离应该小于这个阈值 + * @return int 成功匹配的数量 + */ + int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF, const set &sAlreadyFound, const float th , const int ORBdist) + { + int nmatches = 0; + + const Sophus::SE3f Tcw = CurrentFrame.GetPose(); + Eigen::Vector3f Ow = Tcw.inverse().translation(); + + // Rotation Histogram (to check rotation consistency) + // Step 1 建立旋转直方图，用于检测旋转一致性 + vector rotHist[HISTO_LENGTH]; + for(int i=0;i vpMPs = pKF->GetMapPointMatches(); + + // Step 2 遍历关键帧中的每个地图点，通过相机投影模型，得到投影到当前帧的像素坐标 + for(size_t i=0, iend=vpMPs.size(); iisBad() && !sAlreadyFound.count(pMP)) + { + //Project + Eigen::Vector3f x3Dw = pMP->GetWorldPos(); + Eigen::Vector3f x3Dc = Tcw * x3Dw; + + const Eigen::Vector2f uv = CurrentFrame.mpCamera->project(x3Dc); + + if(uv(0)CurrentFrame.mnMaxX) + continue; + if(uv(1)CurrentFrame.mnMaxY) + continue; + + // Compute predicted scale level + Eigen::Vector3f PO = x3Dw-Ow; + float dist3D = PO.norm(); + + const float maxDistance = pMP->GetMaxDistanceInvariance(); + const float minDistance = pMP->GetMinDistanceInvariance(); + + // Depth must be inside the scale pyramid of the image + if(dist3DmaxDistance) + continue; + + //预测尺度 + int nPredictedLevel = pMP->PredictScale(dist3D,&CurrentFrame); + + // Search in a window + // 搜索半径和尺度相关 + const float radius = th*CurrentFrame.mvScaleFactors[nPredictedLevel]; + + // Step 3 搜索候选匹配点 + const vector vIndices2 = CurrentFrame.GetFeaturesInArea(uv(0), uv(1), radius, nPredictedLevel-1, nPredictedLevel+1); + + if(vIndices2.empty()) + continue; + + const cv::Mat dMP = pMP->GetDescriptor(); + + int bestDist = 256; + int bestIdx2 = -1; + // Step 4 遍历候选匹配点，寻找距离最小的最佳匹配点 + for(vector::const_iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++) + { + const size_t i2 = *vit; + if(CurrentFrame.mvpMapPoints[i2]) + continue; + + const cv::Mat &d = CurrentFrame.mDescriptors.row(i2); + + const int dist = DescriptorDistance(dMP,d); + + if(distmvKeysUn[i].angle-CurrentFrame.mvKeysUn[bestIdx2].angle; + if(rot<0.0) + rot+=360.0f; + int bin = round(rot*factor); + if(bin==HISTO_LENGTH) + bin=0; + assert(bin>=0 && bin* histo, const int L, int &ind1, int &ind2, int &ind3) + { + int max1=0; + int max2=0; + int max3=0; + + for(int i=0; imax1) + { + max3=max2; + max2=max1; + max1=s; + ind3=ind2; + ind2=ind1; + ind1=i; + } + else if(s>max2) + { + max3=max2; + max2=s; + ind3=ind2; + ind2=i; + } + else if(s>max3) + { + max3=s; + ind3=i; + } + } + + // 如果差距太大了,说明次优的非常不好,这里就索性放弃了,都置为-1 + if(max2<0.1f*(float)max1) + { + ind2=-1; + ind3=-1; + } + else if(max3<0.1f*(float)max1) + { + ind3=-1; + } + } + + + // Bit set count operation from + // Hamming distance：两个二进制串之间的汉明距离，指的是其不同位数的个数 + // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel + int ORBmatcher::DescriptorDistance(const cv::Mat &a, const cv::Mat &b) + { + const int *pa = a.ptr(); + const int *pb = b.ptr(); + + int dist=0; + + // 8*32=256bit + + for(int i=0; i<8; i++, pa++, pb++) + { + unsigned int v = *pa ^ *pb; // 相等为0,不等为1 + // 下面的操作就是计算其中bit为1的个数了,这个操作看上面的链接就好 + // 其实我觉得也还阔以直接使用8bit的查找表,然后做32次寻址操作就完成了;不过缺点是没有利用好CPU的字长 + v = v - ((v >> 1) & 0x55555555); + v = (v & 0x33333333) + ((v >> 2) & 0x33333333); + dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24; + } + + return dist; + } + +} //namespace ORB_SLAM diff --git a/ORB-SLAM3-UESTC/Workspace/src/OptimizableTypes.cpp b/ORB-SLAM3-UESTC/Workspace/src/OptimizableTypes.cpp new file mode 100644 index 0000000..abeb717 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/OptimizableTypes.cpp @@ -0,0 +1,393 @@ +/** + * This file is part of ORB-SLAM3 + * + * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * + * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public + * License as published by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even + * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with ORB-SLAM3. + * If not, see . + */ + +#include "OptimizableTypes.h" + +namespace ORB_SLAM3 +{ +bool EdgeSE3ProjectXYZOnlyPose::read(std::istream &is) +{ + for (int i = 0; i < 2; i++) + { + is >> _measurement[i]; + } + for (int i = 0; i < 2; i++) + for (int j = i; j < 2; j++) + { + is >> information()(i, j); + if (i != j) + information()(j, i) = information()(i, j); + } + return true; +} + +bool EdgeSE3ProjectXYZOnlyPose::write(std::ostream &os) const +{ + + for (int i = 0; i < 2; i++) + { + os << measurement()[i] << " "; + } + + for (int i = 0; i < 2; i++) + for (int j = i; j < 2; j++) + { + os << " " << information()(i, j); + } + return os.good(); +} + +/** + * @brief 求解二维像素坐标关于位姿的雅克比矩阵 _jacobianOplusXi + */ +void EdgeSE3ProjectXYZOnlyPose::linearizeOplus() +{ + g2o::VertexSE3Expmap *vi = static_cast(_vertices[0]); + Eigen::Vector3d xyz_trans = vi->estimate().map(Xw); + + double x = xyz_trans[0]; + double y = xyz_trans[1]; + double z = xyz_trans[2]; + + Eigen::Matrix SE3deriv; + SE3deriv << 0.f, z, -y, 1.f, 0.f, 0.f, + -z, 0.f, x, 0.f, 1.f, 0.f, + y, -x, 0.f, 0.f, 0.f, 1.f; + + _jacobianOplusXi = -pCamera->projectJac(xyz_trans) * SE3deriv; +} + +bool EdgeSE3ProjectXYZOnlyPoseToBody::read(std::istream &is) +{ + for (int i = 0; i < 2; i++) + { + is >> _measurement[i]; + } + for (int i = 0; i < 2; i++) + for (int j = i; j < 2; j++) + { + is >> information()(i, j); + if (i != j) + information()(j, i) = information()(i, j); + } + return true; +} + +bool EdgeSE3ProjectXYZOnlyPoseToBody::write(std::ostream &os) const +{ + + for (int i = 0; i < 2; i++) + { + os << measurement()[i] << " "; + } + + for (int i = 0; i < 2; i++) + for (int j = i; j < 2; j++) + { + os << " " << information()(i, j); + } + return os.good(); +} + +/** + * @brief 求解右目的二维像素坐标关于左目位姿的雅克比矩阵 _jacobianOplusXi + */ +void EdgeSE3ProjectXYZOnlyPoseToBody::linearizeOplus() +{ + // 获得三维点在右相机坐标系下的坐标 + g2o::VertexSE3Expmap *vi = static_cast(_vertices[0]); + g2o::SE3Quat T_lw(vi->estimate()); + Eigen::Vector3d X_l = T_lw.map(Xw); + Eigen::Vector3d X_r = mTrl.map(T_lw.map(Xw)); + + double x_w = X_l[0]; + double y_w = X_l[1]; + double z_w = X_l[2]; + + Eigen::Matrix SE3deriv; + SE3deriv << 0.f, z_w, -y_w, 1.f, 0.f, 0.f, + -z_w, 0.f, x_w, 0.f, 1.f, 0.f, + y_w, -x_w, 0.f, 0.f, 0.f, 1.f; + + /* + 注意这里是对李代数求导，ρlw != tlw 所以不能使用Pl = Rlw*Pw + tlw + Pl = EXP(ξlw)*Pw Pr = Rrl * EXP(ξlw) * Pw + trl + 让Pr 对 ξlw 求雅克比 + 相当于Rrl*(Pl 对 ξlw的雅克比) + */ + _jacobianOplusXi = -pCamera->projectJac(X_r) * mTrl.rotation().toRotationMatrix() * SE3deriv; +} + +EdgeSE3ProjectXYZ::EdgeSE3ProjectXYZ() : BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>() +{ +} + +bool EdgeSE3ProjectXYZ::read(std::istream &is) +{ + for (int i = 0; i < 2; i++) + { + is >> _measurement[i]; + } + for (int i = 0; i < 2; i++) + for (int j = i; j < 2; j++) + { + is >> information()(i, j); + if (i != j) + information()(j, i) = information()(i, j); + } + return true; +} + +bool EdgeSE3ProjectXYZ::write(std::ostream &os) const +{ + + for (int i = 0; i < 2; i++) + { + os << measurement()[i] << " "; + } + + for (int i = 0; i < 2; i++) + for (int j = i; j < 2; j++) + { + os << " " << information()(i, j); + } + return os.good(); +} + +/** + * @brief 求解二维像素坐标关于位姿的雅克比矩阵 _jacobianOplusXj 二维像素坐标关于三维点世界坐标的雅克比矩阵 _jacobianOplusXi + */ +void EdgeSE3ProjectXYZ::linearizeOplus() +{ + g2o::VertexSE3Expmap *vj = static_cast(_vertices[1]); + g2o::SE3Quat T(vj->estimate()); + g2o::VertexSBAPointXYZ *vi = static_cast(_vertices[0]); + Eigen::Vector3d xyz = vi->estimate(); + Eigen::Vector3d xyz_trans = T.map(xyz); + + double x = xyz_trans[0]; + double y = xyz_trans[1]; + double z = xyz_trans[2]; + + auto projectJac = -pCamera->projectJac(xyz_trans); + // Pc = Rcw*Pw + tcw 先求Pw改变对Pc的影响，所以直接为Rcw，前面再乘Pc对像素的影响 + _jacobianOplusXi = projectJac * T.rotation().toRotationMatrix(); + + Eigen::Matrix SE3deriv; + SE3deriv << 0.f, z, -y, 1.f, 0.f, 0.f, + -z, 0.f, x, 0.f, 1.f, 0.f, + y, -x, 0.f, 0.f, 0.f, 1.f; + + _jacobianOplusXj = projectJac * SE3deriv; +} + +EdgeSE3ProjectXYZToBody::EdgeSE3ProjectXYZToBody() : BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>() +{ +} + +bool EdgeSE3ProjectXYZToBody::read(std::istream &is) +{ + for (int i = 0; i < 2; i++) + { + is >> _measurement[i]; + } + for (int i = 0; i < 2; i++) + for (int j = i; j < 2; j++) + { + is >> information()(i, j); + if (i != j) + information()(j, i) = information()(i, j); + } + return true; +} + +bool EdgeSE3ProjectXYZToBody::write(std::ostream &os) const +{ + + for (int i = 0; i < 2; i++) + { + os << measurement()[i] << " "; + } + + for (int i = 0; i < 2; i++) + for (int j = i; j < 2; j++) + { + os << " " << information()(i, j); + } + return os.good(); +} + +/** + * @brief 求解右目二维像素坐标关于位姿的雅克比矩阵 _jacobianOplusXj 右目二维像素坐标关于三维点世界坐标的雅克比矩阵 _jacobianOplusXi + */ +void EdgeSE3ProjectXYZToBody::linearizeOplus() +{ + g2o::VertexSE3Expmap *vj = static_cast(_vertices[1]); + g2o::SE3Quat T_lw(vj->estimate()); + g2o::SE3Quat T_rw = mTrl * T_lw; + g2o::VertexSBAPointXYZ *vi = static_cast(_vertices[0]); + Eigen::Vector3d X_w = vi->estimate(); + Eigen::Vector3d X_l = T_lw.map(X_w); + Eigen::Vector3d X_r = mTrl.map(T_lw.map(X_w)); + + _jacobianOplusXi = -pCamera->projectJac(X_r) * T_rw.rotation().toRotationMatrix(); + + double x = X_l[0]; + double y = X_l[1]; + double z = X_l[2]; + + Eigen::Matrix SE3deriv; + SE3deriv << 0.f, z, -y, 1.f, 0.f, 0.f, + -z, 0.f, x, 0.f, 1.f, 0.f, + y, -x, 0.f, 0.f, 0.f, 1.f; + /* + 注意这里是对李代数求导，ρlw != tlw 所以不能使用Pl = Rlw*Pw + tlw + Pl = EXP(ξlw)*Pw Pr = Rrl * EXP(ξlw) * Pw + trl + 让Pr 对 ξlw 求雅克比 + 相当于Rrl*(Pl 对 ξlw的雅克比) + */ + _jacobianOplusXj = -pCamera->projectJac(X_r) * mTrl.rotation().toRotationMatrix() * SE3deriv; +} + +VertexSim3Expmap::VertexSim3Expmap() : BaseVertex<7, g2o::Sim3>() +{ + _marginalized = false; + _fix_scale = false; +} + +bool VertexSim3Expmap::read(std::istream &is) +{ + g2o::Vector7d cam2world; + for (int i = 0; i < 6; i++) + { + is >> cam2world[i]; + } + is >> cam2world[6]; + + float nextParam; + for (size_t i = 0; i < pCamera1->size(); i++) + { + is >> nextParam; + pCamera1->setParameter(nextParam, i); + } + + for (size_t i = 0; i < pCamera2->size(); i++) + { + is >> nextParam; + pCamera2->setParameter(nextParam, i); + } + + setEstimate(g2o::Sim3(cam2world).inverse()); + return true; +} + +bool VertexSim3Expmap::write(std::ostream &os) const +{ + g2o::Sim3 cam2world(estimate().inverse()); + g2o::Vector7d lv = cam2world.log(); + for (int i = 0; i < 7; i++) + { + os << lv[i] << " "; + } + + for (size_t i = 0; i < pCamera1->size(); i++) + { + os << pCamera1->getParameter(i) << " "; + } + + for (size_t i = 0; i < pCamera2->size(); i++) + { + os << pCamera2->getParameter(i) << " "; + } + + return os.good(); +} + +EdgeSim3ProjectXYZ::EdgeSim3ProjectXYZ() : g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, VertexSim3Expmap>() +{ +} + +bool EdgeSim3ProjectXYZ::read(std::istream &is) +{ + for (int i = 0; i < 2; i++) + { + is >> _measurement[i]; + } + + for (int i = 0; i < 2; i++) + for (int j = i; j < 2; j++) + { + is >> information()(i, j); + if (i != j) + information()(j, i) = information()(i, j); + } + return true; +} + +bool EdgeSim3ProjectXYZ::write(std::ostream &os) const +{ + for (int i = 0; i < 2; i++) + { + os << _measurement[i] << " "; + } + + for (int i = 0; i < 2; i++) + for (int j = i; j < 2; j++) + { + os << " " << information()(i, j); + } + return os.good(); +} + +EdgeInverseSim3ProjectXYZ::EdgeInverseSim3ProjectXYZ() : g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, VertexSim3Expmap>() +{ +} + +bool EdgeInverseSim3ProjectXYZ::read(std::istream &is) +{ + for (int i = 0; i < 2; i++) + { + is >> _measurement[i]; + } + + for (int i = 0; i < 2; i++) + for (int j = i; j < 2; j++) + { + is >> information()(i, j); + if (i != j) + information()(j, i) = information()(i, j); + } + return true; +} + +bool EdgeInverseSim3ProjectXYZ::write(std::ostream &os) const +{ + for (int i = 0; i < 2; i++) + { + os << _measurement[i] << " "; + } + + for (int i = 0; i < 2; i++) + for (int j = i; j < 2; j++) + { + os << " " << information()(i, j); + } + return os.good(); +} + +} diff --git a/ORB-SLAM3-UESTC/Workspace/src/Optimizer.cc b/ORB-SLAM3-UESTC/Workspace/src/Optimizer.cc new file mode 100644 index 0000000..614a46a --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/Optimizer.cc @@ -0,0 +1,6641 @@ +/** + * This file is part of ORB-SLAM3 + * + * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * + * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public + * License as published by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even + * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with ORB-SLAM3. + * If not, see . + */ + +#include "Optimizer.h" + +#include + +#include +#include +#include + +#include "Thirdparty/g2o/g2o/core/sparse_block_matrix.h" +#include "Thirdparty/g2o/g2o/core/block_solver.h" +#include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h" +#include "Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h" +#include "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h" +#include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" +#include "Thirdparty/g2o/g2o/core/robust_kernel_impl.h" +#include "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h" +#include "G2oTypes.h" +#include "Converter.h" + +#include + +#include "OptimizableTypes.h" + +namespace ORB_SLAM3 +{ +bool sortByVal(const pair &a, const pair &b) +{ + return (a.second < b.second); +} + +/**************************************以下为单帧优化**************************************************************/ + +/** + * @brief 位姿优化，纯视觉时使用。优化目标：单帧的位姿 + * @param pFrame 待优化的帧 + */ +int Optimizer::PoseOptimization(Frame *pFrame) +{ + // 该优化函数主要用于Tracking线程中：运动跟踪、参考帧跟踪、地图跟踪、重定位 + + // Step 1：构造g2o优化器, BlockSolver_6_3表示：位姿 _PoseDim 为6维，路标点 _LandmarkDim 是3维 + g2o::SparseOptimizer optimizer; + g2o::BlockSolver_6_3::LinearSolverType *linearSolver; + + linearSolver = new g2o::LinearSolverDense(); + + g2o::BlockSolver_6_3 *solver_ptr = new g2o::BlockSolver_6_3(linearSolver); + + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + optimizer.setAlgorithm(solver); + + // 输入的帧中,有效的,参与优化过程的2D-3D点对 + int nInitialCorrespondences = 0; + + // Set Frame vertex + // Step 2：添加顶点：待优化当前帧的Tcw + g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap(); + Sophus::SE3 Tcw = pFrame->GetPose(); + vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(), Tcw.translation().cast())); + + // 设置id，保证本次优化过程中id独立即可 + vSE3->setId(0); + // 要优化的变量，所以不能固定 + vSE3->setFixed(false); + // 添加节点 + optimizer.addVertex(vSE3); + + // Set MapPoint vertices + // 当前帧的特征点数量 + const int N = pFrame->N; + + vector vpEdgesMono; // 存放单目边 + vector vpEdgesMono_FHR; // 存放另一目的边 + vector vnIndexEdgeMono, vnIndexEdgeRight; // 边对应特征点的id + vpEdgesMono.reserve(N); + vpEdgesMono_FHR.reserve(N); + vnIndexEdgeMono.reserve(N); + vnIndexEdgeRight.reserve(N); + + vector vpEdgesStereo; // 存放双目边 + vector vnIndexEdgeStereo; + vpEdgesStereo.reserve(N); + vnIndexEdgeStereo.reserve(N); + + // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991 + // 可以理解为卡方值高于5.991 95%的几率维外点 + const float deltaMono = sqrt(5.991); + // 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815 + const float deltaStereo = sqrt(7.815); + + // Step 3：添加一元边 + { + // 锁定地图点。由于需要使用地图点来构造顶点和边,因此不希望在构造的过程中部分地图点被改写造成不一致甚至是段错误 + unique_lock lock(MapPoint::mGlobalMutex); + + // 遍历当前帧中的所有地图点 + for (int i = 0; i < N; i++) + { + MapPoint *pMP = pFrame->mvpMapPoints[i]; + // 如果这个地图点还存在没有被剔除掉 + if (pMP) + { + // Conventional SLAM + // 不存在相机2，则有可能为单目与双目 + if (!pFrame->mpCamera2) + { + // Monocular observation + // 单目情况，因为双目模式下pFrame->mvuRight[i]会大于0 + if (pFrame->mvuRight[i] < 0) + { + nInitialCorrespondences++; + pFrame->mvbOutlier[i] = false; + + // 对这个地图点的观测，也就是去畸变像素坐标 + Eigen::Matrix obs; + const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i]; + obs << kpUn.pt.x, kpUn.pt.y; + // 新建节点,注意这个节点的只是优化位姿Pose + ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose *e = new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose(); + + e->setVertex(0, dynamic_cast(optimizer.vertex(0))); + e->setMeasurement(obs); + // 这个点的可信程度和特征点所在的图层有关，层数约高invSigma2越小，信息矩阵越小，表示误差越大，优化时考虑的比较少 + const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + // 在这里使用了鲁棒核函数 + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(deltaMono); + + // 设置相机 + e->pCamera = pFrame->mpCamera; + // 地图点的空间位置,作为迭代的初始值，因为是一元边，所以不以节点的形式出现 + e->Xw = pMP->GetWorldPos().cast(); + + optimizer.addEdge(e); + + vpEdgesMono.push_back(e); + vnIndexEdgeMono.push_back(i); + } + else // 双目 + { + nInitialCorrespondences++; + pFrame->mvbOutlier[i] = false; + + // SET EDGE + // 观测多了一项右目的坐标 + Eigen::Matrix obs; + const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i]; + const float &kp_ur = pFrame->mvuRight[i]; + obs << kpUn.pt.x, kpUn.pt.y, kp_ur; + // 新建节点,注意这里也是只优化位姿 + g2o::EdgeStereoSE3ProjectXYZOnlyPose *e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose(); + + e->setVertex(0, dynamic_cast(optimizer.vertex(0))); + e->setMeasurement(obs); + // 置信程度主要是看左目特征点所在的图层 + const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]; + Eigen::Matrix3d Info = Eigen::Matrix3d::Identity() * invSigma2; + e->setInformation(Info); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(deltaStereo); + + // 双目下没有加入相机 + e->fx = pFrame->fx; + e->fy = pFrame->fy; + e->cx = pFrame->cx; + e->cy = pFrame->cy; + e->bf = pFrame->mbf; + e->Xw = pMP->GetWorldPos().cast(); + + optimizer.addEdge(e); + + vpEdgesStereo.push_back(e); + vnIndexEdgeStereo.push_back(i); + } + } + // SLAM with respect a rigid body + else // 两个相机 + { + nInitialCorrespondences++; + + cv::KeyPoint kpUn; + // 总数N = 相机1所有特征点数量+相机2所有特征点数量 + if (i < pFrame->Nleft) + { // Left camera observation + kpUn = pFrame->mvKeys[i]; + + pFrame->mvbOutlier[i] = false; + + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y; + + ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose *e = new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose(); + + e->setVertex(0, dynamic_cast(optimizer.vertex(0))); + e->setMeasurement(obs); + const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(deltaMono); + + e->pCamera = pFrame->mpCamera; + e->Xw = pMP->GetWorldPos().cast(); + + optimizer.addEdge(e); + + vpEdgesMono.push_back(e); + vnIndexEdgeMono.push_back(i); + } + else + { // Right camera observation + kpUn = pFrame->mvKeysRight[i - pFrame->Nleft]; + + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y; + + pFrame->mvbOutlier[i] = false; + + ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody *e = new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody(); + + e->setVertex(0, dynamic_cast(optimizer.vertex(0))); + e->setMeasurement(obs); + const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(deltaMono); + + e->pCamera = pFrame->mpCamera2; + e->Xw = pMP->GetWorldPos().cast(); + + e->mTrl = g2o::SE3Quat( + pFrame->GetRelativePoseTrl().unit_quaternion().cast(), pFrame->GetRelativePoseTrl().translation().cast()); + + optimizer.addEdge(e); + + vpEdgesMono_FHR.push_back(e); + vnIndexEdgeRight.push_back(i); + } + } + } + } + } + // 如果没有足够的匹配点,那么就只好放弃了 + // cout << "PO: vnIndexEdgeMono.size() = " << vnIndexEdgeMono.size() << " vnIndexEdgeRight.size() = " << vnIndexEdgeRight.size() << endl; + if (nInitialCorrespondences < 3) + return 0; + + // We perform 4 optimizations, after each optimization we classify observation as inlier/outlier + // At the next optimization, outliers are not included, but at the end they can be classified as inliers again. + // Step 4：开始优化，总共优化四次，每次优化迭代10次,每次优化后，将观测分为outlier和inlier，outlier不参与下次优化 + // 由于每次优化后是对所有的观测进行outlier和inlier判别，因此之前被判别为outlier有可能变成inlier，反之亦然 + // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差） + const float chi2Mono[4] = {5.991, 5.991, 5.991, 5.991}; // 单目 + const float chi2Stereo[4] = {7.815, 7.815, 7.815, 7.815}; // 双目 + const int its[4] = {10, 10, 10, 10}; // 四次迭代，每次迭代的次数 + + // bad 的地图点个数 + int nBad = 0; + // 一共进行四次优化，每次会剔除外点 + for (size_t it = 0; it < 4; it++) + { + Tcw = pFrame->GetPose(); + vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(), Tcw.translation().cast())); + // 其实就是初始化优化器,这里的参数0就算是不填写,默认也是0,也就是只对level为0的边进行优化 + optimizer.initializeOptimization(0); + // 开始优化，优化10次 + optimizer.optimize(its[it]); + + nBad = 0; + // 优化结束,开始遍历参与优化的每一条误差边(单目或双相机的相机1) + for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) + { + ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose *e = vpEdgesMono[i]; + + const size_t idx = vnIndexEdgeMono[i]; + + // 如果这条误差边是来自于outlier，也就是上一次优化去掉的 + // 重新计算，因为上一次外点这一次可能成为内点 + if (pFrame->mvbOutlier[idx]) + { + e->computeError(); + } + + // 就是error*\Omega*error,表征了这个点的误差大小(考虑置信度以后) + const float chi2 = e->chi2(); + + if (chi2 > chi2Mono[it]) + { + pFrame->mvbOutlier[idx] = true; + e->setLevel(1); // 设置为outlier , level 1 对应为外点,上面的过程中我们设置其为不优化 + nBad++; + } + else + { + pFrame->mvbOutlier[idx] = false; + e->setLevel(0); // 设置为inlier, level 0 对应为内点,上面的过程中我们就是要优化这些关系 + } + + if (it == 2) + e->setRobustKernel(0); // 除了前两次优化需要RobustKernel以外, 其余的优化都不需要 -- 因为重投影的误差已经有明显的下降了 + } + // 对于相机2 + for (size_t i = 0, iend = vpEdgesMono_FHR.size(); i < iend; i++) + { + ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody *e = vpEdgesMono_FHR[i]; + + const size_t idx = vnIndexEdgeRight[i]; + + if (pFrame->mvbOutlier[idx]) + { + e->computeError(); + } + + const float chi2 = e->chi2(); + + if (chi2 > chi2Mono[it]) + { + pFrame->mvbOutlier[idx] = true; + e->setLevel(1); + nBad++; + } + else + { + pFrame->mvbOutlier[idx] = false; + e->setLevel(0); + } + + if (it == 2) + e->setRobustKernel(0); + } + // 对于双目 + for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++) + { + g2o::EdgeStereoSE3ProjectXYZOnlyPose *e = vpEdgesStereo[i]; + + const size_t idx = vnIndexEdgeStereo[i]; + + if (pFrame->mvbOutlier[idx]) + { + e->computeError(); + } + + const float chi2 = e->chi2(); + + if (chi2 > chi2Stereo[it]) + { + pFrame->mvbOutlier[idx] = true; + e->setLevel(1); + nBad++; + } + else + { + e->setLevel(0); + pFrame->mvbOutlier[idx] = false; + } + + if (it == 2) + e->setRobustKernel(0); + } + + if (optimizer.edges().size() < 10) + break; + } + + // Recover optimized pose and return number of inliers + // Step 5 得到优化后的当前帧的位姿 + g2o::VertexSE3Expmap *vSE3_recov = static_cast(optimizer.vertex(0)); + g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate(); + Sophus::SE3 pose( + SE3quat_recov.rotation().cast(), + SE3quat_recov.translation().cast()); + pFrame->SetPose(pose); + + // 并且返回内点数目 + return nInitialCorrespondences - nBad; +} + +/** + * @brief 使用上一关键帧+当前帧的视觉信息和IMU信息，优化当前帧位姿 + * + * 可分为以下几个步骤： + * // Step 1：创建g2o优化器，初始化顶点和边 + * // Step 2：启动多轮优化，剔除外点 + * // Step 3：更新当前帧位姿、速度、IMU偏置 + * // Step 4：记录当前帧的优化状态，包括参数信息和对应的海森矩阵 + * + * @param[in] pFrame 当前帧，也是待优化的帧 + * @param[in] bRecInit 调用这个函数的位置并没有传这个参数，因此它的值默认为false + * @return int 返回优化后的内点数 + */ +int Optimizer::PoseInertialOptimizationLastKeyFrame(Frame *pFrame, bool bRecInit) +{ + // 1. 创建g2o优化器，初始化顶点和边 + // 构建一个稀疏求解器 + g2o::SparseOptimizer optimizer; + g2o::BlockSolverX::LinearSolverType *linearSolver; + + // 使用dense的求解器，（常见非dense求解器有cholmod线性求解器和shur补线性求解器） + linearSolver = new g2o::LinearSolverDense(); + + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); + // 使用高斯牛顿求解器 + g2o::OptimizationAlgorithmGaussNewton *solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr); + optimizer.setVerbose(false); + optimizer.setAlgorithm(solver); + + // 当前帧单（左）目地图点数目 + int nInitialMonoCorrespondences = 0; + int nInitialStereoCorrespondences = 0; + //上面两项的和，即参与优化的地图点总数目 + int nInitialCorrespondences = 0; + + // Set Frame vertex + // 2. 确定节点 + // 当前帧的位姿，旋转+平移，6-dim + VertexPose *VP = new VertexPose(pFrame); + VP->setId(0); + VP->setFixed(false); + optimizer.addVertex(VP); + // 当前帧的速度，3-dim + VertexVelocity *VV = new VertexVelocity(pFrame); + VV->setId(1); + VV->setFixed(false); + optimizer.addVertex(VV); + // 当前帧的陀螺仪偏置，3-dim + VertexGyroBias *VG = new VertexGyroBias(pFrame); + VG->setId(2); + VG->setFixed(false); + optimizer.addVertex(VG); + // 当前帧的加速度偏置，3-dim + VertexAccBias *VA = new VertexAccBias(pFrame); + VA->setId(3); + VA->setFixed(false); + optimizer.addVertex(VA); + // setFixed(false)这个设置使以上四个顶点（15个参数）在优化时更新 + + // Set MapPoint vertices + // 当前帧的特征点总数 + const int N = pFrame->N; + // 当前帧左目的特征点总数 + const int Nleft = pFrame->Nleft; + // 当前帧是否存在右目（即是否为双目） + const bool bRight = (Nleft != -1); + + vector vpEdgesMono; + vector vpEdgesStereo; + vector vnIndexEdgeMono; + vector vnIndexEdgeStereo; + vpEdgesMono.reserve(N); + vpEdgesStereo.reserve(N); + vnIndexEdgeMono.reserve(N); + vnIndexEdgeStereo.reserve(N); + + // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991 + const float thHuberMono = sqrt(5.991); + // 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815 + const float thHuberStereo = sqrt(7.815); + + { + // 锁定地图点。由于需要使用地图点来构造顶点和边,因此不希望在构造的过程中部分地图点被改写造成不一致甚至是段错误 + // 3. 投影边 + unique_lock lock(MapPoint::mGlobalMutex); + + for (int i = 0; i < N; i++) + { + MapPoint *pMP = pFrame->mvpMapPoints[i]; + if (pMP) + { + cv::KeyPoint kpUn; + + // Left monocular observation + // 这里说的Left monocular包含两种情况：1.单目情况 2.双目情况下的左目 + if ((!bRight && pFrame->mvuRight[i] < 0) || i < Nleft) + { + // 如果是双目情况下的左目 + if (i < Nleft) // pair left-right + //使用未畸变校正的特征点 + kpUn = pFrame->mvKeys[i]; + // 如果是单目 + else + // 使用畸变校正过的特征点 + kpUn = pFrame->mvKeysUn[i]; + + // 单目地图点计数增加 + nInitialMonoCorrespondences++; + // 当前地图点默认设置为不是外点 + pFrame->mvbOutlier[i] = false; + + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y; + + // 第一种边(视觉重投影约束)：地图点投影到该帧图像的坐标与特征点坐标偏差尽可能小 + EdgeMonoOnlyPose *e = new EdgeMonoOnlyPose(pMP->GetWorldPos(), 0); + + // 将位姿作为第一个顶点 + e->setVertex(0, VP); + + // 设置观测值，即去畸变后的像素坐标 + e->setMeasurement(obs); + + // Add here uncerteinty + // 获取不确定度，这里调用uncertainty2返回固定值1.0 + // ?这里的1.0是作为缺省值的意思吗？是否可以根据对视觉信息的信任度动态修改这个值，比如标定的误差？ + const float unc2 = pFrame->mpCamera->uncertainty2(obs); + + // invSigma2 = (Inverse(协方差矩阵))^2，表明该约束在各个维度上的可信度 + // 图像金字塔层数越高，可信度越差 + const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2; + // 设置该约束的信息矩阵 + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + // 设置鲁棒核函数，避免其误差的平方项出现数值过大的增长 注：后续在优化2次后会用e->setRobustKernel(0)禁掉鲁棒核函数 + e->setRobustKernel(rk); + + // 重投影误差的自由度为2，设置对应的卡方阈值 + rk->setDelta(thHuberMono); + + // 将第一种边加入优化器 + optimizer.addEdge(e); + + // 将第一种边加入vpEdgesMono + vpEdgesMono.push_back(e); + // 将对应的特征点索引加入vnIndexEdgeMono + vnIndexEdgeMono.push_back(i); + } + // Stereo observation + else if (!bRight) + { + nInitialStereoCorrespondences++; + pFrame->mvbOutlier[i] = false; + + kpUn = pFrame->mvKeysUn[i]; + const float kp_ur = pFrame->mvuRight[i]; + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y, kp_ur; + + EdgeStereoOnlyPose *e = new EdgeStereoOnlyPose(pMP->GetWorldPos()); + + e->setVertex(0, VP); + e->setMeasurement(obs); + + // Add here uncerteinty + const float unc2 = pFrame->mpCamera->uncertainty2(obs.head(2)); + + const float &invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2; + e->setInformation(Eigen::Matrix3d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberStereo); + + optimizer.addEdge(e); + + vpEdgesStereo.push_back(e); + vnIndexEdgeStereo.push_back(i); + } + + // Right monocular observation + if (bRight && i >= Nleft) + { + nInitialMonoCorrespondences++; + pFrame->mvbOutlier[i] = false; + + kpUn = pFrame->mvKeysRight[i - Nleft]; + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y; + + EdgeMonoOnlyPose *e = new EdgeMonoOnlyPose(pMP->GetWorldPos(), 1); + + e->setVertex(0, VP); + e->setMeasurement(obs); + + // Add here uncerteinty + const float unc2 = pFrame->mpCamera->uncertainty2(obs); + + const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberMono); + + optimizer.addEdge(e); + + vpEdgesMono.push_back(e); + vnIndexEdgeMono.push_back(i); + } + } + } + } + // 统计参与优化的地图点总数目 + nInitialCorrespondences = nInitialMonoCorrespondences + nInitialStereoCorrespondences; + + // 4. 上一个关键帧节点 + // pKF为上一关键帧 + KeyFrame *pKF = pFrame->mpLastKeyFrame; + + // 上一关键帧的位姿，旋转+平移，6-dim + VertexPose *VPk = new VertexPose(pKF); + VPk->setId(4); + VPk->setFixed(true); + optimizer.addVertex(VPk); + // 上一关键帧的速度，3-dim + VertexVelocity *VVk = new VertexVelocity(pKF); + VVk->setId(5); + VVk->setFixed(true); + optimizer.addVertex(VVk); + // 上一关键帧的陀螺仪偏置，3-dim + VertexGyroBias *VGk = new VertexGyroBias(pKF); + VGk->setId(6); + VGk->setFixed(true); + optimizer.addVertex(VGk); + // 上一关键帧的加速度偏置，3-dim + VertexAccBias *VAk = new VertexAccBias(pKF); + VAk->setId(7); + VAk->setFixed(true); + optimizer.addVertex(VAk); + // setFixed(true)这个设置使以上四个顶点（15个参数）的值在优化时保持固定 + // 既然被选为关键帧，就不能太善变 + + // 5. 第二种边（IMU预积分约束）：两帧之间位姿的变化量与IMU预积分的值偏差尽可能小 + EdgeInertial *ei = new EdgeInertial(pFrame->mpImuPreintegrated); + + // 将上一关键帧四个顶点（P、V、BG、BA）和当前帧两个顶点（P、V）加入第二种边 + ei->setVertex(0, VPk); + ei->setVertex(1, VVk); + ei->setVertex(2, VGk); + ei->setVertex(3, VAk); + ei->setVertex(4, VP); + ei->setVertex(5, VV); + // 把第二种边加入优化器 + optimizer.addEdge(ei); + + // 6. 第三种边（陀螺仪随机游走约束）：陀螺仪的随机游走值在相近帧间不会相差太多 residual=VG-VGk + // 用大白话来讲就是用固定的VGK拽住VG，防止VG在优化中放飞自我 + EdgeGyroRW *egr = new EdgeGyroRW(); + + // 将上一关键帧的BG加入第三种边 + egr->setVertex(0, VGk); + // 将当前帧的BG加入第三种边 + egr->setVertex(1, VG); + // C值在预积分阶段更新，range(9,12)对应陀螺仪偏置的协方差，最终cvInfoG值为inv(∑(GyroRW^2/freq)) + Eigen::Matrix3d InfoG = pFrame->mpImuPreintegrated->C.block<3, 3>(9, 9).cast().inverse(); + egr->setInformation(InfoG); + // 把第三种边加入优化器 + optimizer.addEdge(egr); + + // 7. 第四种边（加速度随机游走约束）：加速度的随机游走值在相近帧间不会相差太多 residual=VA-VAk + // 用大白话来讲就是用固定的VAK拽住VA，防止VA在优化中放飞自我 + EdgeAccRW *ear = new EdgeAccRW(); + // 将上一关键帧的BA加入第四种边 + ear->setVertex(0, VAk); + // 将当前帧的BA加入第四种边 + ear->setVertex(1, VA); + // C值在预积分阶段更新，range(12,15)对应加速度偏置的协方差，最终cvInfoG值为inv(∑(AccRW^2/freq)) + Eigen::Matrix3d InfoA = pFrame->mpImuPreintegrated->C.block<3, 3>(12, 12).cast().inverse(); + ear->setInformation(InfoA); + // 把第四种边加入优化器 + optimizer.addEdge(ear); + + // 8. 启动多轮优化，剔除外点 + + // We perform 4 optimizations, after each optimization we classify observation as inlier/outlier + // At the next optimization, outliers are not included, but at the end they can be classified as inliers again. + // 卡方检验值呈递减趋势，目的是让检验越来越苛刻 + float chi2Mono[4] = {12, 7.5, 5.991, 5.991}; + float chi2Stereo[4] = {15.6, 9.8, 7.815, 7.815}; + // 4次优化的迭代次数都为10 + int its[4] = {10, 10, 10, 10}; + + // 坏点数 + int nBad = 0; + // 单目坏点数 + int nBadMono = 0; + // 双目坏点数 + int nBadStereo = 0; + // 单目内点数 + int nInliersMono = 0; + // 双目内点数 + int nInliersStereo = 0; + // 内点数 + int nInliers = 0; + bool bOut = false; + + // 进行4次优化 + for (size_t it = 0; it < 4; it++) + { + // 初始化优化器,这里的参数0代表只对level为0的边进行优化（不传参数默认也是0） + optimizer.initializeOptimization(0); + // 每次优化迭代十次 + optimizer.optimize(its[it]); + + // 每次优化都重新统计各类点的数目 + nBad = 0; + nBadMono = 0; + nBadStereo = 0; + nInliers = 0; + nInliersMono = 0; + nInliersStereo = 0; + + // 使用1.5倍的chi2Mono作为“近点”的卡方检验值，意味着地图点越近，检验越宽松 + // 地图点如何定义为“近点”在下面的代码中有解释 + float chi2close = 1.5 * chi2Mono[it]; + + // For monocular observations + for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) + { + EdgeMonoOnlyPose *e = vpEdgesMono[i]; + + const size_t idx = vnIndexEdgeMono[i]; + + // 如果这条误差边是来自于outlier + if (pFrame->mvbOutlier[idx]) + { + // 计算这条边上次优化后的误差 + e->computeError(); + } + + // 就是error*\Omega*error，表示了这条边考虑置信度以后的误差大小 + const float chi2 = e->chi2(); + + // 当地图点在当前帧的深度值小于10时，该地图点属于close（近点） + // mTrackDepth是在Frame.cc的isInFrustum函数中计算出来的 + bool bClose = pFrame->mvpMapPoints[idx]->mTrackDepth < 10.f; + + // 判断某地图点为外点的条件有以下三种： + // 1.该地图点不是近点并且误差大于卡方检验值chi2Mono[it] + // 2.该地图点是近点并且误差大于卡方检验值chi2close + // 3.深度不为正 + // 每次优化后，用更小的卡方检验值，原因是随着优化的进行，对划分为内点的信任程度越来越低 + if ((chi2 > chi2Mono[it] && !bClose) || (bClose && chi2 > chi2close) || !e->isDepthPositive()) + { + // 将该点设置为外点 + pFrame->mvbOutlier[idx] = true; + // 外点不参与下一轮优化 + e->setLevel(1); + // 单目坏点数+1 + nBadMono++; + } + else + { + // 将该点设置为内点（暂时） + pFrame->mvbOutlier[idx] = false; + // 内点继续参与下一轮优化 + e->setLevel(0); + // 单目内点数+1 + nInliersMono++; + } + + // 从第三次优化开始就不设置鲁棒核函数了，原因是经过两轮优化已经趋向准确值，不会有太大误差 + if (it == 2) + e->setRobustKernel(0); + } + + // For stereo observations + for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++) + { + EdgeStereoOnlyPose *e = vpEdgesStereo[i]; + + const size_t idx = vnIndexEdgeStereo[i]; + + if (pFrame->mvbOutlier[idx]) + { + e->computeError(); + } + + const float chi2 = e->chi2(); + + if (chi2 > chi2Stereo[it]) + { + pFrame->mvbOutlier[idx] = true; + e->setLevel(1); // not included in next optimization + nBadStereo++; + } + else + { + pFrame->mvbOutlier[idx] = false; + e->setLevel(0); + nInliersStereo++; + } + + if (it == 2) + e->setRobustKernel(0); + } + + // 内点总数=单目内点数+双目内点数 + nInliers = nInliersMono + nInliersStereo; + // 坏点数=单目坏点数+双目坏点数 + nBad = nBadMono + nBadStereo; + + if (optimizer.edges().size() < 10) + { + cout << "PIOLKF: NOT ENOUGH EDGES" << endl; + break; + } + } + + // If not too much tracks, recover not too bad points + // 9. 若4次优化后内点数小于30，尝试恢复一部分不那么糟糕的坏点 + if ((nInliers < 30) && !bRecInit) + { + // 重新从0开始统计坏点数 + nBad = 0; + // 单目可容忍的卡方检验最大值（如果误差比这还大就不要挣扎了...） + const float chi2MonoOut = 18.f; + const float chi2StereoOut = 24.f; + EdgeMonoOnlyPose *e1; + EdgeStereoOnlyPose *e2; + // 遍历所有单目特征点 + for (size_t i = 0, iend = vnIndexEdgeMono.size(); i < iend; i++) + { + const size_t idx = vnIndexEdgeMono[i]; + // 获取这些特征点对应的边 + e1 = vpEdgesMono[i]; + e1->computeError(); + // 判断误差值是否超过单目可容忍的卡方检验最大值，是的话就把这个点保下来 + if (e1->chi2() < chi2MonoOut) + pFrame->mvbOutlier[idx] = false; + else + nBad++; + } + for (size_t i = 0, iend = vnIndexEdgeStereo.size(); i < iend; i++) + { + const size_t idx = vnIndexEdgeStereo[i]; + e2 = vpEdgesStereo[i]; + e2->computeError(); + if (e2->chi2() < chi2StereoOut) + pFrame->mvbOutlier[idx] = false; + else + nBad++; + } + } + + // 10. 更新当前帧位姿、速度、IMU偏置 + + // Recover optimized pose, velocity and biases + // 给当前帧设置优化后的旋转、位移、速度，用来更新位姿 + pFrame->SetImuPoseVelocity(VP->estimate().Rwb.cast(), VP->estimate().twb.cast(), VV->estimate().cast()); + Vector6d b; + b << VG->estimate(), VA->estimate(); + // 给当前帧设置优化后的bg，ba + pFrame->mImuBias = IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2]); + + // 11. 记录当前帧的优化状态，包括参数信息和对应的海森矩阵 + // Recover Hessian, marginalize keyFframe states and generate new prior for frame + Eigen::Matrix H; + H.setZero(); + + // H(x)=J(x).t()*info*J(x) + + // J(x)取的是EdgeInertial中的_jacobianOplus[4]和_jacobianOplus[5]，即EdgeInertial::computeError计算出来的er,ev,ep对当前帧Pose和Velocity的偏导 + //因此ei->GetHessian2的结果为： + // H(∂er/∂r) H(∂er/∂t) H(∂er/∂v) + // H(∂ev/∂r) H(∂ev/∂t) H(∂ev/∂v) + // H(∂ep/∂r) H(∂ep/∂t) H(∂ep/∂v) + //每项H都是3x3，故GetHessian2的结果是9x9 + H.block<9, 9>(0, 0) += ei->GetHessian2(); + + // J(x)取的是EdgeGyroRW中的_jacobianOplusXj，即EdgeGyroRW::computeError计算出来的_error(ebg)对当前帧bg的偏导 + //因此egr->GetHessian2的结果为： + // H(∂ebg/∂bg) ，3x3 + H.block<3, 3>(9, 9) += egr->GetHessian2(); + + // J(x)取的是EdgeAccRW中的_jacobianOplusXj，即EdgeAccRW::computeError计算出来的_error(ebg)对当前帧ba的偏导 + //因此ear->GetHessian2的结果为： + // H(∂eba/∂ba) ，3x3 + H.block<3, 3>(12, 12) += ear->GetHessian2(); + + //经过前面几步，Hessian Matrix长这个样子（注：省略了->GetHessian2()） + // ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0 + // ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0 + // ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0 + // ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0 + // ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0 + // ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0 + // ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0 + // ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0 + // ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0 + // 0 0 0 0 0 0 0 0 0 egr egr egr 0 0 0 + // 0 0 0 0 0 0 0 0 0 egr egr egr 0 0 0 + // 0 0 0 0 0 0 0 0 0 egr egr egr 0 0 0 + // 0 0 0 0 0 0 0 0 0 0 0 0 ear ear ear + // 0 0 0 0 0 0 0 0 0 0 0 0 ear ear ear + // 0 0 0 0 0 0 0 0 0 0 0 0 ear ear ear + + int tot_in = 0, tot_out = 0; + for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) + { + EdgeMonoOnlyPose *e = vpEdgesMono[i]; + + const size_t idx = vnIndexEdgeMono[i]; + + if (!pFrame->mvbOutlier[idx]) + { + H.block<6, 6>(0, 0) += e->GetHessian(); + tot_in++; + } + else + tot_out++; + } + + for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++) + { + EdgeStereoOnlyPose *e = vpEdgesStereo[i]; + + const size_t idx = vnIndexEdgeStereo[i]; + + if (!pFrame->mvbOutlier[idx]) + { + H.block<6, 6>(0, 0) += e->GetHessian(); + tot_in++; + } + else + tot_out++; + } + + // 设eie = ei->GetHessian2()+∑(e->GetHessian()) + // 则最终Hessian Matrix长这样 + // eie eie eie eie eie eie ei ei ei 0 0 0 0 0 0 + // eie eie eie eie eie eie ei ei ei 0 0 0 0 0 0 + // eie eie eie eie eie eie ei ei ei 0 0 0 0 0 0 + // eie eie eie eie eie eie ei ei ei 0 0 0 0 0 0 + // eie eie eie eie eie eie ei ei ei 0 0 0 0 0 0 + // eie eie eie eie eie eie ei ei ei 0 0 0 0 0 0 + // ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0 + // ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0 + // ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0 + // 0 0 0 0 0 0 0 0 0 egr egr egr 0 0 0 + // 0 0 0 0 0 0 0 0 0 egr egr egr 0 0 0 + // 0 0 0 0 0 0 0 0 0 egr egr egr 0 0 0 + // 0 0 0 0 0 0 0 0 0 0 0 0 ear ear ear + // 0 0 0 0 0 0 0 0 0 0 0 0 ear ear ear + // 0 0 0 0 0 0 0 0 0 0 0 0 ear ear ear + + // 构造一个ConstraintPoseImu对象，为下一帧提供先验约束 + // 构造对象所使用的参数是当前帧P、V、BG、BA的估计值和H矩阵 + pFrame->mpcpi = new ConstraintPoseImu(VP->estimate().Rwb, VP->estimate().twb, VV->estimate(), VG->estimate(), VA->estimate(), H); + // 在PoseInertialOptimizationLastFrame函数中，会将ConstraintPoseImu信息作为“上一帧先验约束”生成一条优化边 + + // 返回值：内点数 = 总地图点数目 - 坏点（外点）数目 + return nInitialCorrespondences - nBad; +} + +/** + * @brief 使用上一帧+当前帧的视觉信息和IMU信息，优化当前帧位姿 + * + * 可分为以下几个步骤： + * // Step 1：创建g2o优化器，初始化顶点和边 + * // Step 2：启动多轮优化，剔除外点 + * // Step 3：更新当前帧位姿、速度、IMU偏置 + * // Step 4：记录当前帧的优化状态，包括参数信息和边缘化后的海森矩阵 + * + * @param[in] pFrame 当前帧，也是待优化的帧 + * @param[in] bRecInit 调用这个函数的位置并没有传这个参数，因此它的值默认为false + * @return int 返回优化后的内点数 + */ +int Optimizer::PoseInertialOptimizationLastFrame(Frame *pFrame, bool bRecInit) +{ + // Step 1：创建g2o优化器，初始化顶点和边 + // 构建一个稀疏求解器 + g2o::SparseOptimizer optimizer; + g2o::BlockSolverX::LinearSolverType *linearSolver; + + // 使用dense的求解器，（常见非dense求解器有cholmod线性求解器和shur补线性求解器） + linearSolver = new g2o::LinearSolverDense(); + + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); + + // 使用高斯牛顿求解器 + g2o::OptimizationAlgorithmGaussNewton *solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr); + optimizer.setAlgorithm(solver); + optimizer.setVerbose(false); + + // 当前帧单（左）目地图点数目 + int nInitialMonoCorrespondences = 0; + int nInitialStereoCorrespondences = 0; + int nInitialCorrespondences = 0; + + // Set Current Frame vertex + // 当前帧的位姿，旋转+平移，6-dim + VertexPose *VP = new VertexPose(pFrame); + VP->setId(0); + VP->setFixed(false); // 需要优化，所以不固定 + optimizer.addVertex(VP); + // 当前帧的速度，3-dim + VertexVelocity *VV = new VertexVelocity(pFrame); + VV->setId(1); + VV->setFixed(false); + optimizer.addVertex(VV); + // 当前帧的陀螺仪偏置，3-dim + VertexGyroBias *VG = new VertexGyroBias(pFrame); + VG->setId(2); + VG->setFixed(false); + optimizer.addVertex(VG); + // 当前帧的加速度偏置，3-dim + VertexAccBias *VA = new VertexAccBias(pFrame); + VA->setId(3); + VA->setFixed(false); + optimizer.addVertex(VA); + + // Set MapPoint vertices + // 当前帧的特征点总数 N = Nleft + Nright + // 对于单目 N!=0, Nleft=-1，Nright=-1 + // 对于双目 N!=0, Nleft!=-1，Nright!=-1 + const int N = pFrame->N; + // 当前帧左目的特征点总数 + const int Nleft = pFrame->Nleft; + // 当前帧是否存在右目（即是否为双目），存在为true + //?感觉可以更直接点啊 bRight = (Nright!=-1) + const bool bRight = (Nleft != -1); + + vector vpEdgesMono; + vector vpEdgesStereo; + vector vnIndexEdgeMono; + vector vnIndexEdgeStereo; + vpEdgesMono.reserve(N); + vpEdgesStereo.reserve(N); + vnIndexEdgeMono.reserve(N); + vnIndexEdgeStereo.reserve(N); + + // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991 + const float thHuberMono = sqrt(5.991); + // 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815 + const float thHuberStereo = sqrt(7.815); + + { + // 锁定地图点。由于需要使用地图点来构造顶点和边,因此不希望在构造的过程中部分地图点被改写造成不一致甚至是段错误 + unique_lock lock(MapPoint::mGlobalMutex); + + for (int i = 0; i < N; i++) + { + MapPoint *pMP = pFrame->mvpMapPoints[i]; + if (pMP) + { + cv::KeyPoint kpUn; + // Left monocular observation + // 这里说的Left monocular包含两种情况：1.单目情况 2.双目情况下的左目 + if ((!bRight && pFrame->mvuRight[i] < 0) || i < Nleft) + { + // 如果是双目情况下的左目 + if (i < Nleft) // pair left-right + // 使用未畸变校正的特征点 + kpUn = pFrame->mvKeys[i]; + // 如果是单目 + else + // 使用畸变校正过的特征点 + kpUn = pFrame->mvKeysUn[i]; + + // 单目地图点计数增加 + nInitialMonoCorrespondences++; + // 当前地图点默认设置为不是外点 + pFrame->mvbOutlier[i] = false; + // 观测的特征点 + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y; + + // 第一种边(视觉重投影约束)：地图点投影到该帧图像的坐标与特征点坐标偏差尽可能小 + EdgeMonoOnlyPose *e = new EdgeMonoOnlyPose(pMP->GetWorldPos(), 0); + + // 将位姿作为第一个顶点 + e->setVertex(0, VP); + + // 设置观测值，即去畸变后的像素坐标 + e->setMeasurement(obs); + + // Add here uncerteinty + // 获取不确定度，这里调用uncertainty2返回固定值1.0 + // ?这里返回1.0是作为缺省值，是否可以根据对视觉信息的信任度动态修改这个值，比如标定的误差？ + const float unc2 = pFrame->mpCamera->uncertainty2(obs); + + // invSigma2 = (Inverse(协方差矩阵))^2，表明该约束在各个维度上的可信度 + // 图像金字塔层数越高，可信度越差 + const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2; + // 设置该约束的信息矩阵 + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + // 设置鲁棒核函数，避免其误差的平方项出现数值过大的增长 注：后续在优化2次后会用e->setRobustKernel(0)禁掉鲁棒核函数 + e->setRobustKernel(rk); + // 重投影误差的自由度为2，设置对应的卡方阈值 + rk->setDelta(thHuberMono); + + // 将第一种边加入优化器 + optimizer.addEdge(e); + + // 将第一种边加入vpEdgesMono + vpEdgesMono.push_back(e); + // 将对应的特征点索引加入vnIndexEdgeMono + vnIndexEdgeMono.push_back(i); + } + // Stereo observation + else if (!bRight) + { + nInitialStereoCorrespondences++; + pFrame->mvbOutlier[i] = false; + + kpUn = pFrame->mvKeysUn[i]; + const float kp_ur = pFrame->mvuRight[i]; + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y, kp_ur; + + EdgeStereoOnlyPose *e = new EdgeStereoOnlyPose(pMP->GetWorldPos()); + + e->setVertex(0, VP); + e->setMeasurement(obs); + + // Add here uncerteinty + const float unc2 = pFrame->mpCamera->uncertainty2(obs.head(2)); + + const float &invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2; + e->setInformation(Eigen::Matrix3d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberStereo); + + optimizer.addEdge(e); + + vpEdgesStereo.push_back(e); + vnIndexEdgeStereo.push_back(i); + } + + // Right monocular observation + if (bRight && i >= Nleft) + { + nInitialMonoCorrespondences++; + pFrame->mvbOutlier[i] = false; + + kpUn = pFrame->mvKeysRight[i - Nleft]; + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y; + + EdgeMonoOnlyPose *e = new EdgeMonoOnlyPose(pMP->GetWorldPos(), 1); + + e->setVertex(0, VP); + e->setMeasurement(obs); + + // Add here uncerteinty + const float unc2 = pFrame->mpCamera->uncertainty2(obs); + + const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberMono); + + optimizer.addEdge(e); + + vpEdgesMono.push_back(e); + vnIndexEdgeMono.push_back(i); + } + } + } + } + // 统计参与优化的地图点总数目 + nInitialCorrespondences = nInitialMonoCorrespondences + nInitialStereoCorrespondences; + + // Set Previous Frame Vertex + // pFp为上一帧 + Frame *pFp = pFrame->mpPrevFrame; + + // 上一帧的位姿，旋转+平移，6-dim + VertexPose *VPk = new VertexPose(pFp); + VPk->setId(4); + VPk->setFixed(false); + optimizer.addVertex(VPk); + // 上一帧的速度，3-dim + VertexVelocity *VVk = new VertexVelocity(pFp); + VVk->setId(5); + VVk->setFixed(false); + optimizer.addVertex(VVk); + // 上一帧的陀螺仪偏置，3-dim + VertexGyroBias *VGk = new VertexGyroBias(pFp); + VGk->setId(6); + VGk->setFixed(false); + optimizer.addVertex(VGk); + // 上一帧的加速度偏置，3-dim + VertexAccBias *VAk = new VertexAccBias(pFp); + VAk->setId(7); + VAk->setFixed(false); + optimizer.addVertex(VAk); + // setFixed(false)这个设置使以上四个顶点（15个参数）的值随优化而变，这样做会给上一帧再提供一些优化空间 + // 但理论上不应该优化过多，否则会有不良影响，故后面的代码会用第五种边来约束上一帧的变化量 + + // 第二种边（IMU预积分约束）：两帧之间位姿的变化量与IMU预积分的值偏差尽可能小 + EdgeInertial *ei = new EdgeInertial(pFrame->mpImuPreintegratedFrame); + + // 将上一帧四个顶点（P、V、BG、BA）和当前帧两个顶点（P、V）加入第二种边 + ei->setVertex(0, VPk); + ei->setVertex(1, VVk); + ei->setVertex(2, VGk); + ei->setVertex(3, VAk); + ei->setVertex(4, VP); + ei->setVertex(5, VV); + // 把第二种边加入优化器 + optimizer.addEdge(ei); + + // 第三种边（陀螺仪随机游走约束）：陀螺仪的随机游走值在相邻帧间不会相差太多 residual=VG-VGk + // 用大白话来讲就是用固定的VGK拽住VG，防止VG在优化中放飞自我 + EdgeGyroRW *egr = new EdgeGyroRW(); + // 将上一帧的BG加入第三种边 + egr->setVertex(0, VGk); + // 将当前帧的BG加入第三种边 + egr->setVertex(1, VG); + // C值在预积分阶段更新，range(9,12)对应陀螺仪偏置的协方差，最终cvInfoG值为inv(∑(GyroRW^2/freq)) + Eigen::Matrix3d InfoG = pFrame->mpImuPreintegrated->C.block<3, 3>(9, 9).cast().inverse(); + egr->setInformation(InfoG); + // 把第三种边加入优化器 + optimizer.addEdge(egr); + + // 第四种边（加速度随机游走约束）：加速度的随机游走值在相近帧间不会相差太多 residual=VA-VAk + // 用大白话来讲就是用固定的VAK拽住VA，防止VA在优化中放飞自我 + EdgeAccRW *ear = new EdgeAccRW(); + // 将上一帧的BA加入第四种边 + ear->setVertex(0, VAk); + // 将当前帧的BA加入第四种边 + ear->setVertex(1, VA); + // C值在预积分阶段更新，range(12,15)对应加速度偏置的协方差，最终cvInfoG值为inv(∑(AccRW^2/freq)) + Eigen::Matrix3d InfoA = pFrame->mpImuPreintegrated->C.block<3, 3>(12, 12).cast().inverse(); + ear->setInformation(InfoA); + // 把第四种边加入优化器 + optimizer.addEdge(ear); + + // ?既然有判空的操作，可以区分一下有先验信息（五条边）和无先验信息（四条边）的情况 + if (!pFp->mpcpi) + Verbose::PrintMess("pFp->mpcpi does not exist!!!\nPrevious Frame " + to_string(pFp->mnId), Verbose::VERBOSITY_NORMAL); + + // 第五种边（先验约束）：上一帧信息随优化的改变量要尽可能小 + EdgePriorPoseImu *ep = new EdgePriorPoseImu(pFp->mpcpi); + + // 将上一帧的四个顶点（P、V、BG、BA）加入第五种边 + ep->setVertex(0, VPk); + ep->setVertex(1, VVk); + ep->setVertex(2, VGk); + ep->setVertex(3, VAk); + g2o::RobustKernelHuber *rkp = new g2o::RobustKernelHuber; + ep->setRobustKernel(rkp); + rkp->setDelta(5); + // 把第五种边加入优化器 + optimizer.addEdge(ep); + + // Step 2：启动多轮优化，剔除外点 + + // We perform 4 optimizations, after each optimization we classify observation as inlier/outlier + // At the next optimization, outliers are not included, but at the end they can be classified as inliers again. + //与PoseInertialOptimizationLastKeyFrame函数对比，区别在于：在优化过程中保持卡方阈值不变 + // 以下参数的解释 + // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991 + // 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815 + // 自由度为3的卡方分布，显著性水平为0.02，对应的临界阈值9.8 + // 自由度为3的卡方分布，显著性水平为0.001，对应的临界阈值15.6 + // 计算方法：https://stattrek.com/online-calculator/chi-square.aspx + const float chi2Mono[4] = {5.991, 5.991, 5.991, 5.991}; + const float chi2Stereo[4] = {15.6f, 9.8f, 7.815f, 7.815f}; + // 4次优化的迭代次数都为10 + const int its[4] = {10, 10, 10, 10}; + + // 坏点数 + int nBad = 0; + // 单目坏点数 + int nBadMono = 0; + // 双目坏点数 + int nBadStereo = 0; + // 单目内点数 + int nInliersMono = 0; + // 双目内点数 + int nInliersStereo = 0; + // 内点数 + int nInliers = 0; + for (size_t it = 0; it < 4; it++) + { + // 初始化优化器,这里的参数0代表只对level为0的边进行优化（不传参数默认也是0） + optimizer.initializeOptimization(0); + // 每次优化迭代十次 + optimizer.optimize(its[it]); + + // 每次优化都重新统计各类点的数目 + nBad = 0; + nBadMono = 0; + nBadStereo = 0; + nInliers = 0; + nInliersMono = 0; + nInliersStereo = 0; + // 使用1.5倍的chi2Mono作为“近点”的卡方检验值，意味着地图点越近，检验越宽松 + // 地图点如何定义为“近点”在下面的代码中有解释 + float chi2close = 1.5 * chi2Mono[it]; + + for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) + { + EdgeMonoOnlyPose *e = vpEdgesMono[i]; + + const size_t idx = vnIndexEdgeMono[i]; + + // 当地图点在当前帧的深度值小于10时，该地图点属于close（近点） + // mTrackDepth是在Frame.cc的isInFrustum函数中计算出来的 + bool bClose = pFrame->mvpMapPoints[idx]->mTrackDepth < 10.f; + + // 如果这条误差边是来自于outlier + if (pFrame->mvbOutlier[idx]) + { + //计算本次优化后的误差 + e->computeError(); + } + + // 就是error*\Omega*error，表示了这条边考虑置信度以后的误差大小 + const float chi2 = e->chi2(); + + // 判断某地图点为外点的条件有以下三种： + // 1.该地图点不是近点并且误差大于卡方检验值chi2Mono[it] + // 2.该地图点是近点并且误差大于卡方检验值chi2close + // 3.深度不为正 + // 每次优化后，用更小的卡方检验值，原因是随着优化的进行，对划分为内点的信任程度越来越低 + if ((chi2 > chi2Mono[it] && !bClose) || (bClose && chi2 > chi2close) || !e->isDepthPositive()) + { + // 将该点设置为外点 + pFrame->mvbOutlier[idx] = true; + // 外点不参与下一轮优化 + e->setLevel(1); + // 单目坏点数+1 + nBadMono++; + } + else + { + // 将该点设置为内点（暂时） + pFrame->mvbOutlier[idx] = false; + // 内点继续参与下一轮优化 + e->setLevel(0); + // 单目内点数+1 + nInliersMono++; + } + + // 从第三次优化开始就不设置鲁棒核函数了，原因是经过两轮优化已经趋向准确值，不会有太大误差 + if (it == 2) + e->setRobustKernel(0); + } + + for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++) + { + EdgeStereoOnlyPose *e = vpEdgesStereo[i]; + + const size_t idx = vnIndexEdgeStereo[i]; + + if (pFrame->mvbOutlier[idx]) + { + e->computeError(); + } + + const float chi2 = e->chi2(); + + if (chi2 > chi2Stereo[it]) + { + pFrame->mvbOutlier[idx] = true; + e->setLevel(1); + nBadStereo++; + } + else + { + pFrame->mvbOutlier[idx] = false; + e->setLevel(0); + nInliersStereo++; + } + + if (it == 2) + e->setRobustKernel(0); + } + + // 内点总数=单目内点数+双目内点数 + nInliers = nInliersMono + nInliersStereo; + // 坏点数=单目坏点数+双目坏点数 + nBad = nBadMono + nBadStereo; + + if (optimizer.edges().size() < 10) + { + cout << "PIOLF: NOT ENOUGH EDGES" << endl; + break; + } + } + + // 若4次优化后内点数小于30，尝试恢复一部分不那么糟糕的坏点 + if ((nInliers < 30) && !bRecInit) + { + // 重新从0开始统计坏点数 + nBad = 0; + // 单目可容忍的卡方检验最大值（如果误差比这还大就不要挣扎了...） + const float chi2MonoOut = 18.f; + const float chi2StereoOut = 24.f; + EdgeMonoOnlyPose *e1; + EdgeStereoOnlyPose *e2; + // 遍历所有单目特征点 + for (size_t i = 0, iend = vnIndexEdgeMono.size(); i < iend; i++) + { + const size_t idx = vnIndexEdgeMono[i]; + // 获取这些特征点对应的边 + e1 = vpEdgesMono[i]; + e1->computeError(); + if (e1->chi2() < chi2MonoOut) + pFrame->mvbOutlier[idx] = false; + else + nBad++; + } + for (size_t i = 0, iend = vnIndexEdgeStereo.size(); i < iend; i++) + { + const size_t idx = vnIndexEdgeStereo[i]; + e2 = vpEdgesStereo[i]; + e2->computeError(); + if (e2->chi2() < chi2StereoOut) + pFrame->mvbOutlier[idx] = false; + else + nBad++; + } + } + + // ?多此一举？优化过程中nInliers这个值已经计算过了，nInliersMono和nInliersStereo在后续代码中一直保持不变 + nInliers = nInliersMono + nInliersStereo; + + // Step 3：更新当前帧位姿、速度、IMU偏置 + + // Recover optimized pose, velocity and biases + // 给当前帧设置优化后的旋转、位移、速度，用来更新位姿 + pFrame->SetImuPoseVelocity(VP->estimate().Rwb.cast(), VP->estimate().twb.cast(), VV->estimate().cast()); + Vector6d b; + b << VG->estimate(), VA->estimate(); + // 给当前帧设置优化后的bg，ba + pFrame->mImuBias = IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2]); + + // Step 4：记录当前帧的优化状态，包括参数信息和边缘化后的海森矩阵 + // Recover Hessian, marginalize previous frame states and generate new prior for frame + // 包含本次优化所有信息矩阵的和，它代表本次优化对确定性的影响 + Eigen::Matrix H; + H.setZero(); + + //第1步，加上EdgeInertial（IMU预积分约束）的海森矩阵 + // ei的定义 + // EdgeInertial* ei = new EdgeInertial(pFrame->mpImuPreintegratedFrame); + // ei->setVertex(0, VPk); + // ei->setVertex(1, VVk); + // ei->setVertex(2, VGk); + // ei->setVertex(3, VAk); + // ei->setVertex(4, VP); // VertexPose* VP = new VertexPose(pFrame); + // ei->setVertex(5, VV); // VertexVelocity* VV = new VertexVelocity(pFrame); + // ei->GetHessian() = J.t * J 下同，不做详细标注了 + // J + // rn + tn vn gn an rn+1 + tn+1 vn+1 + // er Jp1 Jv1 Jg1 Ja1 Jp2 Jv2 + // 角标1表示上一帧，2表示当前帧 + // 6 3 3 3 6 3 + // Jp1.t * Jp1 Jp1.t * Jv1 Jp1.t * Jg1 Jp1.t * Ja1 Jp1.t * Jp2 Jp1.t * Jv2 6 + // Jv1.t * Jp1 Jv1.t * Jv1 Jv1.t * Jg1 Jv1.t * Ja1 Jv1.t * Jp2 Jv1.t * Jv2 3 + // Jg1.t * Jp1 Jg1.t * Jv1 Jg1.t * Jg1 Jg1.t * Ja1 Jg1.t * Jp2 Jg1.t * Jv2 3 + // Ja1.t * Jp1 Ja1.t * Jv1 Ja1.t * Jg1 Ja1.t * Ja1 Ja1.t * Jp2 Ja1.t * Jv2 3 + // Jp2.t * Jp1 Jp2.t * Jv1 Jp2.t * Jg1 Jp2.t * Ja1 Jp2.t * Jp2 Jp2.t * Jv2 6 + // Jv2.t * Jp1 Jv2.t * Jv1 Jv2.t * Jg1 Jv2.t * Ja1 Jv2.t * Jp2 Jv2.t * Jv2 3 + // 所以矩阵是24*24 的 + H.block<24, 24>(0, 0) += ei->GetHessian(); + // 经过这步H变成了 + // 列数 6 3 3 3 6 3 6 + // ---------------------------------------------------------------------------------- 行数 + // Jp1.t * Jp1 Jp1.t * Jv1 Jp1.t * Jg1 Jp1.t * Ja1 Jp1.t * Jp2 Jp1.t * Jv2 0 | 6 + // Jv1.t * Jp1 Jv1.t * Jv1 Jv1.t * Jg1 Jv1.t * Ja1 Jv1.t * Jp2 Jv1.t * Jv2 0 | 3 + // Jg1.t * Jp1 Jg1.t * Jv1 Jg1.t * Jg1 Jg1.t * Ja1 Jg1.t * Jp2 Jg1.t * Jv2 0 | 3 + // Ja1.t * Jp1 Ja1.t * Jv1 Ja1.t * Jg1 Ja1.t * Ja1 Ja1.t * Jp2 Ja1.t * Jv2 0 | 3 + // Jp2.t * Jp1 Jp2.t * Jv1 Jp2.t * Jg1 Jp2.t * Ja1 Jp2.t * Jp2 Jp2.t * Jv2 0 | 6 + // Jv2.t * Jp1 Jv2.t * Jv1 Jv2.t * Jg1 Jv2.t * Ja1 Jv2.t * Jp2 Jv2.t * Jv2 0 | 3 + // 0 0 0 0 0 0 0 | 6 + // ---------------------------------------------------------------------------------- + + //第2步，加上EdgeGyroRW（陀螺仪随机游走约束）的信息矩阵： + //| 0~8 | 9~11 | 12~23 | 24~26 |27~29 + // 9~11是上一帧的bg(3-dim)，24~26是当前帧的bg(3-dim) + // egr的定义 + // EdgeGyroRW* egr = new EdgeGyroRW(); + // egr->setVertex(0, VGk); + // egr->setVertex(1, VG); + Eigen::Matrix Hgr = egr->GetHessian(); + H.block<3, 3>(9, 9) += Hgr.block<3, 3>(0, 0); // Jgr1.t * Jgr1 + H.block<3, 3>(9, 24) += Hgr.block<3, 3>(0, 3); // Jgr1.t * Jgr2 + H.block<3, 3>(24, 9) += Hgr.block<3, 3>(3, 0); // Jgr2.t * Jgr1 + H.block<3, 3>(24, 24) += Hgr.block<3, 3>(3, 3); // Jgr2.t * Jgr2 + // 经过这步H变成了 + // 列数 6 3 3 3 6 3 3 3 + // ----------------------------------------------------------------------------------------------------------------- 行数 + // Jp1.t * Jp1 Jp1.t * Jv1 Jp1.t * Jg1 Jp1.t * Ja1 Jp1.t * Jp2 Jp1.t * Jv2 0 0 | 6 + // Jv1.t * Jp1 Jv1.t * Jv1 Jv1.t * Jg1 Jv1.t * Ja1 Jv1.t * Jp2 Jv1.t * Jv2 0 0 | 3 + // Jg1.t * Jp1 Jg1.t * Jv1 Jg1.t * Jg1 + Jgr1.t * Jgr1 Jg1.t * Ja1 Jg1.t * Jp2 Jg1.t * Jv2 Jgr1.t * Jgr2 0 | 3 + // Ja1.t * Jp1 Ja1.t * Jv1 Ja1.t * Jg1 Ja1.t * Ja1 Ja1.t * Jp2 Ja1.t * Jv2 0 0 | 3 + // Jp2.t * Jp1 Jp2.t * Jv1 Jp2.t * Jg1 Jp2.t * Ja1 Jp2.t * Jp2 Jp2.t * Jv2 0 0 | 6 + // Jv2.t * Jp1 Jv2.t * Jv1 Jv2.t * Jg1 Jv2.t * Ja1 Jv2.t * Jp2 Jv2.t * Jv2 0 0 | 3 + // 0 0 Jgr2.t * Jgr1 0 0 0 Jgr2.t * Jgr2 0 | 3 + // 0 0 0 0 0 0 0 0 | 3 + // ----------------------------------------------------------------------------------------------------------------- + + //第3步，加上EdgeAccRW（加速度随机游走约束）的信息矩阵： + //| 0~11 | 12~14 | 15~26 | 27~29 |30 + // 12~14是上一帧的ba(3-dim)，27~29是当前帧的ba(3-dim) + // ear的定义 + // EdgeAccRW* ear = new EdgeAccRW(); + // ear->setVertex(0, VAk); + // ear->setVertex(1, VA); + Eigen::Matrix Har = ear->GetHessian(); + H.block<3, 3>(12, 12) += Har.block<3, 3>(0, 0); // Jar1.t * Jar1 + H.block<3, 3>(12, 27) += Har.block<3, 3>(0, 3); // Jar1.t * Jar2 + H.block<3, 3>(27, 12) += Har.block<3, 3>(3, 0); // Jar2.t * Jar1 + H.block<3, 3>(27, 27) += Har.block<3, 3>(3, 3); // Jar2.t * Jar2 + // 经过这步H变成了 + // 列数 6 3 3 3 6 3 3 3 + // --------------------------------------------------------------------------------------------------------------------------------------------------- 行数 + // | Jp1.t * Jp1 Jp1.t * Jv1 Jp1.t * Jg1 Jp1.t * Ja1 | Jp1.t * Jp2 Jp1.t * Jv2 0 0 | 6 + // | Jv1.t * Jp1 Jv1.t * Jv1 Jv1.t * Jg1 Jv1.t * Ja1 | Jv1.t * Jp2 Jv1.t * Jv2 0 0 | 3 + // | Jg1.t * Jp1 Jg1.t * Jv1 Jg1.t * Jg1 + Jgr1.t * Jgr1 Jg1.t * Ja1 | Jg1.t * Jp2 Jg1.t * Jv2 Jgr1.t * Jgr2 0 | 3 + // | Ja1.t * Jp1 Ja1.t * Jv1 Ja1.t * Jg1 Ja1.t * Ja1 + Jar1.t * Jar1 | Ja1.t * Jp2 Ja1.t * Jv2 0 Jar1.t * Jar2 | 3 + // |-------------------------------------------------------------------------------------------------------------------------------------------------- + // | Jp2.t * Jp1 Jp2.t * Jv1 Jp2.t * Jg1 Jp2.t * Ja1 | Jp2.t * Jp2 Jp2.t * Jv2 0 0 | 6 + // | Jv2.t * Jp1 Jv2.t * Jv1 Jv2.t * Jg1 Jv2.t * Ja1 | Jv2.t * Jp2 Jv2.t * Jv2 0 0 | 3 + // | 0 0 Jgr2.t * Jgr1 0 | 0 0 Jgr2.t * Jgr2 0 | 3 + // | 0 0 0 Jar2.t * Jar1 | 0 0 0 Jar2.t * Jar2 | 3 + // --------------------------------------------------------------------------------------------------------------------------------------------------- + + //第4步，加上EdgePriorPoseImu（先验约束）的信息矩阵： + //| 0~14 | 15~29 + // 0~14是上一帧的P(6-dim)、V(3-dim)、BG(3-dim)、BA(3-dim) + // ep定义 + // EdgePriorPoseImu* ep = new EdgePriorPoseImu(pFp->mpcpi); + // ep->setVertex(0, VPk); + // ep->setVertex(1, VVk); + // ep->setVertex(2, VGk); + // ep->setVertex(3, VAk); + // 6 3 3 3 + // Jp1.t * Jp1 Jp1.t * Jv1 Jp1.t * Jg1 Jp1.t * Ja1 6 + // Jv1.t * Jp1 Jv1.t * Jv1 Jv1.t * Jg1 Jv1.t * Ja1 3 + // Jg1.t * Jp1 Jg1.t * Jv1 Jg1.t * Jg1 Jg1.t * Ja1 3 + // Ja1.t * Jp1 Ja1.t * Jv1 Ja1.t * Jg1 Ja1.t * Ja1 3 + H.block<15, 15>(0, 0) += ep->GetHessian(); // 上一帧 的H矩阵，矩阵太大了不写了。。。总之就是加到下标为1相关的了 + + int tot_in = 0, tot_out = 0; + // 第5步 关于位姿的海森 + for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) + { + EdgeMonoOnlyPose *e = vpEdgesMono[i]; + + const size_t idx = vnIndexEdgeMono[i]; + + if (!pFrame->mvbOutlier[idx]) + { + // 0~14 | 15~20 | 21~29 + // 15~20是当前帧的P(6-dim) + H.block<6, 6>(15, 15) += e->GetHessian(); // 当前帧的H矩阵，矩阵太大了不写了。。。总之就是加到p2相关的了 + tot_in++; + } + else + tot_out++; + } + + for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++) + { + EdgeStereoOnlyPose *e = vpEdgesStereo[i]; + + const size_t idx = vnIndexEdgeStereo[i]; + + if (!pFrame->mvbOutlier[idx]) + { + // 0~14 | 15~20 | 21~29 + H.block<6, 6>(15, 15) += e->GetHessian(); + tot_in++; + } + else + tot_out++; + } + + // H = |B E.t| ------> |0 0| + // |E A| |0 A-E*B.inv*E.t| + H = Marginalize(H, 0, 14); + /* + Marginalize里的函数在此处的调用等效于： + Eigen::JacobiSVD svd(H.block(0, 0, 15, 15), Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::JacobiSVD::SingularValuesType singularValues_inv = svd.singularValues(); + for (int i = 0; i < 15; ++i) + { + if (singularValues_inv(i) > 1e-6) + singularValues_inv(i) = 1.0 / singularValues_inv(i); + else + singularValues_inv(i) = 0; + } + Eigen::MatrixXd invHb = svd.matrixV() * singularValues_inv.asDiagonal() * svd.matrixU().transpose(); + H.block(15, 15, 15, 15) = H.block(15, 15, 15, 15) - H.block(15, 0, 15, 15) * invHb - H.block(0, 15, 15, 15); + */ + + //构造一个ConstraintPoseImu对象，为下一帧提供先验约束 + //构造对象所使用的参数是当前帧P、V、BG、BA的估计值和边缘化后的H矩阵 + pFrame->mpcpi = new ConstraintPoseImu(VP->estimate().Rwb, VP->estimate().twb, VV->estimate(), VG->estimate(), VA->estimate(), H.block<15, 15>(15, 15)); + //下一帧使用的EdgePriorPoseImu参数来自于此 + delete pFp->mpcpi; + pFp->mpcpi = NULL; + + //返回值：内点数 = 总地图点数目 - 坏点（外点）数目 + return nInitialCorrespondences - nBad; +} + +/** + * @brief PoseInertialOptimizationLastFrame 中使用 Marginalize(H, 0, 14); + * 使用舒尔补的方式边缘化海森矩阵，边缘化。 + * 列数 6 3 3 3 6 3 3 3 + * --------------------------------------------------------------------------------------------------------------------------------------------------- 行数 + * | Jp1.t * Jp1 Jp1.t * Jv1 Jp1.t * Jg1 Jp1.t * Ja1 | Jp1.t * Jp2 Jp1.t * Jv2 0 0 | 6 + * | Jv1.t * Jp1 Jv1.t * Jv1 Jv1.t * Jg1 Jv1.t * Ja1 | Jv1.t * Jp2 Jv1.t * Jv2 0 0 | 3 + * | Jg1.t * Jp1 Jg1.t * Jv1 Jg1.t * Jg1 + Jgr1.t * Jgr1 Jg1.t * Ja1 | Jg1.t * Jp2 Jg1.t * Jv2 Jgr1.t * Jgr2 0 | 3 + * | Ja1.t * Jp1 Ja1.t * Jv1 Ja1.t * Jg1 Ja1.t * Ja1 + Jar1.t * Jar1 | Ja1.t * Jp2 Ja1.t * Jv2 Jar1.t * Jar2 0 | 3 + * |-------------------------------------------------------------------------------------------------------------------------------------------------- + * | Jp2.t * Jp1 Jp2.t * Jv1 Jp2.t * Jg1 Jp2.t * Ja1 | Jp2.t * Jp2 Jp2.t * Jv2 0 0 | 6 + * | Jv2.t * Jp1 Jv2.t * Jv1 Jv2.t * Jg1 Jv2.t * Ja1 | Jv2.t * Jp2 Jv2.t * Jv2 0 0 | 3 + * | 0 0 Jgr2.t * Jgr1 0 | 0 0 Jgr2.t * Jgr2 0 | 3 + * | 0 0 0 Jar2.t * Jar1 | 0 0 0 Jar2.t * Jar2 | 3 + * --------------------------------------------------------------------------------------------------------------------------------------------------- + * @param H 30*30的海森矩阵 + * @param start 开始位置 + * @param end 结束位置 + */ +Eigen::MatrixXd Optimizer::Marginalize(const Eigen::MatrixXd &H, const int &start, const int &end) +{ + // Goal + // a | ab | ac a* | 0 | ac* + // ba | b | bc --> 0 | 0 | 0 + // ca | cb | c ca* | 0 | c* + + // Size of block before block to marginalize + const int a = start; + // Size of block to marginalize + const int b = end - start + 1; + // Size of block after block to marginalize + const int c = H.cols() - (end + 1); + + // Reorder as follows: + // a | ab | ac a | ac | ab + // ba | b | bc --> ca | c | cb + // ca | cb | c ba | bc | b + + Eigen::MatrixXd Hn = Eigen::MatrixXd::Zero(H.rows(), H.cols()); + if (a > 0) + { + Hn.block(0, 0, a, a) = H.block(0, 0, a, a); + Hn.block(0, a + c, a, b) = H.block(0, a, a, b); + Hn.block(a + c, 0, b, a) = H.block(a, 0, b, a); + } + if (a > 0 && c > 0) + { + Hn.block(0, a, a, c) = H.block(0, a + b, a, c); + Hn.block(a, 0, c, a) = H.block(a + b, 0, c, a); + } + if (c > 0) + { + Hn.block(a, a, c, c) = H.block(a + b, a + b, c, c); + Hn.block(a, a + c, c, b) = H.block(a + b, a, c, b); + Hn.block(a + c, a, b, c) = H.block(a, a + b, b, c); + } + Hn.block(a + c, a + c, b, b) = H.block(a, a, b, b); + + // Perform marginalization (Schur complement) + Eigen::JacobiSVD svd(Hn.block(a + c, a + c, b, b), Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::JacobiSVD::SingularValuesType singularValues_inv = svd.singularValues(); + for (int i = 0; i < b; ++i) + { + if (singularValues_inv(i) > 1e-6) + singularValues_inv(i) = 1.0 / singularValues_inv(i); + else + singularValues_inv(i) = 0; + } + Eigen::MatrixXd invHb = svd.matrixV() * singularValues_inv.asDiagonal() * svd.matrixU().transpose(); + Hn.block(0, 0, a + c, a + c) = Hn.block(0, 0, a + c, a + c) - Hn.block(0, a + c, a + c, b) * invHb * Hn.block(a + c, 0, b, a + c); + Hn.block(a + c, a + c, b, b) = Eigen::MatrixXd::Zero(b, b); + Hn.block(0, a + c, a + c, b) = Eigen::MatrixXd::Zero(a + c, b); + Hn.block(a + c, 0, b, a + c) = Eigen::MatrixXd::Zero(b, a + c); + + // Inverse reorder + // a* | ac* | 0 a* | 0 | ac* + // ca* | c* | 0 --> 0 | 0 | 0 + // 0 | 0 | 0 ca* | 0 | c* + Eigen::MatrixXd res = Eigen::MatrixXd::Zero(H.rows(), H.cols()); + if (a > 0) + { + res.block(0, 0, a, a) = Hn.block(0, 0, a, a); + res.block(0, a, a, b) = Hn.block(0, a + c, a, b); + res.block(a, 0, b, a) = Hn.block(a + c, 0, b, a); + } + if (a > 0 && c > 0) + { + res.block(0, a + b, a, c) = Hn.block(0, a, a, c); + res.block(a + b, 0, c, a) = Hn.block(a, 0, c, a); + } + if (c > 0) + { + res.block(a + b, a + b, c, c) = Hn.block(a, a, c, c); + res.block(a + b, a, c, b) = Hn.block(a, a + c, c, b); + res.block(a, a + b, b, c) = Hn.block(a + c, a, b, c); + } + + res.block(a, a, b, b) = Hn.block(a + c, a + c, b, b); + + return res; +} + + +/**************************************以下为局部地图优化**************************************************************/ + +/** + * @brief Local Bundle Adjustment LocalMapping::Run() 使用，纯视觉 + * @param pKF KeyFrame + * @param pbStopFlag 是否停止优化的标志 + * @param pMap 在优化后，更新状态时需要用到Map的互斥量mMutexMapUpdate + * @param 剩下的都是调试用的 + * + * 总结下与ORBSLAM2的不同 + * 前面操作基本一样，但优化时2代去掉了误差大的点又进行优化了，3代只是统计但没有去掉继续优化，而后都是将误差大的点干掉 + */ +void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int &num_fixedKF, int &num_OptKF, int &num_MPs, int &num_edges) +{ + // 该优化函数用于LocalMapping线程的局部BA优化 + // Local KeyFrames: First Breath Search from Current Keyframe + list lLocalKeyFrames; + + // 步骤1：将当前关键帧加入lLocalKeyFrames + lLocalKeyFrames.push_back(pKF); + pKF->mnBALocalForKF = pKF->mnId; + Map *pCurrentMap = pKF->GetMap(); + + // 步骤2：找到关键帧连接的关键帧（一级相连），加入lLocalKeyFrames中 + const vector vNeighKFs = pKF->GetVectorCovisibleKeyFrames(); + for (int i = 0, iend = vNeighKFs.size(); i < iend; i++) + { + KeyFrame *pKFi = vNeighKFs[i]; + // 记录局部优化id，该数为不断变化，数值等于局部化的关键帧的id，该id用于防止重复添加 + pKFi->mnBALocalForKF = pKF->mnId; + if (!pKFi->isBad() && pKFi->GetMap() == pCurrentMap) + lLocalKeyFrames.push_back(pKFi); + } + + // Local MapPoints seen in Local KeyFrames + num_fixedKF = 0; + // 步骤3：遍历lLocalKeyFrames中关键帧，将它们观测的MapPoints加入到lLocalMapPoints + list lLocalMapPoints; + set sNumObsMP; + for (list::iterator lit = lLocalKeyFrames.begin(), lend = lLocalKeyFrames.end(); lit != lend; lit++) + { + KeyFrame *pKFi = *lit; + if (pKFi->mnId == pMap->GetInitKFid()) + { + num_fixedKF = 1; + } + vector vpMPs = pKFi->GetMapPointMatches(); + for (vector::iterator vit = vpMPs.begin(), vend = vpMPs.end(); vit != vend; vit++) + { + MapPoint *pMP = *vit; + if (pMP) + if (!pMP->isBad() && pMP->GetMap() == pCurrentMap) + { + + if (pMP->mnBALocalForKF != pKF->mnId) + { + lLocalMapPoints.push_back(pMP); + pMP->mnBALocalForKF = pKF->mnId; + } + } + } + } + + // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes + // 步骤4：得到能被局部MapPoints观测到，但不属于局部关键帧的关键帧，这些关键帧在局部BA优化时不优化 + list lFixedCameras; + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) + { + map> observations = (*lit)->GetObservations(); + for (map>::iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++) + { + KeyFrame *pKFi = mit->first; + + if (pKFi->mnBALocalForKF != pKF->mnId && pKFi->mnBAFixedForKF != pKF->mnId) + { + pKFi->mnBAFixedForKF = pKF->mnId; + if (!pKFi->isBad() && pKFi->GetMap() == pCurrentMap) + lFixedCameras.push_back(pKFi); + } + } + } + // 步骤4.1：相比ORBSLAM2多出了判断固定关键帧的个数，最起码要两个固定的,如果实在没有就把lLocalKeyFrames中最早的KF固定，还是不够再加上第二早的KF固定 + num_fixedKF = lFixedCameras.size() + num_fixedKF; + + // 1.0 版本没有以下这段 + // if (num_fixedKF < 2) + // { + // list::iterator lit = lLocalKeyFrames.begin(); + // int lowerId = pKF->mnId; + // KeyFrame *pLowerKf; + // int secondLowerId = pKF->mnId; + // KeyFrame *pSecondLowerKF; + + // for (; lit != lLocalKeyFrames.end(); lit++) + // { + // KeyFrame *pKFi = *lit; + // if (pKFi == pKF || pKFi->mnId == pMap->GetInitKFid()) + // { + // continue; + // } + + // if (pKFi->mnId < lowerId) + // { + // lowerId = pKFi->mnId; + // pLowerKf = pKFi; + // } + // else if (pKFi->mnId < secondLowerId) + // { + // secondLowerId = pKFi->mnId; + // pSecondLowerKF = pKFi; + // } + // } + // lFixedCameras.push_back(pLowerKf); + // lLocalKeyFrames.remove(pLowerKf); + // num_fixedKF++; + // if (num_fixedKF < 2) + // { + // lFixedCameras.push_back(pSecondLowerKF); + // lLocalKeyFrames.remove(pSecondLowerKF); + // num_fixedKF++; + // } + // } + + if (num_fixedKF == 0) + { + Verbose::PrintMess("LM-LBA: There are 0 fixed KF in the optimizations, LBA aborted", Verbose::VERBOSITY_NORMAL); + return; + } + + // Setup optimizer + // 步骤5：构造g2o优化器 + g2o::SparseOptimizer optimizer; + g2o::BlockSolver_6_3::LinearSolverType *linearSolver; + + linearSolver = new g2o::LinearSolverEigen(); + + g2o::BlockSolver_6_3 *solver_ptr = new g2o::BlockSolver_6_3(linearSolver); + + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + if (pMap->IsInertial()) + solver->setUserLambdaInit(100.0); + + optimizer.setAlgorithm(solver); + optimizer.setVerbose(false); + + if (pbStopFlag) + optimizer.setForceStopFlag(pbStopFlag); + + unsigned long maxKFid = 0; + + // DEBUG LBA + pCurrentMap->msOptKFs.clear(); + pCurrentMap->msFixedKFs.clear(); + + // Set Local KeyFrame vertices + // 步骤6：添加顶点：Pose of Local KeyFrame + for (list::iterator lit = lLocalKeyFrames.begin(), lend = lLocalKeyFrames.end(); lit != lend; lit++) + { + KeyFrame *pKFi = *lit; + g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap(); + Sophus::SE3 Tcw = pKFi->GetPose(); + vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(), Tcw.translation().cast())); + vSE3->setId(pKFi->mnId); + vSE3->setFixed(pKFi->mnId == pMap->GetInitKFid()); + optimizer.addVertex(vSE3); + if (pKFi->mnId > maxKFid) + maxKFid = pKFi->mnId; + // DEBUG LBA + pCurrentMap->msOptKFs.insert(pKFi->mnId); + } + num_OptKF = lLocalKeyFrames.size(); + + // Set Fixed KeyFrame vertices + // 步骤7：添加顶点：Pose of Fixed KeyFrame，注意这里调用了vSE3->setFixed(true)。 + for (list::iterator lit = lFixedCameras.begin(), lend = lFixedCameras.end(); lit != lend; lit++) + { + KeyFrame *pKFi = *lit; + g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap(); + Sophus::SE3 Tcw = pKFi->GetPose(); + vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(), Tcw.translation().cast())); + vSE3->setId(pKFi->mnId); + vSE3->setFixed(true); + optimizer.addVertex(vSE3); + if (pKFi->mnId > maxKFid) + maxKFid = pKFi->mnId; + // DEBUG LBA + pCurrentMap->msFixedKFs.insert(pKFi->mnId); + } + + // Set MapPoint vertices + // 步骤7：添加3D顶点 + // 存放的方式(举例) + // 边id: 1 2 3 4 5 6 7 8 9 + // KFid: 1 2 3 4 1 2 3 2 3 + // MPid: 1 1 1 1 2 2 2 3 3 + // 所以这个个数大约是点数×帧数，实际肯定比这个要少 + const int nExpectedSize = (lLocalKeyFrames.size() + lFixedCameras.size()) * lLocalMapPoints.size(); + + // 存放单目时的边 + vector vpEdgesMono; + vpEdgesMono.reserve(nExpectedSize); + // 存放双目鱼眼时另一个相机的边 + vector vpEdgesBody; + vpEdgesBody.reserve(nExpectedSize); + // 存放单目时的KF + vector vpEdgeKFMono; + vpEdgeKFMono.reserve(nExpectedSize); + // 存放单目时的MP + + // 存放双目鱼眼时另一个相机的KF + vector vpEdgeKFBody; + vpEdgeKFBody.reserve(nExpectedSize); + // 存放单目时的MP + vector vpMapPointEdgeMono; + vpMapPointEdgeMono.reserve(nExpectedSize); + // 存放双目鱼眼时另一个相机的MP + vector vpMapPointEdgeBody; + vpMapPointEdgeBody.reserve(nExpectedSize); + + // 存放双目时的边 + vector vpEdgesStereo; + vpEdgesStereo.reserve(nExpectedSize); + // 存放双目时的KF + vector vpEdgeKFStereo; + vpEdgeKFStereo.reserve(nExpectedSize); + // 存放双目时的MP + vector vpMapPointEdgeStereo; + vpMapPointEdgeStereo.reserve(nExpectedSize); + + const float thHuberMono = sqrt(5.991); + const float thHuberStereo = sqrt(7.815); + + int nPoints = 0; + + int nEdges = 0; + + // 添加顶点：MapPoint + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) + { + MapPoint *pMP = *lit; + g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ(); + vPoint->setEstimate(pMP->GetWorldPos().cast()); + int id = pMP->mnId + maxKFid + 1; + vPoint->setId(id); + // 这里的边缘化与滑动窗口不同，而是为了加速稀疏矩阵的计算BlockSolver_6_3默认了6维度的不边缘化，3自由度的三维点被边缘化，所以所有三维点都设置边缘化 + vPoint->setMarginalized(true); + optimizer.addVertex(vPoint); + nPoints++; + + const map> observations = pMP->GetObservations(); + + // Set edges + // 步骤8：对每一对关联的MapPoint和KeyFrame构建边 + for (map>::const_iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++) + { + KeyFrame *pKFi = mit->first; + + if (!pKFi->isBad() && pKFi->GetMap() == pCurrentMap) + { + const int leftIndex = get<0>(mit->second); + + // Monocular observation + // 单目 + if (leftIndex != -1 && pKFi->mvuRight[get<0>(mit->second)] < 0) + { + const cv::KeyPoint &kpUn = pKFi->mvKeysUn[leftIndex]; + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y; + + ORB_SLAM3::EdgeSE3ProjectXYZ *e = new ORB_SLAM3::EdgeSE3ProjectXYZ(); + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); + e->setMeasurement(obs); + const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberMono); + + e->pCamera = pKFi->mpCamera; + + optimizer.addEdge(e); + vpEdgesMono.push_back(e); + vpEdgeKFMono.push_back(pKFi); + vpMapPointEdgeMono.push_back(pMP); + + nEdges++; + } + else if (leftIndex != -1 && pKFi->mvuRight[get<0>(mit->second)] >= 0) // Stereo observation + { + const cv::KeyPoint &kpUn = pKFi->mvKeysUn[leftIndex]; + Eigen::Matrix obs; + const float kp_ur = pKFi->mvuRight[get<0>(mit->second)]; + obs << kpUn.pt.x, kpUn.pt.y, kp_ur; + + g2o::EdgeStereoSE3ProjectXYZ *e = new g2o::EdgeStereoSE3ProjectXYZ(); + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); + e->setMeasurement(obs); + const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; + Eigen::Matrix3d Info = Eigen::Matrix3d::Identity() * invSigma2; + e->setInformation(Info); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberStereo); + + e->fx = pKFi->fx; + e->fy = pKFi->fy; + e->cx = pKFi->cx; + e->cy = pKFi->cy; + e->bf = pKFi->mbf; + + optimizer.addEdge(e); + vpEdgesStereo.push_back(e); + vpEdgeKFStereo.push_back(pKFi); + vpMapPointEdgeStereo.push_back(pMP); + + nEdges++; + } + + if (pKFi->mpCamera2) + { + int rightIndex = get<1>(mit->second); + + if (rightIndex != -1) + { + rightIndex -= pKFi->NLeft; + + Eigen::Matrix obs; + cv::KeyPoint kp = pKFi->mvKeysRight[rightIndex]; + obs << kp.pt.x, kp.pt.y; + + ORB_SLAM3::EdgeSE3ProjectXYZToBody *e = new ORB_SLAM3::EdgeSE3ProjectXYZToBody(); + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); + e->setMeasurement(obs); + const float &invSigma2 = pKFi->mvInvLevelSigma2[kp.octave]; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberMono); + + Sophus::SE3f Trl = pKFi->GetRelativePoseTrl(); + e->mTrl = g2o::SE3Quat(Trl.unit_quaternion().cast(), Trl.translation().cast()); + + e->pCamera = pKFi->mpCamera2; + + optimizer.addEdge(e); + vpEdgesBody.push_back(e); + vpEdgeKFBody.push_back(pKFi); + vpMapPointEdgeBody.push_back(pMP); + + nEdges++; + } + } + } + } + } + num_edges = nEdges; + + if (pbStopFlag) + if (*pbStopFlag) + return; + + // 步骤9：开始优化 + optimizer.initializeOptimization(); + optimizer.optimize(10); + + vector> vToErase; + vToErase.reserve(vpEdgesMono.size() + vpEdgesBody.size() + vpEdgesStereo.size()); + + // Check inlier observations + // 步骤10：在优化后重新计算误差，剔除连接误差比较大的关键帧和MapPoint + for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) + { + ORB_SLAM3::EdgeSE3ProjectXYZ *e = vpEdgesMono[i]; + MapPoint *pMP = vpMapPointEdgeMono[i]; + + if (pMP->isBad()) + continue; + + if (e->chi2() > 5.991 || !e->isDepthPositive()) + { + KeyFrame *pKFi = vpEdgeKFMono[i]; + vToErase.push_back(make_pair(pKFi, pMP)); + } + } + + for (size_t i = 0, iend = vpEdgesBody.size(); i < iend; i++) + { + ORB_SLAM3::EdgeSE3ProjectXYZToBody *e = vpEdgesBody[i]; + MapPoint *pMP = vpMapPointEdgeBody[i]; + + if (pMP->isBad()) + continue; + + if (e->chi2() > 5.991 || !e->isDepthPositive()) + { + KeyFrame *pKFi = vpEdgeKFBody[i]; + vToErase.push_back(make_pair(pKFi, pMP)); + } + } + + for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++) + { + g2o::EdgeStereoSE3ProjectXYZ *e = vpEdgesStereo[i]; + MapPoint *pMP = vpMapPointEdgeStereo[i]; + + if (pMP->isBad()) + continue; + + if (e->chi2() > 7.815 || !e->isDepthPositive()) + { + KeyFrame *pKFi = vpEdgeKFStereo[i]; + vToErase.push_back(make_pair(pKFi, pMP)); + } + } + + // Get Map Mutex + unique_lock lock(pMap->mMutexMapUpdate); + + if (!vToErase.empty()) + { + for (size_t i = 0; i < vToErase.size(); i++) + { + KeyFrame *pKFi = vToErase[i].first; + MapPoint *pMPi = vToErase[i].second; + pKFi->EraseMapPointMatch(pMPi); + pMPi->EraseObservation(pKFi); + } + } + + // Recover optimized data + // Keyframes + bool bShowStats = false; + for (list::iterator lit = lLocalKeyFrames.begin(), lend = lLocalKeyFrames.end(); lit != lend; lit++) + { + KeyFrame *pKFi = *lit; + g2o::VertexSE3Expmap *vSE3 = static_cast(optimizer.vertex(pKFi->mnId)); + g2o::SE3Quat SE3quat = vSE3->estimate(); + Sophus::SE3f Tiw(SE3quat.rotation().cast(), SE3quat.translation().cast()); + pKFi->SetPose(Tiw); + } + + // Points + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) + { + MapPoint *pMP = *lit; + g2o::VertexSBAPointXYZ *vPoint = static_cast(optimizer.vertex(pMP->mnId + maxKFid + 1)); + pMP->SetWorldPos(vPoint->estimate().cast()); + pMP->UpdateNormalAndDepth(); + } + + pMap->IncreaseChangeIndex(); +} + +/** + * @brief 局部地图＋惯导BA LocalMapping IMU中使用，地图经过imu初始化时用这个函数代替LocalBundleAdjustment + * + * @param[in] pKF //关键帧 + * @param[in] pbStopFlag //是否停止的标志 + * @param[in] pMap //地图 + * @param[in] num_fixedKF //固定不优化关键帧的数目 + * @param[in] num_OptKF + * @param[in] num_MPs + * @param[in] num_edges + * @param[in] bLarge 成功跟踪匹配的点数是否足够多 + * @param[in] bRecInit 经过BA2初始化后这个变量为false + */ +void Optimizer::LocalInertialBA( + KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int &num_fixedKF, int &num_OptKF, + int &num_MPs, int &num_edges, bool bLarge, bool bRecInit) +{ + // Step 1. 确定待优化的关键帧们 + Map *pCurrentMap = pKF->GetMap(); + + int maxOpt = 10; // 最大优化关键帧数目 + int opt_it = 10; // 每次优化的迭代次数 + if (bLarge) + { + maxOpt = 25; + opt_it = 4; + } + // 预计待优化的关键帧数，min函数是为了控制优化关键帧的数量 + const int Nd = std::min((int)pCurrentMap->KeyFramesInMap() - 2, maxOpt); + const unsigned long maxKFid = pKF->mnId; + + vector vpOptimizableKFs; + const vector vpNeighsKFs = pKF->GetVectorCovisibleKeyFrames(); + list lpOptVisKFs; + + vpOptimizableKFs.reserve(Nd); + vpOptimizableKFs.push_back(pKF); + pKF->mnBALocalForKF = pKF->mnId; + for (int i = 1; i < Nd; i++) + { + if (vpOptimizableKFs.back()->mPrevKF) + { + vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mPrevKF); + vpOptimizableKFs.back()->mnBALocalForKF = pKF->mnId; + } + else + break; + } + + int N = vpOptimizableKFs.size(); + + // Optimizable points seen by temporal optimizable keyframes + // Step 2. 确定这些关键帧对应的地图点，存入lLocalMapPoints + list lLocalMapPoints; + for (int i = 0; i < N; i++) + { + vector vpMPs = vpOptimizableKFs[i]->GetMapPointMatches(); + for (vector::iterator vit = vpMPs.begin(), vend = vpMPs.end(); vit != vend; vit++) + { + MapPoint *pMP = *vit; + if (pMP) + if (!pMP->isBad()) + if (pMP->mnBALocalForKF != pKF->mnId) + { + lLocalMapPoints.push_back(pMP); + pMP->mnBALocalForKF = pKF->mnId; + } + } + } + + // Fixed Keyframe: First frame previous KF to optimization window) + // Step 3. 固定一帧，为vpOptimizableKFs中最早的那一关键帧的上一关键帧，如果没有上一关键帧了就用最早的那一帧，毕竟目前得到的地图虽然有尺度但并不是绝对的位置 + list lFixedKeyFrames; + if (vpOptimizableKFs.back()->mPrevKF) + { + lFixedKeyFrames.push_back(vpOptimizableKFs.back()->mPrevKF); + vpOptimizableKFs.back()->mPrevKF->mnBAFixedForKF = pKF->mnId; + } + else + { + vpOptimizableKFs.back()->mnBALocalForKF = 0; + vpOptimizableKFs.back()->mnBAFixedForKF = pKF->mnId; + lFixedKeyFrames.push_back(vpOptimizableKFs.back()); + vpOptimizableKFs.pop_back(); + } + + // Optimizable visual KFs + // 4. 做了一系列操作发现最后lpOptVisKFs为空。这段应该是调试遗留代码，如果实现的话其实就是把共视图中在前面没有加过的关键帧们加进来， + // 但作者可能发现之前就把共视图的全部帧加进来了，也有可能发现优化的效果不好浪费时间 + // 获得与当前关键帧有共视关系的一些关键帧，大于15个点，排序为从小到大 + const int maxCovKF = 0; + for (int i = 0, iend = vpNeighsKFs.size(); i < iend; i++) + { + if (lpOptVisKFs.size() >= maxCovKF) + break; + + KeyFrame *pKFi = vpNeighsKFs[i]; + if (pKFi->mnBALocalForKF == pKF->mnId || pKFi->mnBAFixedForKF == pKF->mnId) + continue; + pKFi->mnBALocalForKF = pKF->mnId; + if (!pKFi->isBad() && pKFi->GetMap() == pCurrentMap) + { + lpOptVisKFs.push_back(pKFi); + + vector vpMPs = pKFi->GetMapPointMatches(); + for (vector::iterator vit = vpMPs.begin(), vend = vpMPs.end(); vit != vend; vit++) + { + MapPoint *pMP = *vit; + if (pMP) + if (!pMP->isBad()) + if (pMP->mnBALocalForKF != pKF->mnId) + { + lLocalMapPoints.push_back(pMP); + pMP->mnBALocalForKF = pKF->mnId; + } + } + } + } + + // Fixed KFs which are not covisible optimizable + // 5. 将所有mp点对应的关键帧（除了前面加过的）放入到固定组里面，后面优化时不改变 + const int maxFixKF = 200; + + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) + { + map> observations = (*lit)->GetObservations(); + for (map>::iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++) + { + KeyFrame *pKFi = mit->first; + + if (pKFi->mnBALocalForKF != pKF->mnId && pKFi->mnBAFixedForKF != pKF->mnId) + { + pKFi->mnBAFixedForKF = pKF->mnId; + if (!pKFi->isBad()) + { + lFixedKeyFrames.push_back(pKFi); + break; + } + } + } + if (lFixedKeyFrames.size() >= maxFixKF) + break; + } + + bool bNonFixed = (lFixedKeyFrames.size() == 0); + + // Setup optimizer + // 6. 构造优化器，正式开始优化 + g2o::SparseOptimizer optimizer; + g2o::BlockSolverX::LinearSolverType *linearSolver; + linearSolver = new g2o::LinearSolverEigen(); + + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); + + if (bLarge) + { + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + solver->setUserLambdaInit(1e-2); // to avoid iterating for finding optimal lambda + optimizer.setAlgorithm(solver); + } + else + { + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + solver->setUserLambdaInit(1e0); + optimizer.setAlgorithm(solver); + } + + // Set Local temporal KeyFrame vertices + // 7. 建立关于关键帧的节点，其中包括，位姿，速度，以及两个偏置 + N = vpOptimizableKFs.size(); + for (int i = 0; i < N; i++) + { + KeyFrame *pKFi = vpOptimizableKFs[i]; + + VertexPose *VP = new VertexPose(pKFi); + VP->setId(pKFi->mnId); + VP->setFixed(false); + optimizer.addVertex(VP); + + if (pKFi->bImu) + { + VertexVelocity *VV = new VertexVelocity(pKFi); + VV->setId(maxKFid + 3 * (pKFi->mnId) + 1); + VV->setFixed(false); + optimizer.addVertex(VV); + VertexGyroBias *VG = new VertexGyroBias(pKFi); + VG->setId(maxKFid + 3 * (pKFi->mnId) + 2); + VG->setFixed(false); + optimizer.addVertex(VG); + VertexAccBias *VA = new VertexAccBias(pKFi); + VA->setId(maxKFid + 3 * (pKFi->mnId) + 3); + VA->setFixed(false); + optimizer.addVertex(VA); + } + } + + // Set Local visual KeyFrame vertices + // 8. 建立关于共视关键帧的节点，但这里为空 + for (list::iterator it = lpOptVisKFs.begin(), itEnd = lpOptVisKFs.end(); it != itEnd; it++) + { + KeyFrame *pKFi = *it; + VertexPose *VP = new VertexPose(pKFi); + VP->setId(pKFi->mnId); + VP->setFixed(false); + optimizer.addVertex(VP); + } + + // Set Fixed KeyFrame vertices + // 9. 建立关于固定关键帧的节点，其中包括，位姿，速度，以及两个偏置 + for (list::iterator lit = lFixedKeyFrames.begin(), lend = lFixedKeyFrames.end(); lit != lend; lit++) + { + KeyFrame *pKFi = *lit; + VertexPose *VP = new VertexPose(pKFi); + VP->setId(pKFi->mnId); + VP->setFixed(true); + optimizer.addVertex(VP); + + if (pKFi->bImu) // This should be done only for keyframe just before temporal window + { + VertexVelocity *VV = new VertexVelocity(pKFi); + VV->setId(maxKFid + 3 * (pKFi->mnId) + 1); + VV->setFixed(true); + optimizer.addVertex(VV); + VertexGyroBias *VG = new VertexGyroBias(pKFi); + VG->setId(maxKFid + 3 * (pKFi->mnId) + 2); + VG->setFixed(true); + optimizer.addVertex(VG); + VertexAccBias *VA = new VertexAccBias(pKFi); + VA->setId(maxKFid + 3 * (pKFi->mnId) + 3); + VA->setFixed(true); + optimizer.addVertex(VA); + } + } + + // Create intertial constraints + // 暂时没看到有什么用 + vector vei(N, (EdgeInertial *)NULL); + vector vegr(N, (EdgeGyroRW *)NULL); + vector vear(N, (EdgeAccRW *)NULL); + // 10. 建立惯性边，没有imu跳过 + for (int i = 0; i < N; i++) + { + KeyFrame *pKFi = vpOptimizableKFs[i]; + + if (!pKFi->mPrevKF) + { + cout << "NOT INERTIAL LINK TO PREVIOUS FRAME!!!!" << endl; + continue; + } + if (pKFi->bImu && pKFi->mPrevKF->bImu && pKFi->mpImuPreintegrated) + { + pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias()); + g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId); + g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 1); + g2o::HyperGraph::Vertex *VG1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 2); + g2o::HyperGraph::Vertex *VA1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 3); + g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId); + g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1); + g2o::HyperGraph::Vertex *VG2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2); + g2o::HyperGraph::Vertex *VA2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3); + + if (!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2 || !VG2 || !VA2) + { + cerr << "Error " << VP1 << ", " << VV1 << ", " << VG1 << ", " << VA1 << ", " << VP2 << ", " << VV2 << ", " << VG2 << ", " << VA2 << endl; + continue; + } + + vei[i] = new EdgeInertial(pKFi->mpImuPreintegrated); + + vei[i]->setVertex(0, dynamic_cast(VP1)); + vei[i]->setVertex(1, dynamic_cast(VV1)); + vei[i]->setVertex(2, dynamic_cast(VG1)); + vei[i]->setVertex(3, dynamic_cast(VA1)); + vei[i]->setVertex(4, dynamic_cast(VP2)); + vei[i]->setVertex(5, dynamic_cast(VV2)); + + // 到最早的一个可优化的关键帧或者地图没有ba2时添加鲁棒核函数 + if (i == N - 1 || bRecInit) + { + // All inertial residuals are included without robust cost function, but not that one linking the + // last optimizable keyframe inside of the local window and the first fixed keyframe out. The + // information matrix for this measurement is also downweighted. This is done to avoid accumulating + // error due to fixing variables. + // 所有惯性残差都没有鲁棒核，但不包括窗口内最早一个可优化关键帧与第一个固定关键帧链接起来的惯性残差。 + // 该度量的信息矩阵也被降权。这样做是为了避免由于固定变量而累积错误。 + g2o::RobustKernelHuber *rki = new g2o::RobustKernelHuber; + vei[i]->setRobustKernel(rki); + if (i == N - 1) + vei[i]->setInformation(vei[i]->information() * 1e-2); + rki->setDelta(sqrt(16.92)); + } + optimizer.addEdge(vei[i]); + + vegr[i] = new EdgeGyroRW(); + vegr[i]->setVertex(0, VG1); + vegr[i]->setVertex(1, VG2); + Eigen::Matrix3d InfoG = pKFi->mpImuPreintegrated->C.block<3, 3>(9, 9).cast().inverse(); + vegr[i]->setInformation(InfoG); + optimizer.addEdge(vegr[i]); + + vear[i] = new EdgeAccRW(); + vear[i]->setVertex(0, VA1); + vear[i]->setVertex(1, VA2); + Eigen::Matrix3d InfoA = pKFi->mpImuPreintegrated->C.block<3, 3>(12, 12).cast().inverse(); + vear[i]->setInformation(InfoA); + + optimizer.addEdge(vear[i]); + } + else + cout << "ERROR building inertial edge" << endl; + } + + // Set MapPoint vertices + const int nExpectedSize = (N + lFixedKeyFrames.size()) * lLocalMapPoints.size(); + + // Mono + vector vpEdgesMono; + vpEdgesMono.reserve(nExpectedSize); + + vector vpEdgeKFMono; + vpEdgeKFMono.reserve(nExpectedSize); + + vector vpMapPointEdgeMono; + vpMapPointEdgeMono.reserve(nExpectedSize); + + // Stereo + vector vpEdgesStereo; + vpEdgesStereo.reserve(nExpectedSize); + + vector vpEdgeKFStereo; + vpEdgeKFStereo.reserve(nExpectedSize); + + vector vpMapPointEdgeStereo; + vpMapPointEdgeStereo.reserve(nExpectedSize); + + // 11. 建立视觉边 + const float thHuberMono = sqrt(5.991); + const float chi2Mono2 = 5.991; + const float thHuberStereo = sqrt(7.815); + const float chi2Stereo2 = 7.815; + + const unsigned long iniMPid = maxKFid * 5; + + map mVisEdges; + for (int i = 0; i < N; i++) + { + KeyFrame *pKFi = vpOptimizableKFs[i]; + mVisEdges[pKFi->mnId] = 0; + } + for (list::iterator lit = lFixedKeyFrames.begin(), lend = lFixedKeyFrames.end(); lit != lend; lit++) + { + mVisEdges[(*lit)->mnId] = 0; + } + + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) + { + MapPoint *pMP = *lit; + g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ(); + vPoint->setEstimate(pMP->GetWorldPos().cast()); + + unsigned long id = pMP->mnId + iniMPid + 1; + vPoint->setId(id); + vPoint->setMarginalized(true); + optimizer.addVertex(vPoint); + const map> observations = pMP->GetObservations(); + + // Create visual constraints + for (map>::const_iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++) + { + KeyFrame *pKFi = mit->first; + + if (pKFi->mnBALocalForKF != pKF->mnId && pKFi->mnBAFixedForKF != pKF->mnId) + continue; + + if (!pKFi->isBad() && pKFi->GetMap() == pCurrentMap) + { + const int leftIndex = get<0>(mit->second); + + cv::KeyPoint kpUn; + + // Monocular left observation + if (leftIndex != -1 && pKFi->mvuRight[leftIndex] < 0) + { + mVisEdges[pKFi->mnId]++; + + kpUn = pKFi->mvKeysUn[leftIndex]; + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y; + + EdgeMono *e = new EdgeMono(0); + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); + e->setMeasurement(obs); + + // Add here uncerteinty + const float unc2 = pKFi->mpCamera->uncertainty2(obs); + + const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave] / unc2; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberMono); + + optimizer.addEdge(e); + vpEdgesMono.push_back(e); + vpEdgeKFMono.push_back(pKFi); + vpMapPointEdgeMono.push_back(pMP); + } + // Stereo-observation + else if (leftIndex != -1) // Stereo observation + { + kpUn = pKFi->mvKeysUn[leftIndex]; + mVisEdges[pKFi->mnId]++; + + const float kp_ur = pKFi->mvuRight[leftIndex]; + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y, kp_ur; + + EdgeStereo *e = new EdgeStereo(0); + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); + e->setMeasurement(obs); + + // Add here uncerteinty + const float unc2 = pKFi->mpCamera->uncertainty2(obs.head(2)); + + const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave] / unc2; + e->setInformation(Eigen::Matrix3d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberStereo); + + optimizer.addEdge(e); + vpEdgesStereo.push_back(e); + vpEdgeKFStereo.push_back(pKFi); + vpMapPointEdgeStereo.push_back(pMP); + } + + // Monocular right observation + if (pKFi->mpCamera2) + { + int rightIndex = get<1>(mit->second); + + if (rightIndex != -1) + { + rightIndex -= pKFi->NLeft; + mVisEdges[pKFi->mnId]++; + + Eigen::Matrix obs; + cv::KeyPoint kp = pKFi->mvKeysRight[rightIndex]; + obs << kp.pt.x, kp.pt.y; + + EdgeMono *e = new EdgeMono(1); + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); + e->setMeasurement(obs); + + // Add here uncerteinty + const float unc2 = pKFi->mpCamera->uncertainty2(obs); + + const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave] / unc2; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberMono); + + optimizer.addEdge(e); + vpEdgesMono.push_back(e); + vpEdgeKFMono.push_back(pKFi); + vpMapPointEdgeMono.push_back(pMP); + } + } + } + } + } + + // cout << "Total map points: " << lLocalMapPoints.size() << endl; + // TODO debug会报错先注释掉 + for (map::iterator mit = mVisEdges.begin(), mend = mVisEdges.end(); mit != mend; mit++) + { + assert(mit->second >= 3); + } + + // 12. 开始优化 + optimizer.initializeOptimization(); + optimizer.computeActiveErrors(); + + float err = optimizer.activeRobustChi2(); + optimizer.optimize(opt_it); // Originally to 2 + float err_end = optimizer.activeRobustChi2(); + if (pbStopFlag) + optimizer.setForceStopFlag(pbStopFlag); + + vector> vToErase; + vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size()); + + // 13. 开始确认待删除的连接关系 + // Check inlier observations + // Mono + for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) + { + EdgeMono *e = vpEdgesMono[i]; + MapPoint *pMP = vpMapPointEdgeMono[i]; + bool bClose = pMP->mTrackDepth < 10.f; + + if (pMP->isBad()) + continue; + + if ((e->chi2() > chi2Mono2 && !bClose) || (e->chi2() > 1.5f * chi2Mono2 && bClose) || !e->isDepthPositive()) + { + KeyFrame *pKFi = vpEdgeKFMono[i]; + vToErase.push_back(make_pair(pKFi, pMP)); + } + } + + // Stereo + for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++) + { + EdgeStereo *e = vpEdgesStereo[i]; + MapPoint *pMP = vpMapPointEdgeStereo[i]; + + if (pMP->isBad()) + continue; + + if (e->chi2() > chi2Stereo2) + { + KeyFrame *pKFi = vpEdgeKFStereo[i]; + vToErase.push_back(make_pair(pKFi, pMP)); + } + } + + // Get Map Mutex and erase outliers + unique_lock lock(pMap->mMutexMapUpdate); + + // TODO: Some convergence problems have been detected here + if ((2 * err < err_end || isnan(err) || isnan(err_end)) && !bLarge) // bGN) + { + cout << "FAIL LOCAL-INERTIAL BA!!!!" << endl; + return; + } + + // 14. 删除连接关系 + if (!vToErase.empty()) + { + for (size_t i = 0; i < vToErase.size(); i++) + { + KeyFrame *pKFi = vToErase[i].first; + MapPoint *pMPi = vToErase[i].second; + pKFi->EraseMapPointMatch(pMPi); + pMPi->EraseObservation(pKFi); + } + } + + for (list::iterator lit = lFixedKeyFrames.begin(), lend = lFixedKeyFrames.end(); lit != lend; lit++) + (*lit)->mnBAFixedForKF = 0; + + // 15. 取出结果 + // Recover optimized data + // Local temporal Keyframes + N = vpOptimizableKFs.size(); + for (int i = 0; i < N; i++) + { + KeyFrame *pKFi = vpOptimizableKFs[i]; + + VertexPose *VP = static_cast(optimizer.vertex(pKFi->mnId)); + Sophus::SE3f Tcw(VP->estimate().Rcw[0].cast(), VP->estimate().tcw[0].cast()); + pKFi->SetPose(Tcw); + pKFi->mnBALocalForKF = 0; + + if (pKFi->bImu) + { + VertexVelocity *VV = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1)); + pKFi->SetVelocity(VV->estimate().cast()); + VertexGyroBias *VG = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2)); + VertexAccBias *VA = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3)); + Vector6d b; + b << VG->estimate(), VA->estimate(); + pKFi->SetNewBias(IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2])); + } + } + + // Local visual KeyFrame + // 空 + for (list::iterator it = lpOptVisKFs.begin(), itEnd = lpOptVisKFs.end(); it != itEnd; it++) + { + KeyFrame *pKFi = *it; + VertexPose *VP = static_cast(optimizer.vertex(pKFi->mnId)); + Sophus::SE3f Tcw(VP->estimate().Rcw[0].cast(), VP->estimate().tcw[0].cast()); + pKFi->SetPose(Tcw); + pKFi->mnBALocalForKF = 0; + } + + // Points + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) + { + MapPoint *pMP = *lit; + g2o::VertexSBAPointXYZ *vPoint = static_cast(optimizer.vertex(pMP->mnId + iniMPid + 1)); + pMP->SetWorldPos(vPoint->estimate().cast()); + pMP->UpdateNormalAndDepth(); + } + + pMap->IncreaseChangeIndex(); +} + +/**************************************以下为全局优化**************************************************************/ + +/** + * @brief 全局BA： pMap中所有的MapPoints和关键帧做bundle adjustment优化 + * 这个全局BA优化在本程序中有两个地方使用： + * 1、单目初始化：CreateInitialMapMonocular函数 + * 2、闭环优化：RunGlobalBundleAdjustment函数 + * @param[in] pMap 地图点 + * @param[in] nIterations 迭代次数 + * @param[in] pbStopFlag 外部控制BA结束标志 + * @param[in] nLoopKF 形成了闭环的当前关键帧的id + * @param[in] bRobust 是否使用鲁棒核函数 + */ +void Optimizer::GlobalBundleAdjustemnt(Map *pMap, int nIterations, bool *pbStopFlag, const unsigned long nLoopKF, const bool bRobust) +{ + // 获取地图中的所有关键帧 + vector vpKFs = pMap->GetAllKeyFrames(); + // 获取地图中的所有地图点 + vector vpMP = pMap->GetAllMapPoints(); + // 调用GBA + BundleAdjustment(vpKFs, vpMP, nIterations, pbStopFlag, nLoopKF, bRobust); +} + +/** + * @brief bundle adjustment 优化过程 + * @param[in] vpKFs 参与BA的所有关键帧 + * @param[in] vpMP 参与BA的所有地图点 + * @param[in] nIterations 优化迭代次数 + * @param[in] pbStopFlag 外部控制BA结束标志 + * @param[in] nLoopKF 形成了闭环的当前关键帧的id + * @param[in] bRobust 是否使用核函数 + */ +void Optimizer::BundleAdjustment( + const vector &vpKFs, const vector &vpMP, + int nIterations, bool *pbStopFlag, const unsigned long nLoopKF, const bool bRobust) +{ + // 不参与优化的地图点，下面会用到 + vector vbNotIncludedMP; + vbNotIncludedMP.resize(vpMP.size()); + + Map *pMap = vpKFs[0]->GetMap(); + // Step 1 初始化g2o优化器 + g2o::SparseOptimizer optimizer; + g2o::BlockSolver_6_3::LinearSolverType *linearSolver; + + linearSolver = new g2o::LinearSolverEigen(); + + g2o::BlockSolver_6_3 *solver_ptr = new g2o::BlockSolver_6_3(linearSolver); + + // 使用LM算法优化 + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + optimizer.setAlgorithm(solver); + optimizer.setVerbose(false); + // 如果这个时候外部请求终止，那就结束 + // 注意这句执行之后，外部再请求结束BA，就结束不了了 + if (pbStopFlag) + optimizer.setForceStopFlag(pbStopFlag); + + // 记录添加到优化器中的顶点的最大关键帧id + long unsigned int maxKFid = 0; + + const int nExpectedSize = (vpKFs.size()) * vpMP.size(); + + vector vpEdgesMono; + vpEdgesMono.reserve(nExpectedSize); + + vector vpEdgesBody; + vpEdgesBody.reserve(nExpectedSize); + + vector vpEdgeKFMono; + vpEdgeKFMono.reserve(nExpectedSize); + + vector vpEdgeKFBody; + vpEdgeKFBody.reserve(nExpectedSize); + + vector vpMapPointEdgeMono; + vpMapPointEdgeMono.reserve(nExpectedSize); + + vector vpMapPointEdgeBody; + vpMapPointEdgeBody.reserve(nExpectedSize); + + vector vpEdgesStereo; + vpEdgesStereo.reserve(nExpectedSize); + + vector vpEdgeKFStereo; + vpEdgeKFStereo.reserve(nExpectedSize); + + vector vpMapPointEdgeStereo; + vpMapPointEdgeStereo.reserve(nExpectedSize); + + // Step 2 向优化器添加顶点 + // Set KeyFrame vertices + // Step 2.1 ：向优化器添加关键帧位姿顶点 + // 对于当前地图中的所有关键帧 + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKF = vpKFs[i]; + // 去除无效的 + if (pKF->isBad()) + continue; + + // 对于每一个能用的关键帧构造SE3顶点,其实就是当前关键帧的位姿 + g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap(); + Sophus::SE3 Tcw = pKF->GetPose(); + vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(), Tcw.translation().cast())); + vSE3->setId(pKF->mnId); + // 只有第0帧关键帧不优化（参考基准） + vSE3->setFixed(pKF->mnId == pMap->GetInitKFid()); + // 向优化器中添加顶点，并且更新maxKFid + optimizer.addVertex(vSE3); + if (pKF->mnId > maxKFid) + maxKFid = pKF->mnId; + } + + // 卡方分布 95% 以上可信度的时候的阈值 + const float thHuber2D = sqrt(5.99); // 自由度为2 + const float thHuber3D = sqrt(7.815); // 自由度为3 + + // Set MapPoint vertices + // Step 2.2：向优化器添加MapPoints顶点 + // 遍历地图中的所有地图点 + for (size_t i = 0; i < vpMP.size(); i++) + { + MapPoint *pMP = vpMP[i]; + // 跳过无效地图点 + if (pMP->isBad()) + continue; + + // 创建顶点 + g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ(); + vPoint->setEstimate(pMP->GetWorldPos().cast()); + // 前面记录maxKFid 是在这里使用的 + const int id = pMP->mnId + maxKFid + 1; + vPoint->setId(id); + // g2o在做BA的优化时必须将其所有地图点全部schur掉，否则会出错。 + // 原因是使用了g2o::LinearSolver这个类型来指定linearsolver, + // 其中模板参数当中的位姿矩阵类型在程序中为相机姿态参数的维度，于是BA当中schur消元后解得线性方程组必须是只含有相机姿态变量。 + // Ceres库则没有这样的限制 + vPoint->setMarginalized(true); + optimizer.addVertex(vPoint); + + // 边的关系，其实就是点和关键帧之间观测的关系 + const map> observations = pMP->GetObservations(); + + // 边计数 + int nEdges = 0; + // SET EDGES + // Step 3：向优化器添加投影边（是在遍历地图点、添加地图点的顶点的时候顺便添加的） + // 遍历观察到当前地图点的所有关键帧 + for (map>::const_iterator mit = observations.begin(); mit != observations.end(); mit++) + { + KeyFrame *pKF = mit->first; + // 滤出不合法的关键帧 + if (pKF->isBad() || pKF->mnId > maxKFid) + continue; + if (optimizer.vertex(id) == NULL || optimizer.vertex(pKF->mnId) == NULL) + continue; + nEdges++; + + const int leftIndex = get<0>(mit->second); + + if (leftIndex != -1 && pKF->mvuRight[get<0>(mit->second)] < 0) + { + // 如果是单目相机按照下面操作 + const cv::KeyPoint &kpUn = pKF->mvKeysUn[leftIndex]; + + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y; + + // 创建边 + ORB_SLAM3::EdgeSE3ProjectXYZ *e = new ORB_SLAM3::EdgeSE3ProjectXYZ(); + // 填充数据，构造约束关系 + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKF->mnId))); + e->setMeasurement(obs); + // 信息矩阵，也是协方差，表明了这个约束的观测在各个维度（x,y）上的可信程度，在我们这里对于具体的一个点，两个坐标的可信程度都是相同的， + // 其可信程度受到特征点在图像金字塔中的图层有关，图层越高，可信度越差 + // 为了避免出现信息矩阵中元素为负数的情况，这里使用的是sigma^(-2) + const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave]; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + // 如果要使用鲁棒核函数 + if (bRobust) + { + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + // 这里的重投影误差，自由度为2，所以这里设置为卡方分布中自由度为2的阈值，如果重投影的误差大约大于1个像素的时候，就认为不太靠谱的点了， + // 核函数是为了避免其误差的平方项出现数值上过大的增长 + rk->setDelta(thHuber2D); + } + + // 设置相机内参 + e->pCamera = pKF->mpCamera; + + optimizer.addEdge(e); + + vpEdgesMono.push_back(e); + vpEdgeKFMono.push_back(pKF); + vpMapPointEdgeMono.push_back(pMP); + } + else if (leftIndex != -1 && pKF->mvuRight[leftIndex] >= 0) // Stereo observation + { + // 双目或RGBD相机按照下面操作 + // 双目相机的观测数据则是由三个部分组成：投影点的x坐标，投影点的y坐标，以及投影点在右目中的x坐标（默认y方向上已经对齐了） + + const cv::KeyPoint &kpUn = pKF->mvKeysUn[leftIndex]; + + Eigen::Matrix obs; + const float kp_ur = pKF->mvuRight[get<0>(mit->second)]; + obs << kpUn.pt.x, kpUn.pt.y, kp_ur; + + // 对于双目输入，g2o也有专门的误差边 + g2o::EdgeStereoSE3ProjectXYZ *e = new g2o::EdgeStereoSE3ProjectXYZ(); + // 填充 + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKF->mnId))); + e->setMeasurement(obs); + // 信息矩阵这里是相同的，考虑的是左目特征点的所在图层 + const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave]; + Eigen::Matrix3d Info = Eigen::Matrix3d::Identity() * invSigma2; + e->setInformation(Info); + + // 如果要使用鲁棒核函数 + if (bRobust) + { + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + // 由于现在的观测有三个值，重投影误差会有三个平方项的和组成，因此对应的卡方分布的自由度为3，所以这里设置的也是自由度为3的时候的阈值 + rk->setDelta(thHuber3D); + } + + // 填充相机的基本参数 + e->fx = pKF->fx; + e->fy = pKF->fy; + e->cx = pKF->cx; + e->cy = pKF->cy; + e->bf = pKF->mbf; + + optimizer.addEdge(e); + + vpEdgesStereo.push_back(e); + vpEdgeKFStereo.push_back(pKF); + vpMapPointEdgeStereo.push_back(pMP); + } + + if (pKF->mpCamera2) + { + int rightIndex = get<1>(mit->second); + + if (rightIndex != -1 && rightIndex < pKF->mvKeysRight.size()) + { + rightIndex -= pKF->NLeft; + + Eigen::Matrix obs; + cv::KeyPoint kp = pKF->mvKeysRight[rightIndex]; + obs << kp.pt.x, kp.pt.y; + + ORB_SLAM3::EdgeSE3ProjectXYZToBody *e = new ORB_SLAM3::EdgeSE3ProjectXYZToBody(); + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKF->mnId))); + e->setMeasurement(obs); + const float &invSigma2 = pKF->mvInvLevelSigma2[kp.octave]; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuber2D); + + Sophus::SE3f Trl = pKF->GetRelativePoseTrl(); + e->mTrl = g2o::SE3Quat(Trl.unit_quaternion().cast(), Trl.translation().cast()); + + e->pCamera = pKF->mpCamera2; + + optimizer.addEdge(e); + vpEdgesBody.push_back(e); + vpEdgeKFBody.push_back(pKF); + vpMapPointEdgeBody.push_back(pMP); + } + } + } + + // 如果因为一些特殊原因,实际上并没有任何关键帧观测到当前的这个地图点,那么就删除掉这个顶点,并且这个地图点也就不参与优化 + if (nEdges == 0) + { + optimizer.removeVertex(vPoint); + vbNotIncludedMP[i] = true; + } + else + { + vbNotIncludedMP[i] = false; + } + } + + // Optimize! + // Step 4：开始优化 + optimizer.setVerbose(false); + optimizer.initializeOptimization(); + optimizer.optimize(nIterations); + Verbose::PrintMess("BA: End of the optimization", Verbose::VERBOSITY_NORMAL); + + // Recover optimized data + // Step 5：得到优化的结果 + + // Step 5.1 Keyframes + // 遍历所有的关键帧 + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKF = vpKFs[i]; + if (pKF->isBad()) + continue; + // 获取到优化结果 + g2o::VertexSE3Expmap *vSE3 = static_cast(optimizer.vertex(pKF->mnId)); + + g2o::SE3Quat SE3quat = vSE3->estimate(); + + + if (nLoopKF == pMap->GetOriginKF()->mnId) + { + // 原则上来讲不会出现"当前闭环关键帧是第0帧"的情况,如果这种情况出现,只能够说明是在创建初始地图点的时候调用的这个全局BA函数. + // 这个时候,地图中就只有两个关键帧,其中优化后的位姿数据可以直接写入到帧的成员变量中 + // 老白：视觉初始化时 + pKF->SetPose(Sophus::SE3f(SE3quat.rotation().cast(), SE3quat.translation().cast())); + } + else + { + // 正常的操作,先把优化后的位姿写入到帧的一个专门的成员变量mTcwGBA中备用 + pKF->mTcwGBA = Sophus::SE3d(SE3quat.rotation(), SE3quat.translation()).cast(); + pKF->mnBAGlobalForKF = nLoopKF; // 标记这个关键帧参与了这次全局优化 + + // 下面都是一些调试操作，计算优化前后的位移 + Sophus::SE3f mTwc = pKF->GetPoseInverse(); + Sophus::SE3f mTcGBA_c = pKF->mTcwGBA * mTwc; + Eigen::Vector3f vector_dist = mTcGBA_c.translation(); + double dist = vector_dist.norm(); + if (dist > 1) + { + int numMonoBadPoints = 0, numMonoOptPoints = 0; + int numStereoBadPoints = 0, numStereoOptPoints = 0; + vector vpMonoMPsOpt, vpStereoMPsOpt; + + for (size_t i2 = 0, iend = vpEdgesMono.size(); i2 < iend; i2++) + { + ORB_SLAM3::EdgeSE3ProjectXYZ *e = vpEdgesMono[i2]; + MapPoint *pMP = vpMapPointEdgeMono[i2]; + KeyFrame *pKFedge = vpEdgeKFMono[i2]; + + if (pKF != pKFedge) + { + continue; + } + + if (pMP->isBad()) + continue; + + if (e->chi2() > 5.991 || !e->isDepthPositive()) + { + numMonoBadPoints++; + } + else + { + numMonoOptPoints++; + vpMonoMPsOpt.push_back(pMP); + } + } + + for (size_t i2 = 0, iend = vpEdgesStereo.size(); i2 < iend; i2++) + { + g2o::EdgeStereoSE3ProjectXYZ *e = vpEdgesStereo[i2]; + MapPoint *pMP = vpMapPointEdgeStereo[i2]; + KeyFrame *pKFedge = vpEdgeKFMono[i2]; + + if (pKF != pKFedge) + { + continue; + } + + if (pMP->isBad()) + continue; + + if (e->chi2() > 7.815 || !e->isDepthPositive()) + { + numStereoBadPoints++; + } + else + { + numStereoOptPoints++; + vpStereoMPsOpt.push_back(pMP); + } + } + } + } + } + + // Step 5.2 遍历所有地图点,去除其中没有参与优化过程的地图点 + for (size_t i = 0; i < vpMP.size(); i++) + { + if (vbNotIncludedMP[i]) + continue; + + MapPoint *pMP = vpMP[i]; + + if (pMP->isBad()) + continue; + // 获取优化之后的地图点的位置 + g2o::VertexSBAPointXYZ *vPoint = static_cast(optimizer.vertex(pMP->mnId + maxKFid + 1)); + + if (nLoopKF == pMap->GetOriginKF()->mnId) + { + // 如果这个GBA是在创建初始地图的时候调用的话,那么地图点的位姿也可以直接写入 + pMP->SetWorldPos(vPoint->estimate().cast()); + pMP->UpdateNormalAndDepth(); + } + else + { + // 反之,如果是正常的闭环过程调用,就先临时保存一下 + pMP->mPosGBA = vPoint->estimate().cast(); + pMP->mnBAGlobalForKF = nLoopKF; + } + } +} + +/** + * @brief imu初始化优化，LocalMapping::InitializeIMU中使用 LoopClosing::RunGlobalBundleAdjustment + * 地图全部做BA。也就是imu版的GlobalBundleAdjustemnt + * @param pMap 地图 + * @param its 迭代次数 + * @param bFixLocal 是否固定局部，为false + * @param nLoopId 回环id + * @param pbStopFlag 是否停止的标志 + * @param bInit 提供priorG时为true，此时偏置只优化最后一帧的至0，然后所有关键帧的偏置都赋值为优化后的值 + * 若为false，则建立每两帧之间的偏置边，优化使其相差为0 + * @param priorG 陀螺仪偏置的信息矩阵系数，主动设置时一般bInit为true，也就是只优化最后一帧的偏置，这个数会作为计算信息矩阵时使用 + * @param priorA 加速度计偏置的信息矩阵系数 + * @param vSingVal 没用，估计调试用的 + * @param bHess 没用，估计调试用的 + */ +void Optimizer::FullInertialBA( + Map *pMap, int its, const bool bFixLocal, const long unsigned int nLoopId, bool *pbStopFlag, + bool bInit, float priorG, float priorA, Eigen::VectorXd *vSingVal, bool *bHess) +{ + // 获取地图里所有mp与kf，以及最大kf的id + long unsigned int maxKFid = pMap->GetMaxKFid(); + const vector vpKFs = pMap->GetAllKeyFrames(); + const vector vpMPs = pMap->GetAllMapPoints(); + + // Setup optimizer + // 1. 很经典的一套设置优化器流程 + g2o::SparseOptimizer optimizer; + g2o::BlockSolverX::LinearSolverType *linearSolver; + + linearSolver = new g2o::LinearSolverEigen(); + + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); + + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + solver->setUserLambdaInit(1e-5); + optimizer.setAlgorithm(solver); + optimizer.setVerbose(false); + + if (pbStopFlag) + optimizer.setForceStopFlag(pbStopFlag); + + int nNonFixed = 0; + + // 2. 设置关键帧与偏置节点 + // Set KeyFrame vertices + KeyFrame *pIncKF; // vpKFs中最后一个id符合要求的关键帧 + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKFi = vpKFs[i]; + if (pKFi->mnId > maxKFid) + continue; + VertexPose *VP = new VertexPose(pKFi); + VP->setId(pKFi->mnId); + pIncKF = pKFi; + bool bFixed = false; + if (bFixLocal) + { + bFixed = (pKFi->mnBALocalForKF >= (maxKFid - 1)) || (pKFi->mnBAFixedForKF >= (maxKFid - 1)); + if (!bFixed) + nNonFixed++; + VP->setFixed(bFixed); // 固定，这里可能跟回环有关 + } + optimizer.addVertex(VP); + // 如果是初始化的那几个及后面的关键帧，加入速度节点 + if (pKFi->bImu) + { + VertexVelocity *VV = new VertexVelocity(pKFi); + VV->setId(maxKFid + 3 * (pKFi->mnId) + 1); + VV->setFixed(bFixed); + optimizer.addVertex(VV); + // priorA==0.f 时 bInit为false，也就是又加入了偏置节点 + if (!bInit) + { + VertexGyroBias *VG = new VertexGyroBias(pKFi); + VG->setId(maxKFid + 3 * (pKFi->mnId) + 2); + VG->setFixed(bFixed); + optimizer.addVertex(VG); + VertexAccBias *VA = new VertexAccBias(pKFi); + VA->setId(maxKFid + 3 * (pKFi->mnId) + 3); + VA->setFixed(bFixed); + optimizer.addVertex(VA); + } + } + } + // priorA!=0.f 时 bInit为true，加入了偏置节点 + if (bInit) + { + VertexGyroBias *VG = new VertexGyroBias(pIncKF); + VG->setId(4 * maxKFid + 2); + VG->setFixed(false); + optimizer.addVertex(VG); + VertexAccBias *VA = new VertexAccBias(pIncKF); + VA->setId(4 * maxKFid + 3); + VA->setFixed(false); + optimizer.addVertex(VA); + } + // false + if (bFixLocal) + { + if (nNonFixed < 3) + return; + } + + // 3. 添加关于imu的边 + // IMU links + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKFi = vpKFs[i]; + // 必须有对应的上一个关键帧，感觉跟下面的判定冲突了 + if (!pKFi->mPrevKF) + { + Verbose::PrintMess("NOT INERTIAL LINK TO PREVIOUS FRAME!", Verbose::VERBOSITY_NORMAL); + continue; + } + + if (pKFi->mPrevKF && pKFi->mnId <= maxKFid) + { + if (pKFi->isBad() || pKFi->mPrevKF->mnId > maxKFid) + continue; + // 这两个都必须为初始化后的关键帧 + if (pKFi->bImu && pKFi->mPrevKF->bImu) + { + // 3.1 根据上一帧的偏置设定当前帧的新偏置 + pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias()); + // 3.2 提取节点 + g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId); + g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 1); + + g2o::HyperGraph::Vertex *VG1; + g2o::HyperGraph::Vertex *VA1; + g2o::HyperGraph::Vertex *VG2; + g2o::HyperGraph::Vertex *VA2; + // 根据不同输入配置相应的偏置节点 + if (!bInit) + { + VG1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 2); + VA1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 3); + VG2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2); + VA2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3); + } + else + { + VG1 = optimizer.vertex(4 * maxKFid + 2); + VA1 = optimizer.vertex(4 * maxKFid + 3); + } + + g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId); + g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1); + + if (!bInit) + { + if (!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2 || !VG2 || !VA2) + { + cout << "Error" << VP1 << ", " << VV1 << ", " << VG1 << ", " << VA1 << ", " << VP2 << ", " << VV2 << ", " << VG2 << ", " << VA2 << endl; + continue; + } + } + else + { + if (!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2) + { + cout << "Error" << VP1 << ", " << VV1 << ", " << VG1 << ", " << VA1 << ", " << VP2 << ", " << VV2 << endl; + continue; + } + } + // 3.3 设置边 + EdgeInertial *ei = new EdgeInertial(pKFi->mpImuPreintegrated); + ei->setVertex(0, dynamic_cast(VP1)); + ei->setVertex(1, dynamic_cast(VV1)); + ei->setVertex(2, dynamic_cast(VG1)); + ei->setVertex(3, dynamic_cast(VA1)); + ei->setVertex(4, dynamic_cast(VP2)); + ei->setVertex(5, dynamic_cast(VV2)); + + g2o::RobustKernelHuber *rki = new g2o::RobustKernelHuber; + ei->setRobustKernel(rki); + // 9个自由度的卡方检验（0.05） + rki->setDelta(sqrt(16.92)); + + optimizer.addEdge(ei); + // 加了每一个关键帧的偏置时，还要优化两帧之间偏置的误差 + if (!bInit) + { + EdgeGyroRW *egr = new EdgeGyroRW(); + egr->setVertex(0, VG1); + egr->setVertex(1, VG2); + Eigen::Matrix3d InfoG = pKFi->mpImuPreintegrated->C.block<3, 3>(9, 9).cast().inverse(); + egr->setInformation(InfoG); + egr->computeError(); + optimizer.addEdge(egr); + + EdgeAccRW *ear = new EdgeAccRW(); + ear->setVertex(0, VA1); + ear->setVertex(1, VA2); + Eigen::Matrix3d InfoA = pKFi->mpImuPreintegrated->C.block<3, 3>(12, 12).cast().inverse(); + ear->setInformation(InfoA); + ear->computeError(); + optimizer.addEdge(ear); + } + } + else + cout << pKFi->mnId << " or " << pKFi->mPrevKF->mnId << " no imu" << endl; + } + } + + // 只加入pIncKF帧的偏置，优化偏置到0 + if (bInit) + { + g2o::HyperGraph::Vertex *VG = optimizer.vertex(4 * maxKFid + 2); + g2o::HyperGraph::Vertex *VA = optimizer.vertex(4 * maxKFid + 3); + + // Add prior to comon biases + Eigen::Vector3f bprior; + bprior.setZero(); + + EdgePriorAcc *epa = new EdgePriorAcc(bprior); + epa->setVertex(0, dynamic_cast(VA)); + double infoPriorA = priorA; // + epa->setInformation(infoPriorA * Eigen::Matrix3d::Identity()); + optimizer.addEdge(epa); + + EdgePriorGyro *epg = new EdgePriorGyro(bprior); + epg->setVertex(0, dynamic_cast(VG)); + double infoPriorG = priorG; // + epg->setInformation(infoPriorG * Eigen::Matrix3d::Identity()); + optimizer.addEdge(epg); + } + + const float thHuberMono = sqrt(5.991); + const float thHuberStereo = sqrt(7.815); + + const unsigned long iniMPid = maxKFid * 5; + + vector vbNotIncludedMP(vpMPs.size(), false); + // 5. 添加关于mp的节点与边，这段比较好理解，很传统的视觉上的重投影误差 + for (size_t i = 0; i < vpMPs.size(); i++) + { + MapPoint *pMP = vpMPs[i]; + g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ(); + vPoint->setEstimate(pMP->GetWorldPos().cast()); + unsigned long id = pMP->mnId + iniMPid + 1; + vPoint->setId(id); + vPoint->setMarginalized(true); + optimizer.addVertex(vPoint); + + const map> observations = pMP->GetObservations(); + + bool bAllFixed = true; + + // Set edges + // 遍历所有能观测到这个点的关键帧 + for (map>::const_iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++) + { + KeyFrame *pKFi = mit->first; + + if (pKFi->mnId > maxKFid) + continue; + + if (!pKFi->isBad()) + { + const int leftIndex = get<0>(mit->second); + cv::KeyPoint kpUn; + // 添加边 + if (leftIndex != -1 && pKFi->mvuRight[get<0>(mit->second)] < 0) // Monocular observation + { + kpUn = pKFi->mvKeysUn[leftIndex]; + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y; + + EdgeMono *e = new EdgeMono(0); + + g2o::OptimizableGraph::Vertex *VP = dynamic_cast(optimizer.vertex(pKFi->mnId)); + if (bAllFixed) + if (!VP->fixed()) + bAllFixed = false; + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, VP); + e->setMeasurement(obs); + const float invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; + + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberMono); + + optimizer.addEdge(e); + } + else if (leftIndex != -1 && pKFi->mvuRight[leftIndex] >= 0) // stereo observation + { + kpUn = pKFi->mvKeysUn[leftIndex]; + const float kp_ur = pKFi->mvuRight[leftIndex]; + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y, kp_ur; + + EdgeStereo *e = new EdgeStereo(0); + + g2o::OptimizableGraph::Vertex *VP = dynamic_cast(optimizer.vertex(pKFi->mnId)); + if (bAllFixed) + if (!VP->fixed()) + bAllFixed = false; + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, VP); + e->setMeasurement(obs); + const float invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; + + e->setInformation(Eigen::Matrix3d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberStereo); + + optimizer.addEdge(e); + } + + if (pKFi->mpCamera2) + { // Monocular right observation + int rightIndex = get<1>(mit->second); + + if (rightIndex != -1 && rightIndex < pKFi->mvKeysRight.size()) + { + rightIndex -= pKFi->NLeft; + + Eigen::Matrix obs; + kpUn = pKFi->mvKeysRight[rightIndex]; + obs << kpUn.pt.x, kpUn.pt.y; + + EdgeMono *e = new EdgeMono(1); + + g2o::OptimizableGraph::Vertex *VP = dynamic_cast(optimizer.vertex(pKFi->mnId)); + if (bAllFixed) + if (!VP->fixed()) + bAllFixed = false; + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, VP); + e->setMeasurement(obs); + const float invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberMono); + + optimizer.addEdge(e); + } + } + } + } + + // false + if (bAllFixed) + { + optimizer.removeVertex(vPoint); + vbNotIncludedMP[i] = true; + } + } + + if (pbStopFlag) + if (*pbStopFlag) + return; + + optimizer.initializeOptimization(); + optimizer.optimize(its); + + // 5. 取出优化结果，对应的值赋值 + // Recover optimized data + // Keyframes + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKFi = vpKFs[i]; + if (pKFi->mnId > maxKFid) + continue; + VertexPose *VP = static_cast(optimizer.vertex(pKFi->mnId)); + if (nLoopId == 0) + { + Sophus::SE3f Tcw(VP->estimate().Rcw[0].cast(), VP->estimate().tcw[0].cast()); + pKFi->SetPose(Tcw); + } + else + { + pKFi->mTcwGBA = Sophus::SE3f(VP->estimate().Rcw[0].cast(), VP->estimate().tcw[0].cast()); + pKFi->mnBAGlobalForKF = nLoopId; + } + if (pKFi->bImu) + { + VertexVelocity *VV = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1)); + if (nLoopId == 0) + { + pKFi->SetVelocity(VV->estimate().cast()); + } + else + { + pKFi->mVwbGBA = VV->estimate().cast(); + } + + VertexGyroBias *VG; + VertexAccBias *VA; + if (!bInit) + { + VG = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2)); + VA = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3)); + } + else + { + VG = static_cast(optimizer.vertex(4 * maxKFid + 2)); + VA = static_cast(optimizer.vertex(4 * maxKFid + 3)); + } + + Vector6d vb; + vb << VG->estimate(), VA->estimate(); + IMU::Bias b(vb[3], vb[4], vb[5], vb[0], vb[1], vb[2]); + if (nLoopId == 0) + { + pKFi->SetNewBias(b); + } + else + { + pKFi->mBiasGBA = b; + } + } + } + + // Points + for (size_t i = 0; i < vpMPs.size(); i++) + { + if (vbNotIncludedMP[i]) + continue; + + MapPoint *pMP = vpMPs[i]; + g2o::VertexSBAPointXYZ *vPoint = static_cast(optimizer.vertex(pMP->mnId + iniMPid + 1)); + + if (nLoopId == 0) + { + pMP->SetWorldPos(vPoint->estimate().cast()); + pMP->UpdateNormalAndDepth(); + } + else + { + pMP->mPosGBA = vPoint->estimate().cast(); + pMP->mnBAGlobalForKF = nLoopId; + } + } + + pMap->IncreaseChangeIndex(); +} + +/**************************************以下为尺度与重力优化**************************************************************/ + +/** + * @brief imu初始化优化，LocalMapping::InitializeIMU中使用，其中kf的位姿是固定不变的 + * 优化目标 重力方向，尺度，速度与偏置 + * @param pMap 地图 + * @param Rwg 重力方向到速度方向的转角 + * @param scale 尺度（输出cout用） + * @param bg 陀螺仪偏置（输出cout用） + * @param ba 加速度计偏置（输出cout用） + * @param bMono 是否为单目 + * @param covInertial 惯导协方差矩阵(暂时没用，9*9的0矩阵) + * @param bFixedVel 是否固定速度不优化，false + * @param bGauss 没用，false + * @param priorG 陀螺仪偏置的信息矩阵系数 + * @param priorA 加速度计偏置的信息矩阵系数 + */ +void Optimizer::InertialOptimization( + Map *pMap, Eigen::Matrix3d &Rwg, double &scale, Eigen::Vector3d &bg, Eigen::Vector3d &ba, bool bMono, + Eigen::MatrixXd &covInertial, bool bFixedVel, bool bGauss, float priorG, float priorA) +{ + Verbose::PrintMess("inertial optimization", Verbose::VERBOSITY_NORMAL); + int its = 200; + long unsigned int maxKFid = pMap->GetMaxKFid(); + const vector vpKFs = pMap->GetAllKeyFrames(); // 获取所有关键帧 + + // Setup optimizer + // 1. 构建优化器 + g2o::SparseOptimizer optimizer; + g2o::BlockSolverX::LinearSolverType *linearSolver; + + linearSolver = new g2o::LinearSolverEigen(); + + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); + + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + + if (priorG != 0.f) + solver->setUserLambdaInit(1e3); + + optimizer.setAlgorithm(solver); + + // Set KeyFrame vertices (fixed poses and optimizable velocities) + // 2. 确定关键帧节点（锁住的位姿及可优化的速度） + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKFi = vpKFs[i]; + // 跳过id大于当前地图最大id的关键帧 + if (pKFi->mnId > maxKFid) + continue; + VertexPose *VP = new VertexPose(pKFi); // 继承于public g2o::BaseVertex<6, ImuCamPose> + VP->setId(pKFi->mnId); + VP->setFixed(true); + optimizer.addVertex(VP); + + VertexVelocity *VV = new VertexVelocity(pKFi); // 继承于public g2o::BaseVertex<3, Eigen::Vector3d> + VV->setId(maxKFid + (pKFi->mnId) + 1); + if (bFixedVel) + VV->setFixed(true); + else + VV->setFixed(false); + + optimizer.addVertex(VV); + } + + // Biases + // 3. 确定偏置节点，陀螺仪与加速度计 + VertexGyroBias *VG = new VertexGyroBias(vpKFs.front()); + VG->setId(maxKFid * 2 + 2); + if (bFixedVel) + VG->setFixed(true); + else + VG->setFixed(false); + optimizer.addVertex(VG); + VertexAccBias *VA = new VertexAccBias(vpKFs.front()); + VA->setId(maxKFid * 2 + 3); + if (bFixedVel) + VA->setFixed(true); + else + VA->setFixed(false); + + optimizer.addVertex(VA); + + // prior acc bias + Eigen::Vector3f bprior; + bprior.setZero(); + + // 4. 添加关于加速度计与陀螺仪偏置的边，这两个边加入是保证第一帧的偏置为0 + EdgePriorAcc *epa = new EdgePriorAcc(bprior); + epa->setVertex(0, dynamic_cast(VA)); + double infoPriorA = priorA; + epa->setInformation(infoPriorA * Eigen::Matrix3d::Identity()); + optimizer.addEdge(epa); + EdgePriorGyro *epg = new EdgePriorGyro(bprior); + epg->setVertex(0, dynamic_cast(VG)); + double infoPriorG = priorG; + epg->setInformation(infoPriorG * Eigen::Matrix3d::Identity()); + optimizer.addEdge(epg); + + // Gravity and scale + // 5. 添加关于重力方向与尺度的节点 + VertexGDir *VGDir = new VertexGDir(Rwg); + VGDir->setId(maxKFid * 2 + 4); + VGDir->setFixed(false); + optimizer.addVertex(VGDir); + VertexScale *VS = new VertexScale(scale); + VS->setId(maxKFid * 2 + 5); + VS->setFixed(!bMono); // Fixed for stereo case + optimizer.addVertex(VS); + + // Graph edges + // IMU links with gravity and scale + // 6. imu信息链接重力方向与尺度信息 + vector vpei; // 后面虽然加入了边，但是没有用到，应该调试用的 + vpei.reserve(vpKFs.size()); + vector> vppUsedKF; + vppUsedKF.reserve(vpKFs.size()); // 后面虽然加入了关键帧，但是没有用到，应该调试用的 + // std::cout << "build optimization graph" << std::endl; + + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKFi = vpKFs[i]; + + if (pKFi->mPrevKF && pKFi->mnId <= maxKFid) + { + if (pKFi->isBad() || pKFi->mPrevKF->mnId > maxKFid) + continue; + if (!pKFi->mpImuPreintegrated) + std::cout << "Not preintegrated measurement" << std::endl; + // 到这里的条件是pKFi是好的，并且它有上一个关键帧，且他们的id要小于最大id + // 6.1 检查节点指针是否为空 + // 将pKFi偏置设定为上一关键帧的偏置 + pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias()); + g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId); + g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + (pKFi->mPrevKF->mnId) + 1); + g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId); + g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + (pKFi->mnId) + 1); + g2o::HyperGraph::Vertex *VG = optimizer.vertex(maxKFid * 2 + 2); + g2o::HyperGraph::Vertex *VA = optimizer.vertex(maxKFid * 2 + 3); + g2o::HyperGraph::Vertex *VGDir = optimizer.vertex(maxKFid * 2 + 4); + g2o::HyperGraph::Vertex *VS = optimizer.vertex(maxKFid * 2 + 5); + if (!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS) + { + cout << "Error" << VP1 << ", " << VV1 << ", " << VG << ", " << VA << ", " << VP2 << ", " << VV2 << ", " << VGDir << ", " << VS << endl; + + continue; + } + // 6.2 这是一个大边。。。。包含了上面所有信息，注意到前面的两个偏置也做了两个一元边加入 + EdgeInertialGS *ei = new EdgeInertialGS(pKFi->mpImuPreintegrated); + ei->setVertex(0, dynamic_cast(VP1)); + ei->setVertex(1, dynamic_cast(VV1)); + ei->setVertex(2, dynamic_cast(VG)); + ei->setVertex(3, dynamic_cast(VA)); + ei->setVertex(4, dynamic_cast(VP2)); + ei->setVertex(5, dynamic_cast(VV2)); + ei->setVertex(6, dynamic_cast(VGDir)); + ei->setVertex(7, dynamic_cast(VS)); + + vpei.push_back(ei); + + vppUsedKF.push_back(make_pair(pKFi->mPrevKF, pKFi)); + optimizer.addEdge(ei); + } + } + + // Compute error for different scales + std::set setEdges = optimizer.edges(); + + optimizer.setVerbose(false); + optimizer.initializeOptimization(); + optimizer.optimize(its); + + // 7. 取数 + scale = VS->estimate(); + + // Recover optimized data + // Biases + VG = static_cast(optimizer.vertex(maxKFid * 2 + 2)); + VA = static_cast(optimizer.vertex(maxKFid * 2 + 3)); + Vector6d vb; + vb << VG->estimate(), VA->estimate(); + bg << VG->estimate(); + ba << VA->estimate(); + scale = VS->estimate(); + + IMU::Bias b(vb[3], vb[4], vb[5], vb[0], vb[1], vb[2]); + Rwg = VGDir->estimate().Rwg; + + // Keyframes velocities and biases + const int N = vpKFs.size(); + for (size_t i = 0; i < N; i++) + { + KeyFrame *pKFi = vpKFs[i]; + if (pKFi->mnId > maxKFid) + continue; + + VertexVelocity *VV = static_cast(optimizer.vertex(maxKFid + (pKFi->mnId) + 1)); + Eigen::Vector3d Vw = VV->estimate(); // Velocity is scaled after + pKFi->SetVelocity(Vw.cast()); + + if ((pKFi->GetGyroBias() - bg.cast()).norm() > 0.01) + { + pKFi->SetNewBias(b); + if (pKFi->mpImuPreintegrated) + pKFi->mpImuPreintegrated->Reintegrate(); + } + else + pKFi->SetNewBias(b); + } +} + +/** + * @brief LoopClosing::MergeLocal2 中使用 + * 跟参数最多的那个同名函数不同的地方在于很多节点不可选是否固定，优化的目标有： + * 速度，偏置 + * @param pMap 地图 + * @param bg 陀螺仪偏置 + * @param ba 加速度计偏置 + * @param priorG 陀螺仪偏置的信息矩阵系数 + * @param priorA 加速度计偏置的信息矩阵系数 + */ +void Optimizer::InertialOptimization(Map *pMap, Eigen::Vector3d &bg, Eigen::Vector3d &ba, float priorG, float priorA) +{ + int its = 200; // Check number of iterations + long unsigned int maxKFid = pMap->GetMaxKFid(); + const vector vpKFs = pMap->GetAllKeyFrames(); + + // Setup optimizer + // 1. 构建优化器 + g2o::SparseOptimizer optimizer; + g2o::BlockSolverX::LinearSolverType *linearSolver; + + linearSolver = new g2o::LinearSolverEigen(); + + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); + + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + solver->setUserLambdaInit(1e3); + + optimizer.setAlgorithm(solver); + + // Set KeyFrame vertices (fixed poses and optimizable velocities) + // 2. 构建节点，固定rt + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKFi = vpKFs[i]; + if (pKFi->mnId > maxKFid) + continue; + VertexPose *VP = new VertexPose(pKFi); + VP->setId(pKFi->mnId); + VP->setFixed(true); + optimizer.addVertex(VP); + + VertexVelocity *VV = new VertexVelocity(pKFi); + VV->setId(maxKFid + (pKFi->mnId) + 1); + VV->setFixed(false); + + optimizer.addVertex(VV); + } + + // Biases + // 3. 第一个关键帧的偏置 + VertexGyroBias *VG = new VertexGyroBias(vpKFs.front()); + VG->setId(maxKFid * 2 + 2); + VG->setFixed(false); + optimizer.addVertex(VG); + + VertexAccBias *VA = new VertexAccBias(vpKFs.front()); + VA->setId(maxKFid * 2 + 3); + VA->setFixed(false); + + optimizer.addVertex(VA); + // prior acc bias + Eigen::Vector3f bprior; + bprior.setZero(); + + // 4. 先验边，让偏置趋向于0 + EdgePriorAcc *epa = new EdgePriorAcc(bprior); + epa->setVertex(0, dynamic_cast(VA)); + double infoPriorA = priorA; + epa->setInformation(infoPriorA * Eigen::Matrix3d::Identity()); + optimizer.addEdge(epa); + EdgePriorGyro *epg = new EdgePriorGyro(bprior); + epg->setVertex(0, dynamic_cast(VG)); + double infoPriorG = priorG; + epg->setInformation(infoPriorG * Eigen::Matrix3d::Identity()); + optimizer.addEdge(epg); + + // Gravity and scale + // 5. 注意这里固定了尺度与重力方向，所以只优化了偏置与速度 + VertexGDir *VGDir = new VertexGDir(Eigen::Matrix3d::Identity()); + VGDir->setId(maxKFid * 2 + 4); + VGDir->setFixed(true); + optimizer.addVertex(VGDir); + VertexScale *VS = new VertexScale(1.0); + VS->setId(maxKFid * 2 + 5); + VS->setFixed(true); // Fixed since scale is obtained from already well initialized map + optimizer.addVertex(VS); + + // Graph edges + // IMU links with gravity and scale + vector vpei; + vpei.reserve(vpKFs.size()); + vector> vppUsedKF; + vppUsedKF.reserve(vpKFs.size()); + + // 6. 设置残差边 + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKFi = vpKFs[i]; + + if (pKFi->mPrevKF && pKFi->mnId <= maxKFid) + { + if (pKFi->isBad() || pKFi->mPrevKF->mnId > maxKFid) + continue; + + pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias()); + g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId); + g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + (pKFi->mPrevKF->mnId) + 1); + g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId); + g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + (pKFi->mnId) + 1); + g2o::HyperGraph::Vertex *VG = optimizer.vertex(maxKFid * 2 + 2); + g2o::HyperGraph::Vertex *VA = optimizer.vertex(maxKFid * 2 + 3); + g2o::HyperGraph::Vertex *VGDir = optimizer.vertex(maxKFid * 2 + 4); + g2o::HyperGraph::Vertex *VS = optimizer.vertex(maxKFid * 2 + 5); + if (!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS) + { + cout << "Error" << VP1 << ", " << VV1 << ", " << VG << ", " << VA << ", " << VP2 << ", " << VV2 << ", " << VGDir << ", " << VS << endl; + + continue; + } + EdgeInertialGS *ei = new EdgeInertialGS(pKFi->mpImuPreintegrated); + ei->setVertex(0, dynamic_cast(VP1)); + ei->setVertex(1, dynamic_cast(VV1)); + ei->setVertex(2, dynamic_cast(VG)); + ei->setVertex(3, dynamic_cast(VA)); + ei->setVertex(4, dynamic_cast(VP2)); + ei->setVertex(5, dynamic_cast(VV2)); + ei->setVertex(6, dynamic_cast(VGDir)); + ei->setVertex(7, dynamic_cast(VS)); + + vpei.push_back(ei); + + vppUsedKF.push_back(make_pair(pKFi->mPrevKF, pKFi)); + optimizer.addEdge(ei); + } + } + + // 7. 优化 + // Compute error for different scales + optimizer.setVerbose(false); + optimizer.initializeOptimization(); + optimizer.optimize(its); + + // 8. 取数 + // Recover optimized data + // Biases + VG = static_cast(optimizer.vertex(maxKFid * 2 + 2)); + VA = static_cast(optimizer.vertex(maxKFid * 2 + 3)); + Vector6d vb; + vb << VG->estimate(), VA->estimate(); + bg << VG->estimate(); + ba << VA->estimate(); + + IMU::Bias b(vb[3], vb[4], vb[5], vb[0], vb[1], vb[2]); + + // Keyframes velocities and biases + const int N = vpKFs.size(); + for (size_t i = 0; i < N; i++) + { + KeyFrame *pKFi = vpKFs[i]; + if (pKFi->mnId > maxKFid) + continue; + + VertexVelocity *VV = static_cast(optimizer.vertex(maxKFid + (pKFi->mnId) + 1)); + Eigen::Vector3d Vw = VV->estimate(); + pKFi->SetVelocity(Vw.cast()); + + if ((pKFi->GetGyroBias() - bg.cast()).norm() > 0.01) + { + pKFi->SetNewBias(b); + if (pKFi->mpImuPreintegrated) + pKFi->mpImuPreintegrated->Reintegrate(); + } + else + pKFi->SetNewBias(b); + } +} + +/** + * @brief 优化重力方向与尺度，LocalMapping::ScaleRefinement()中使用，优化目标有： + * 重力方向与尺度 + * @param pMap 地图 + * @param Rwg 重力方向到速度方向的转角 + * @param scale 尺度 + */ +void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale) +{ + int its = 10; + long unsigned int maxKFid = pMap->GetMaxKFid(); + const vector vpKFs = pMap->GetAllKeyFrames(); + + // 1. 构建优化器 + // Setup optimizer + g2o::SparseOptimizer optimizer; + g2o::BlockSolverX::LinearSolverType *linearSolver; + + linearSolver = new g2o::LinearSolverEigen(); + + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); + + g2o::OptimizationAlgorithmGaussNewton *solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr); + optimizer.setAlgorithm(solver); + + // Set KeyFrame vertices (all variables are fixed) + // 2. 添加帧节点，其中包括位姿，速度，两个偏置 + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKFi = vpKFs[i]; + if (pKFi->mnId > maxKFid) + continue; + VertexPose *VP = new VertexPose(pKFi); + VP->setId(pKFi->mnId); + VP->setFixed(true); + optimizer.addVertex(VP); + + VertexVelocity *VV = new VertexVelocity(pKFi); + VV->setId(maxKFid + 1 + (pKFi->mnId)); + VV->setFixed(true); + optimizer.addVertex(VV); + + // Vertex of fixed biases + VertexGyroBias *VG = new VertexGyroBias(vpKFs.front()); + VG->setId(2 * (maxKFid + 1) + (pKFi->mnId)); + VG->setFixed(true); + optimizer.addVertex(VG); + VertexAccBias *VA = new VertexAccBias(vpKFs.front()); + VA->setId(3 * (maxKFid + 1) + (pKFi->mnId)); + VA->setFixed(true); + optimizer.addVertex(VA); + } + + // 3. 添加重力方向与尺度的节点，为优化对象 + // Gravity and scale + VertexGDir *VGDir = new VertexGDir(Rwg); + VGDir->setId(4 * (maxKFid + 1)); + VGDir->setFixed(false); + optimizer.addVertex(VGDir); + VertexScale *VS = new VertexScale(scale); + VS->setId(4 * (maxKFid + 1) + 1); + VS->setFixed(false); + optimizer.addVertex(VS); + + // Graph edges + // 4. 添加边 + int count_edges = 0; + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKFi = vpKFs[i]; + + if (pKFi->mPrevKF && pKFi->mnId <= maxKFid) + { + if (pKFi->isBad() || pKFi->mPrevKF->mnId > maxKFid) + continue; + + g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId); + g2o::HyperGraph::Vertex *VV1 = optimizer.vertex((maxKFid + 1) + pKFi->mPrevKF->mnId); + g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId); + g2o::HyperGraph::Vertex *VV2 = optimizer.vertex((maxKFid + 1) + pKFi->mnId); + g2o::HyperGraph::Vertex *VG = optimizer.vertex(2 * (maxKFid + 1) + pKFi->mPrevKF->mnId); + g2o::HyperGraph::Vertex *VA = optimizer.vertex(3 * (maxKFid + 1) + pKFi->mPrevKF->mnId); + g2o::HyperGraph::Vertex *VGDir = optimizer.vertex(4 * (maxKFid + 1)); + g2o::HyperGraph::Vertex *VS = optimizer.vertex(4 * (maxKFid + 1) + 1); + if (!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS) + { + Verbose::PrintMess("Error" + to_string(VP1->id()) + ", " + to_string(VV1->id()) + ", " + to_string(VG->id()) + ", " + to_string(VA->id()) + ", " + to_string(VP2->id()) + ", " + to_string(VV2->id()) + ", " + to_string(VGDir->id()) + ", " + to_string(VS->id()), Verbose::VERBOSITY_NORMAL); + + continue; + } + count_edges++; + EdgeInertialGS *ei = new EdgeInertialGS(pKFi->mpImuPreintegrated); + ei->setVertex(0, dynamic_cast(VP1)); + ei->setVertex(1, dynamic_cast(VV1)); + ei->setVertex(2, dynamic_cast(VG)); + ei->setVertex(3, dynamic_cast(VA)); + ei->setVertex(4, dynamic_cast(VP2)); + ei->setVertex(5, dynamic_cast(VV2)); + ei->setVertex(6, dynamic_cast(VGDir)); + ei->setVertex(7, dynamic_cast(VS)); + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + ei->setRobustKernel(rk); + rk->setDelta(1.f); + optimizer.addEdge(ei); + } + } + // 5. 优化 + // Compute error for different scales + optimizer.setVerbose(false); + optimizer.initializeOptimization(); + optimizer.computeActiveErrors(); + float err = optimizer.activeRobustChi2(); + optimizer.optimize(its); + optimizer.computeActiveErrors(); + float err_end = optimizer.activeRobustChi2(); + // Recover optimized data + // 6. 取数 + scale = VS->estimate(); + Rwg = VGDir->estimate().Rwg; +} + +/**************************************以下为sim3优化**************************************************************/ + +/** + * @brief LoopClosing::DetectAndReffineSim3FromLastKF使用 + * 1投2， 2投1这么来 只优化g2oS12 + * @param pKF1 当前关键帧 + * @param pKF2 候选关键帧 + * @param vpMatches1 当前关键帧与地图匹配上的MP，中间会有NULL，与当前关键帧的特征点一一对应 + * @param g2oS12 相似变换矩阵 + * @param th2 误差上限的平方 + * @param bFixScale 是否固定尺度，单目或者单目imu未做到3阶段初始化为false + * @param mAcumHessian 计算累计海森矩阵（没啥用） + * @param bAllPoints 是否计算所有点（都为true） + */ +int Optimizer::OptimizeSim3( + KeyFrame *pKF1, KeyFrame *pKF2, vector &vpMatches1, g2o::Sim3 &g2oS12, const float th2, + const bool bFixScale, Eigen::Matrix &mAcumHessian, const bool bAllPoints) +{ + // 1. 初始化g2o优化器 + // 先构造求解器 + g2o::SparseOptimizer optimizer; + // 构造线性方程求解器，Hx = -b的求解器 + g2o::BlockSolverX::LinearSolverType *linearSolver; + // 使用dense的求解器，（常见非dense求解器有cholmod线性求解器和shur补线性求解器） + linearSolver = new g2o::LinearSolverDense(); + + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); + // 使用L-M迭代 + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + optimizer.setAlgorithm(solver); + + // Camera poses + // 2.1 添加Sim3顶点 + const Eigen::Matrix3f R1w = pKF1->GetRotation(); + const Eigen::Vector3f t1w = pKF1->GetTranslation(); + const Eigen::Matrix3f R2w = pKF2->GetRotation(); + const Eigen::Vector3f t2w = pKF2->GetTranslation(); + + // Set Sim3 vertex + ORB_SLAM3::VertexSim3Expmap *vSim3 = new ORB_SLAM3::VertexSim3Expmap(); + vSim3->_fix_scale = bFixScale; + vSim3->setEstimate(g2oS12); + vSim3->setId(0); + vSim3->setFixed(false); + vSim3->pCamera1 = pKF1->mpCamera; + vSim3->pCamera2 = pKF2->mpCamera; + optimizer.addVertex(vSim3); + + // Set MapPoint vertices + // 2.1 添加MP顶点 + const int N = vpMatches1.size(); + const vector vpMapPoints1 = pKF1->GetMapPointMatches(); + vector vpEdges12; // pKF2对应的MapPoints到pKF1的投影 + vector vpEdges21; // pKF1对应的MapPoints到pKF2的投影 + vector vnIndexEdge; + vector vbIsInKF2; + + vnIndexEdge.reserve(2 * N); + vpEdges12.reserve(2 * N); + vpEdges21.reserve(2 * N); + vbIsInKF2.reserve(2 * N); + + const float deltaHuber = sqrt(th2); + + int nCorrespondences = 0; + int nBadMPs = 0; // 没有实际用处，没有输出信息 + int nInKF2 = 0; // 输出信息用 + int nOutKF2 = 0; // 输出信息用 + int nMatchWithoutMP = 0; // 输出信息用 + + vector vIdsOnlyInKF2; + + // 2.2 遍历当前关键帧的所有MP点 + for (int i = 0; i < N; i++) + { + if (!vpMatches1[i]) + continue; + + // pMP1和pMP2是匹配的MapPoints，pMP1表示当前帧正常对应的mp，pMP2表示对应的回环的mp + MapPoint *pMP1 = vpMapPoints1[i]; + MapPoint *pMP2 = vpMatches1[i]; + + // (1, 2) (3, 4) (5, 6) + const int id1 = 2 * i + 1; + const int id2 = 2 * (i + 1); + + // 返回这个点在pKF2关键帧中对应的特征点id + const int i2 = get<0>(pMP2->GetIndexInKeyFrame(pKF2)); + + Eigen::Vector3f P3D1c; // 点1在当前关键帧相机坐标系下的坐标 + Eigen::Vector3f P3D2c; // 点2在候选关键帧相机坐标系下的坐标 + + if (pMP1 && pMP2) + { + if (!pMP1->isBad() && !pMP2->isBad()) + { + // 2.3 添加PointXYZ顶点， 且设为了固定 + g2o::VertexSBAPointXYZ *vPoint1 = new g2o::VertexSBAPointXYZ(); + Eigen::Vector3f P3D1w = pMP1->GetWorldPos(); + P3D1c = R1w * P3D1w + t1w; + vPoint1->setEstimate(P3D1c.cast()); // 点1在当前关键帧下的三维点坐标 + vPoint1->setId(id1); + vPoint1->setFixed(true); + optimizer.addVertex(vPoint1); + + g2o::VertexSBAPointXYZ *vPoint2 = new g2o::VertexSBAPointXYZ(); + Eigen::Vector3f P3D2w = pMP2->GetWorldPos(); + P3D2c = R2w * P3D2w + t2w; + vPoint2->setEstimate(P3D2c.cast()); // 点2在候选关键帧下的三维点坐标 + vPoint2->setId(id2); + vPoint2->setFixed(true); + optimizer.addVertex(vPoint2); + } + else + { + nBadMPs++; + continue; + } + } + else + { + nMatchWithoutMP++; + + // The 3D position in KF1 doesn't exist + if (!pMP2->isBad()) + { + // 执行到这里意味着特征点没有对应的原始MP，却有回环MP，将其投到候选帧里面 + g2o::VertexSBAPointXYZ *vPoint2 = new g2o::VertexSBAPointXYZ(); + Eigen::Vector3f P3D2w = pMP2->GetWorldPos(); + P3D2c = R2w * P3D2w + t2w; + vPoint2->setEstimate(P3D2c.cast()); + vPoint2->setId(id2); + vPoint2->setFixed(true); + optimizer.addVertex(vPoint2); + + vIdsOnlyInKF2.push_back(id2); + } + continue; + } + + if (i2 < 0 && !bAllPoints) + { + Verbose::PrintMess(" Remove point -> i2: " + to_string(i2) + "; bAllPoints: " + to_string(bAllPoints), Verbose::VERBOSITY_DEBUG); + continue; + } + + if (P3D2c(2) < 0) + { + Verbose::PrintMess("Sim3: Z coordinate is negative", Verbose::VERBOSITY_DEBUG); + continue; + } + + nCorrespondences++; + + // 2.4 添加两个顶点（3D点）到相机投影的边 + // Set edge x1 = S12*X2 + Eigen::Matrix obs1; + const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i]; + obs1 << kpUn1.pt.x, kpUn1.pt.y; + + // 这个边的误差计算方式 + // 1. 将点2通过g2oS12计算到当前关键帧下 + // 2. 点2在当前关键帧下投影到图像上与观测求误差 + ORB_SLAM3::EdgeSim3ProjectXYZ *e12 = new ORB_SLAM3::EdgeSim3ProjectXYZ(); + + e12->setVertex(0, dynamic_cast(optimizer.vertex(id2))); // 2相机坐标系下的三维点 + e12->setVertex(1, dynamic_cast(optimizer.vertex(0))); // g2oS12 + e12->setMeasurement(obs1); + const float &invSigmaSquare1 = pKF1->mvInvLevelSigma2[kpUn1.octave]; + e12->setInformation(Eigen::Matrix2d::Identity() * invSigmaSquare1); + + g2o::RobustKernelHuber *rk1 = new g2o::RobustKernelHuber; + e12->setRobustKernel(rk1); + rk1->setDelta(deltaHuber); + optimizer.addEdge(e12); + + // Set edge x2 = S21*X1 + // 2.5 另一个边 + Eigen::Matrix obs2; + cv::KeyPoint kpUn2; + bool inKF2; + // 投之前要确定下这个点的像素坐标 + if (i2 >= 0) + { + kpUn2 = pKF2->mvKeysUn[i2]; + obs2 << kpUn2.pt.x, kpUn2.pt.y; + inKF2 = true; + + nInKF2++; // 输出信息，表示在kf2中找到MP2的点数 + } + else //!BUG 如果没找到，使用三维点投影到KF2中，表示并没有特征点与之对应（把这个结果当做obs2是不是会带来一些误差，而且还不通过内参吗？？？，重大bug） + { + float invz = 1 / P3D2c(2); + float x = P3D2c(0) * invz; + float y = P3D2c(1) * invz; + + // float u = pKF2->fx * x + pKF2->cx; + // float v = pKF2->fy * y + pKF2->cy; + // obs2 << u, v; + // kpUn2 = cv::KeyPoint(cv::Point2f(u, v), pMP2->mnTrackScaleLevel); + obs2 << x, y; + kpUn2 = cv::KeyPoint(cv::Point2f(x, y), pMP2->mnTrackScaleLevel); + + inKF2 = false; + nOutKF2++; + } + + // 1相机坐标系下的三维点经过g2oS12投影到kf2下计算重投影误差 + ORB_SLAM3::EdgeInverseSim3ProjectXYZ *e21 = new ORB_SLAM3::EdgeInverseSim3ProjectXYZ(); + + e21->setVertex(0, dynamic_cast(optimizer.vertex(id1))); + e21->setVertex(1, dynamic_cast(optimizer.vertex(0))); + e21->setMeasurement(obs2); + float invSigmaSquare2 = pKF2->mvInvLevelSigma2[kpUn2.octave]; + e21->setInformation(Eigen::Matrix2d::Identity() * invSigmaSquare2); + + g2o::RobustKernelHuber *rk2 = new g2o::RobustKernelHuber; + e21->setRobustKernel(rk2); + rk2->setDelta(deltaHuber); + optimizer.addEdge(e21); + + vpEdges12.push_back(e12); + vpEdges21.push_back(e21); + vnIndexEdge.push_back(i); + + vbIsInKF2.push_back(inKF2); + } + + // Optimize! + // 3. 开始优化 + optimizer.initializeOptimization(); + optimizer.optimize(5); + + // Check inliers + // 4.剔除一些误差大的边，因为e12与e21对应的是同一个三维点，所以只要有一个误差太大就直接搞掉 + // Check inliers + // 进行卡方检验，大于阈值的边剔除，同时删除鲁棒核函数 + int nBad = 0; + int nBadOutKF2 = 0; + for (size_t i = 0; i < vpEdges12.size(); i++) + { + ORB_SLAM3::EdgeSim3ProjectXYZ *e12 = vpEdges12[i]; + ORB_SLAM3::EdgeInverseSim3ProjectXYZ *e21 = vpEdges21[i]; + if (!e12 || !e21) + continue; + + if (e12->chi2() > th2 || e21->chi2() > th2) + { + size_t idx = vnIndexEdge[i]; + vpMatches1[idx] = static_cast(NULL); + optimizer.removeEdge(e12); + optimizer.removeEdge(e21); + vpEdges12[i] = static_cast(NULL); + vpEdges21[i] = static_cast(NULL); + nBad++; + + if (!vbIsInKF2[i]) + { + nBadOutKF2++; + } + continue; + } + + // Check if remove the robust adjustment improve the result + e12->setRobustKernel(0); + e21->setRobustKernel(0); + } + + // 如果有坏点，迭代次数更多 + int nMoreIterations; + if (nBad > 0) + nMoreIterations = 10; + else + nMoreIterations = 5; + + if (nCorrespondences - nBad < 10) + return 0; + + // Optimize again only with inliers + // 5. 再一次优化 + optimizer.initializeOptimization(); + optimizer.optimize(nMoreIterations); + + int nIn = 0; + mAcumHessian = Eigen::MatrixXd::Zero(7, 7); + // 更新vpMatches1，删除外点，统计内点数量 + for (size_t i = 0; i < vpEdges12.size(); i++) + { + ORB_SLAM3::EdgeSim3ProjectXYZ *e12 = vpEdges12[i]; + ORB_SLAM3::EdgeInverseSim3ProjectXYZ *e21 = vpEdges21[i]; + if (!e12 || !e21) + continue; + + e12->computeError(); + e21->computeError(); + + if (e12->chi2() > th2 || e21->chi2() > th2) + { + size_t idx = vnIndexEdge[i]; + vpMatches1[idx] = static_cast(NULL); + } + else + { + nIn++; + } + } + + // Recover optimized Sim3、 + // 6.得到优化后的结果 + g2o::VertexSim3Expmap *vSim3_recov = static_cast(optimizer.vertex(0)); + g2oS12 = vSim3_recov->estimate(); + + return nIn; +} + +/**************************************以下为地图回环优化**************************************************************/ + +/** + * @brief LoopClosing::CorrectLoop() 回环矫正时使用，纯视觉，全局本质图优化 + * 优化目标： 地图中所有MP与关键帧 + * @param pMap 当前的map + * @param pLoopKF mpLoopMatchedKF 与 mpCurrentKF 匹配的关键帧 + * @param pCurKF mpCurrentKF 当前关键帧 + * @param NonCorrectedSim3 通过pKFi->GetPose()计算的放NonCorrectedSim3也就是回环前的位姿 这里面的帧只是与mpCurrentKF相关联的 + * @param CorrectedSim3 通过mg2oLoopScw计算的放CorrectedSim3 这里面的帧只是与mpCurrentKF相关联的 + * @param LoopConnections 因为回环而建立的新的帧与帧的连接关系，里面的key 为pCurKF共视帧 value为共视帧的共视帧 + * @param bFixScale 单目imu且未到第三次imu初始化为false + */ +void Optimizer::OptimizeEssentialGraph( + Map *pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF, + const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, + const LoopClosing::KeyFrameAndPose &CorrectedSim3, + const map> &LoopConnections, const bool &bFixScale) +{ + // Setup optimizer + // Step 1：构造优化器 + g2o::SparseOptimizer optimizer; + optimizer.setVerbose(false); + // 指定线性方程求解器使用Eigen的块求解器 + // 7表示位姿是sim3 3表示三维点坐标维度 + g2o::BlockSolver_7_3::LinearSolverType *linearSolver = + new g2o::LinearSolverEigen(); + // 构造线性求解器 + g2o::BlockSolver_7_3 *solver_ptr = new g2o::BlockSolver_7_3(linearSolver); + // 使用LM算法进行非线性迭代 + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + + // 第一次迭代的初始lambda值，如未指定会自动计算一个合适的值 + solver->setUserLambdaInit(1e-16); + optimizer.setAlgorithm(solver); + + // 获取当前地图中的所有关键帧 和地图点 + const vector vpKFs = pMap->GetAllKeyFrames(); + const vector vpMPs = pMap->GetAllMapPoints(); + + // 最大关键帧id，用于添加顶点时使用 + const unsigned int nMaxKFid = pMap->GetMaxKFid(); + + // 记录所有优化前关键帧的位姿，优先使用在闭环时通过Sim3传播调整过的Sim3位姿 + vector> vScw(nMaxKFid + 1); // 存放每一帧优化前的sim3 + // 记录所有关键帧经过本次本质图优化过的位姿 + vector> vCorrectedSwc(nMaxKFid + 1); // 存放每一帧优化后的sim3，修正mp位姿用 + // 这个变量没有用 + vector vpVertices(nMaxKFid + 1); // 存放节点，没用，还占地方 + + // 调试用，暂时不去管 + vector vZvectors(nMaxKFid + 1); // For debugging + Eigen::Vector3d z_vec; + z_vec << 0.0, 0.0, 1.0; + // 两个关键帧之间共视关系的权重（也就是共视点的数目，单目x1，双目/rgbd x2）的最小值 + const int minFeat = 100; // MODIFICATION originally was set to 100 + + // Set KeyFrame vertices + // Step 2：将地图中所有关键帧的pose作为顶点添加到优化器 + // 尽可能使用经过Sim3调整的位姿 + // 遍历全局地图中的所有的关键帧 + for (size_t i = 0, iend = vpKFs.size(); i < iend; i++) + { + KeyFrame *pKF = vpKFs[i]; + if (pKF->isBad()) + continue; + g2o::VertexSim3Expmap *VSim3 = new g2o::VertexSim3Expmap(); + // 关键帧在所有关键帧中的id，用来设置为顶点的id + const int nIDi = pKF->mnId; + + LoopClosing::KeyFrameAndPose::const_iterator it = CorrectedSim3.find(pKF); + + if (it != CorrectedSim3.end()) + { + // 如果该关键帧在闭环时通过Sim3传播调整过，优先用调整后的Sim3位姿 + vScw[nIDi] = it->second; + VSim3->setEstimate(it->second); + } + else + { + // 如果该关键帧在闭环时没有通过Sim3传播调整过，用跟踪时的位姿 + Sophus::SE3d Tcw = pKF->GetPose().cast(); + g2o::Sim3 Siw(Tcw.unit_quaternion(), Tcw.translation(), 1.0); + vScw[nIDi] = Siw; + VSim3->setEstimate(Siw); + } + // 固定第一帧 + if (pKF->mnId == pMap->GetInitKFid()) + VSim3->setFixed(true); + + VSim3->setId(nIDi); + VSim3->setMarginalized(false); + // 和当前系统的传感器有关，如果是RGBD或者是双目，那么就不需要优化sim3的缩放系数，保持为1即可 + VSim3->_fix_scale = bFixScale; + + optimizer.addVertex(VSim3); + vZvectors[nIDi] = vScw[nIDi].rotation() * z_vec; // For debugging + // 优化前的pose顶点，后面代码中没有使用 + vpVertices[nIDi] = VSim3; + } + + // 保存由于闭环后优化sim3而出现的新的关键帧和关键帧之间的连接关系,因为回环而新建立的连接关系，其中id比较小的关键帧在前,id比较大的关键帧在后 + set> sInsertedEdges; + + const Eigen::Matrix matLambda = Eigen::Matrix::Identity(); + + // Set Loop edges + // Step 3：添加第1种边：LoopConnections是闭环时因为MapPoints调整而出现的新关键帧连接关系（包括当前帧与闭环匹配帧之间的连接关系） + int count_loop = 0; + for (map>::const_iterator mit = LoopConnections.begin(), mend = LoopConnections.end(); mit != mend; mit++) + { + // 3.1 取出帧与帧们 + KeyFrame *pKF = mit->first; + const long unsigned int nIDi = pKF->mnId; + // 和pKF 有连接关系的关键帧 + const set &spConnections = mit->second; + const g2o::Sim3 Siw = vScw[nIDi]; // 优化前的位姿 + const g2o::Sim3 Swi = Siw.inverse(); + + // 对于当前关键帧nIDi而言，遍历每一个新添加的关键帧nIDj链接关系 + for (set::const_iterator sit = spConnections.begin(), send = spConnections.end(); sit != send; sit++) + { + const long unsigned int nIDj = (*sit)->mnId; + // 这里的约束有点意思，对于每一个连接，只要是存在pCurKF或者pLoopKF 那这个连接不管共视了多少MP都优化 + // 反之没有的话共视度要大于100 构建本质图 + if ((nIDi != pCurKF->mnId || nIDj != pLoopKF->mnId) && pKF->GetWeight(*sit) < minFeat) + continue; + + // 通过上面考验的帧有两种情况： + // 得到两个pose间的Sim3变换，个人认为这里好像有些问题，假设说一个当前帧的共视帧，他在vScw中保存的位姿是更新后的 + // 如果与之相连的关键帧没有更新，那么是不是两个相对位姿的边有问题，先留个记号，可以调试看看 + const g2o::Sim3 Sjw = vScw[nIDj]; + // 得到两个pose间的Sim3变换 + const g2o::Sim3 Sji = Sjw * Swi; // 优化前他们的相对位姿 + + g2o::EdgeSim3 *e = new g2o::EdgeSim3(); + e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); + e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + // Sji内部是经过了Sim调整的观测 + e->setMeasurement(Sji); + + // 信息矩阵是单位阵,说明这类新增加的边对总误差的贡献也都是一样大的 + e->information() = matLambda; + + optimizer.addEdge(e); + count_loop++; + // 保证id小的在前,大的在后 + sInsertedEdges.insert(make_pair(min(nIDi, nIDj), max(nIDi, nIDj))); + } + } + + // Set normal edges + // 4. 添加跟踪时形成的边、闭环匹配成功形成的边 + for (size_t i = 0, iend = vpKFs.size(); i < iend; i++) + { + KeyFrame *pKF = vpKFs[i]; + + const int nIDi = pKF->mnId; + + g2o::Sim3 Swi; // 校正前的sim3 + + LoopClosing::KeyFrameAndPose::const_iterator iti = NonCorrectedSim3.find(pKF); + // 找到的话说明是关键帧的共视帧，没找到表示非共视帧，非共视帧vScw[nIDi]里面装的都是矫正前的 + // 所以不管怎样说 Swi都是校正前的 + if (iti != NonCorrectedSim3.end()) + Swi = (iti->second).inverse(); + else + Swi = vScw[nIDi].inverse(); + + KeyFrame *pParentKF = pKF->GetParent(); + + // Spanning tree edge + // 4.1 只添加扩展树的边（有父关键帧） + if (pParentKF) + { + int nIDj = pParentKF->mnId; + + g2o::Sim3 Sjw; + + LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(pParentKF); + + if (itj != NonCorrectedSim3.end()) + Sjw = itj->second; + else + Sjw = vScw[nIDj]; + + // 又是未校正的结果作为观测值 + g2o::Sim3 Sji = Sjw * Swi; + + g2o::EdgeSim3 *e = new g2o::EdgeSim3(); + e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); + e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + e->setMeasurement(Sji); + e->information() = matLambda; + optimizer.addEdge(e); + } + + // Loop edges + // 4.2 添加在CorrectLoop函数中AddLoopEdge函数添加的闭环连接边（当前帧与闭环匹配帧之间的连接关系） + // 使用经过Sim3调整前关键帧之间的相对关系作为边 + const set sLoopEdges = pKF->GetLoopEdges(); + for (set::const_iterator sit = sLoopEdges.begin(), send = sLoopEdges.end(); sit != send; sit++) + { + KeyFrame *pLKF = *sit; + if (pLKF->mnId < pKF->mnId) + { + g2o::Sim3 Slw; + + LoopClosing::KeyFrameAndPose::const_iterator itl = NonCorrectedSim3.find(pLKF); + + if (itl != NonCorrectedSim3.end()) + Slw = itl->second; + else + Slw = vScw[pLKF->mnId]; + + g2o::Sim3 Sli = Slw * Swi; + g2o::EdgeSim3 *el = new g2o::EdgeSim3(); + el->setVertex(1, dynamic_cast(optimizer.vertex(pLKF->mnId))); + el->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + el->setMeasurement(Sli); + el->information() = matLambda; + optimizer.addEdge(el); + } + } + + // Covisibility graph edges + // 4.3 对有很好共视关系的关键帧也作为边进行优化 + // 使用经过Sim3调整前关键帧之间的相对关系作为边 + const vector vpConnectedKFs = pKF->GetCovisiblesByWeight(minFeat); + for (vector::const_iterator vit = vpConnectedKFs.begin(); vit != vpConnectedKFs.end(); vit++) + { + KeyFrame *pKFn = *vit; + if (pKFn && pKFn != pParentKF && !pKF->hasChild(pKFn) /*&& !sLoopEdges.count(pKFn)*/) + { + if (!pKFn->isBad() && pKFn->mnId < pKF->mnId) + { + if (sInsertedEdges.count(make_pair(min(pKF->mnId, pKFn->mnId), max(pKF->mnId, pKFn->mnId)))) + continue; + + g2o::Sim3 Snw; + + LoopClosing::KeyFrameAndPose::const_iterator itn = NonCorrectedSim3.find(pKFn); + + if (itn != NonCorrectedSim3.end()) + Snw = itn->second; + else + Snw = vScw[pKFn->mnId]; + + g2o::Sim3 Sni = Snw * Swi; + + g2o::EdgeSim3 *en = new g2o::EdgeSim3(); + en->setVertex(1, dynamic_cast(optimizer.vertex(pKFn->mnId))); + en->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + en->setMeasurement(Sni); + en->information() = matLambda; + optimizer.addEdge(en); + } + } + } + + // Inertial edges if inertial + // 如果是imu的话还会找前一帧做优化 + if (pKF->bImu && pKF->mPrevKF) + { + g2o::Sim3 Spw; + LoopClosing::KeyFrameAndPose::const_iterator itp = NonCorrectedSim3.find(pKF->mPrevKF); + if (itp != NonCorrectedSim3.end()) + Spw = itp->second; + else + Spw = vScw[pKF->mPrevKF->mnId]; + + g2o::Sim3 Spi = Spw * Swi; + g2o::EdgeSim3 *ep = new g2o::EdgeSim3(); + ep->setVertex(1, dynamic_cast(optimizer.vertex(pKF->mPrevKF->mnId))); + ep->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + ep->setMeasurement(Spi); + ep->information() = matLambda; + optimizer.addEdge(ep); + } + } + + // Optimize! + // 5. 开始g2o优化 + optimizer.initializeOptimization(); + optimizer.computeActiveErrors(); + optimizer.optimize(20); + optimizer.computeActiveErrors(); + unique_lock lock(pMap->mMutexMapUpdate); + + // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1] + // 6. 设定优化后的位姿 + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKFi = vpKFs[i]; + + const int nIDi = pKFi->mnId; + + g2o::VertexSim3Expmap *VSim3 = static_cast(optimizer.vertex(nIDi)); + g2o::Sim3 CorrectedSiw = VSim3->estimate(); + vCorrectedSwc[nIDi] = CorrectedSiw.inverse(); + double s = CorrectedSiw.scale(); + + Sophus::SE3f Tiw(CorrectedSiw.rotation().cast(), CorrectedSiw.translation().cast() / s); + pKFi->SetPose(Tiw); + } + + // Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose + // 7. 步骤5和步骤6优化得到关键帧的位姿后，MapPoints根据参考帧优化前后的相对关系调整自己的位置 + for (size_t i = 0, iend = vpMPs.size(); i < iend; i++) + { + MapPoint *pMP = vpMPs[i]; + + if (pMP->isBad()) + continue; + + int nIDr; + // 该MapPoint经过Sim3调整过，(LoopClosing.cpp，CorrectLoop函数，步骤2.2_ + if (pMP->mnCorrectedByKF == pCurKF->mnId) + { + nIDr = pMP->mnCorrectedReference; + } + else + { + // 通过情况下MapPoint的参考关键帧就是创建该MapPoint的那个关键帧 + KeyFrame *pRefKF = pMP->GetReferenceKeyFrame(); + nIDr = pRefKF->mnId; + } + + // 得到MapPoint参考关键帧步骤5优化前的位姿 + g2o::Sim3 Srw = vScw[nIDr]; + // 得到MapPoint参考关键帧优化后的位姿 + g2o::Sim3 correctedSwr = vCorrectedSwc[nIDr]; + + // 更新前坐标 + Eigen::Matrix eigP3Dw = pMP->GetWorldPos().cast(); + // 更新后坐标 + Eigen::Matrix eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw)); + pMP->SetWorldPos(eigCorrectedP3Dw.cast()); + + pMP->UpdateNormalAndDepth(); + } + + // TODO Check this changeindex + pMap->IncreaseChangeIndex(); +} + +/** + * @brief LoopClosing::CorrectLoop() 回环矫正时使用，IMU加视觉，全局本质图优化，流程基本与上面 OptimizeEssentialGraph 一模一样，同样有严重BUG + * 优化目标： 地图中所有MP与关键帧 + * @param pMap 当前的map + * @param pLoopKF mpLoopMatchedKF 与 mpCurrentKF 匹配的关键帧 + * @param pCurKF mpCurrentKF 当前关键帧 + * @param NonCorrectedSim3 通过pKFi->GetPose()计算的放NonCorrectedSim3也就是回环前的位姿 这里面的帧只是与mpCurrentKF相关联的 + * @param CorrectedSim3 通过mg2oLoopScw计算的放CorrectedSim3 这里面的帧只是与mpCurrentKF共视的 + * @param LoopConnections 因为回环而建立的新的帧与帧的连接关系，里面的key 全部为pCurKF的共视帧与其本身 + */ +void Optimizer::OptimizeEssentialGraph4DoF( + Map *pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF, + const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, + const LoopClosing::KeyFrameAndPose &CorrectedSim3, + const map> &LoopConnections) +{ + typedef g2o::BlockSolver> BlockSolver_4_4; + + // Setup optimizer + // 1. 构建优化器 + g2o::SparseOptimizer optimizer; + optimizer.setVerbose(false); + g2o::BlockSolverX::LinearSolverType *linearSolver = + new g2o::LinearSolverEigen(); + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); + + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + + optimizer.setAlgorithm(solver); + + const vector vpKFs = pMap->GetAllKeyFrames(); // 所有关键帧 + const vector vpMPs = pMap->GetAllMapPoints(); // 所有mp + + const unsigned int nMaxKFid = pMap->GetMaxKFid(); + + vector> vScw(nMaxKFid + 1); // 存放每一帧优化前的sim3 + vector> vCorrectedSwc(nMaxKFid + 1); // 存放每一帧优化后的sim3，修正mp位姿用 + + vector vpVertices(nMaxKFid + 1); + + const int minFeat = 100; // 100 本质图的权重 + // Set KeyFrame vertices + // 2. 关键帧节点 + for (size_t i = 0, iend = vpKFs.size(); i < iend; i++) + { + KeyFrame *pKF = vpKFs[i]; + if (pKF->isBad()) + continue; + + // 自定义的一个优化4自由度的节点 + VertexPose4DoF *V4DoF; + + const int nIDi = pKF->mnId; + + LoopClosing::KeyFrameAndPose::const_iterator it = CorrectedSim3.find(pKF); + + // 找到了表示与当前关键帧共视，使用其他函数更新后的位姿 + if (it != CorrectedSim3.end()) + { + vScw[nIDi] = it->second; + const g2o::Sim3 Swc = it->second.inverse(); + Eigen::Matrix3d Rwc = Swc.rotation().toRotationMatrix(); + Eigen::Vector3d twc = Swc.translation(); + V4DoF = new VertexPose4DoF(Rwc, twc, pKF); + } + else + { + // 没有在CorrectedSim3里面找到表示与pCurKF并不关联，也就是离得远，并没有经过计算得到一个初始值，这里直接使用原始的位置 + Sophus::SE3d Tcw = pKF->GetPose().cast(); + g2o::Sim3 Siw(Tcw.unit_quaternion(), Tcw.translation(), 1.0); + + vScw[nIDi] = Siw; + V4DoF = new VertexPose4DoF(pKF); + } + + // 固定回环帧 + if (pKF == pLoopKF) + V4DoF->setFixed(true); + + V4DoF->setId(nIDi); + V4DoF->setMarginalized(false); + + optimizer.addVertex(V4DoF); + vpVertices[nIDi] = V4DoF; + } + + set> sInsertedEdges; + + // Edge used in posegraph has still 6Dof, even if updates of camera poses are just in 4DoF + Eigen::Matrix matLambda = Eigen::Matrix::Identity(); + matLambda(0, 0) = 1e3; + matLambda(1, 1) = 1e3; + matLambda(0, 0) = 1e3; + + // Set Loop edges + // 3. 添加边：LoopConnections是闭环时因为MapPoints调整而出现的新关键帧连接关系（包括当前帧与闭环匹配帧之间的连接关系） + Edge4DoF *e_loop; + for (map>::const_iterator mit = LoopConnections.begin(), mend = LoopConnections.end(); mit != mend; mit++) + { + // 3.1 取出帧与帧们 + KeyFrame *pKF = mit->first; + const long unsigned int nIDi = pKF->mnId; + const set &spConnections = mit->second; + const g2o::Sim3 Siw = vScw[nIDi]; // 优化前的位姿 + + for (set::const_iterator sit = spConnections.begin(), send = spConnections.end(); sit != send; sit++) + { + const long unsigned int nIDj = (*sit)->mnId; + // 这里的约束有点意思，对于每一个连接，只要是存在pCurKF或者pLoopKF 那这个连接不管共视了多少MP都优化 + // 反之没有的话共视度要大于100 构建本质图 + if ((nIDi != pCurKF->mnId || nIDj != pLoopKF->mnId) && pKF->GetWeight(*sit) < minFeat) + continue; + + const g2o::Sim3 Sjw = vScw[nIDj]; + // 得到两个pose间的Sim3变换，个人认为这里好像有些问题，假设说一个当前帧的共视帧，他在vScw中保存的位姿是更新后的 + // 如果与之相连的关键帧没有更新，那么是不是两个相对位姿的边有问题，先留个记号，可以调试看看 + const g2o::Sim3 Sij = Siw * Sjw.inverse(); + Eigen::Matrix4d Tij; + Tij.block<3, 3>(0, 0) = Sij.rotation().toRotationMatrix(); + Tij.block<3, 1>(0, 3) = Sij.translation(); + Tij(3, 3) = 1.; + + // 认为相对位姿会比较准，那这个当一个约束 + Edge4DoF *e = new Edge4DoF(Tij); + e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); + e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + + e->information() = matLambda; + e_loop = e; + optimizer.addEdge(e); + + sInsertedEdges.insert(make_pair(min(nIDi, nIDj), max(nIDi, nIDj))); + } + } + + // 1. Set normal edges + // 4. 添加跟踪时形成的边、闭环匹配成功形成的边 + for (size_t i = 0, iend = vpKFs.size(); i < iend; i++) + { + KeyFrame *pKF = vpKFs[i]; + + const int nIDi = pKF->mnId; + + g2o::Sim3 Siw; + + // Use noncorrected poses for posegraph edges + LoopClosing::KeyFrameAndPose::const_iterator iti = NonCorrectedSim3.find(pKF); + + // 找到的话说明是关键帧的共视帧，没找到表示非共视帧，非共视帧vScw[nIDi]里面装的都是矫正前的 + // 所以不管怎样说 Swi都是校正前的 + if (iti != NonCorrectedSim3.end()) + Siw = iti->second; + else + Siw = vScw[nIDi]; + + // 1.1.0 Spanning tree edge + // 4.1 只添加扩展树的边（有父关键帧） 这里并没有父帧，这段没执行到 + KeyFrame *pParentKF = static_cast(NULL); + if (pParentKF) + { + int nIDj = pParentKF->mnId; + + g2o::Sim3 Swj; + + LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(pParentKF); + + if (itj != NonCorrectedSim3.end()) + Swj = (itj->second).inverse(); + else + Swj = vScw[nIDj].inverse(); + + g2o::Sim3 Sij = Siw * Swj; + Eigen::Matrix4d Tij; + Tij.block<3, 3>(0, 0) = Sij.rotation().toRotationMatrix(); + Tij.block<3, 1>(0, 3) = Sij.translation(); + Tij(3, 3) = 1.; + + Edge4DoF *e = new Edge4DoF(Tij); + e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); + e->information() = matLambda; + optimizer.addEdge(e); + } + + // 1.1.1 Inertial edges + // 代替父帧的是利用mPrevKF，流程与上面一样 + KeyFrame *prevKF = pKF->mPrevKF; + if (prevKF) + { + int nIDj = prevKF->mnId; + + g2o::Sim3 Swj; + + LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(prevKF); + + if (itj != NonCorrectedSim3.end()) + Swj = (itj->second).inverse(); + else + Swj = vScw[nIDj].inverse(); + + g2o::Sim3 Sij = Siw * Swj; + Eigen::Matrix4d Tij; + Tij.block<3, 3>(0, 0) = Sij.rotation().toRotationMatrix(); + Tij.block<3, 1>(0, 3) = Sij.translation(); + Tij(3, 3) = 1.; + + Edge4DoF *e = new Edge4DoF(Tij); + e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); + e->information() = matLambda; + optimizer.addEdge(e); + } + + // 1.2 Loop edges + // 4.2 添加在CorrectLoop函数中AddLoopEdge函数添加的闭环连接边（当前帧与闭环匹配帧之间的连接关系） + // 使用经过Sim3调整前关键帧之间的相对关系作为边 + const set sLoopEdges = pKF->GetLoopEdges(); + for (set::const_iterator sit = sLoopEdges.begin(), send = sLoopEdges.end(); sit != send; sit++) + { + KeyFrame *pLKF = *sit; + if (pLKF->mnId < pKF->mnId) + { + g2o::Sim3 Swl; + + LoopClosing::KeyFrameAndPose::const_iterator itl = NonCorrectedSim3.find(pLKF); + + if (itl != NonCorrectedSim3.end()) + Swl = itl->second.inverse(); + else + Swl = vScw[pLKF->mnId].inverse(); + + g2o::Sim3 Sil = Siw * Swl; + Eigen::Matrix4d Til; + Til.block<3, 3>(0, 0) = Sil.rotation().toRotationMatrix(); + Til.block<3, 1>(0, 3) = Sil.translation(); + Til(3, 3) = 1.; + + // 同样，认为相对位姿比较准 + Edge4DoF *e = new Edge4DoF(Til); + e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pLKF->mnId))); + e->information() = matLambda; + optimizer.addEdge(e); + } + } + + // 1.3 Covisibility graph edges + // 4.3 最有很好共视关系的关键帧也作为边进行优化 + // 使用经过Sim3调整前关键帧之间的相对关系作为边 + const vector vpConnectedKFs = pKF->GetCovisiblesByWeight(minFeat); + for (vector::const_iterator vit = vpConnectedKFs.begin(); vit != vpConnectedKFs.end(); vit++) + { + KeyFrame *pKFn = *vit; + // 在之前没有添加过 + if (pKFn && pKFn != pParentKF && pKFn != prevKF && pKFn != pKF->mNextKF && !pKF->hasChild(pKFn) && !sLoopEdges.count(pKFn)) + { + if (!pKFn->isBad() && pKFn->mnId < pKF->mnId) + { + if (sInsertedEdges.count(make_pair(min(pKF->mnId, pKFn->mnId), max(pKF->mnId, pKFn->mnId)))) + continue; + + g2o::Sim3 Swn; + + LoopClosing::KeyFrameAndPose::const_iterator itn = NonCorrectedSim3.find(pKFn); + + if (itn != NonCorrectedSim3.end()) + Swn = itn->second.inverse(); + else + Swn = vScw[pKFn->mnId].inverse(); + + g2o::Sim3 Sin = Siw * Swn; + Eigen::Matrix4d Tin; + Tin.block<3, 3>(0, 0) = Sin.rotation().toRotationMatrix(); + Tin.block<3, 1>(0, 3) = Sin.translation(); + Tin(3, 3) = 1.; + Edge4DoF *e = new Edge4DoF(Tin); + e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKFn->mnId))); + e->information() = matLambda; + optimizer.addEdge(e); + } + } + } + } + + // 5. 开始g2o优化 + optimizer.initializeOptimization(); + optimizer.computeActiveErrors(); + optimizer.optimize(20); + + unique_lock lock(pMap->mMutexMapUpdate); + + // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1] + // 6. 设定优化后的位姿 + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKFi = vpKFs[i]; + + const int nIDi = pKFi->mnId; + + VertexPose4DoF *Vi = static_cast(optimizer.vertex(nIDi)); + Eigen::Matrix3d Ri = Vi->estimate().Rcw[0]; + Eigen::Vector3d ti = Vi->estimate().tcw[0]; + + g2o::Sim3 CorrectedSiw = g2o::Sim3(Ri, ti, 1.); + vCorrectedSwc[nIDi] = CorrectedSiw.inverse(); + + Sophus::SE3d Tiw(CorrectedSiw.rotation(), CorrectedSiw.translation()); + pKFi->SetPose(Tiw.cast()); + } + + // Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose + // 7. 步骤5和步骤6优化得到关键帧的位姿后，MapPoints根据参考帧优化前后的相对关系调整自己的位置 + for (size_t i = 0, iend = vpMPs.size(); i < iend; i++) + { + MapPoint *pMP = vpMPs[i]; + + if (pMP->isBad()) + continue; + + int nIDr; + + KeyFrame *pRefKF = pMP->GetReferenceKeyFrame(); + nIDr = pRefKF->mnId; + + // 得到MapPoint参考关键帧步骤5优化前的位姿 + g2o::Sim3 Srw = vScw[nIDr]; + // 得到MapPoint参考关键帧优化后的位姿 + g2o::Sim3 correctedSwr = vCorrectedSwc[nIDr]; + + // 更新的过程就是先将他通过原始位姿转到相机坐标系下，再通过新的位姿转到更新后的世界坐标 + Eigen::Matrix eigP3Dw = pMP->GetWorldPos().cast(); + Eigen::Matrix eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw)); + pMP->SetWorldPos(eigCorrectedP3Dw.cast()); + + pMP->UpdateNormalAndDepth(); + } + pMap->IncreaseChangeIndex(); +} + + + +/**************************************以下为地图融合优化**************************************************************/ + +/** + * @brief Local Bundle Adjustment LoopClosing::MergeLocal() 融合地图时使用，纯视觉 + * 优化目标： 1. vpAdjustKF; 2.vpAdjustKF与vpFixedKF对应的MP点 + * 优化所有的当前关键帧共视窗口里的关键帧和地图点, 固定所有融合帧共视窗口里的帧 + * @param pMainKF mpCurrentKF 当前关键帧 + * @param vpAdjustKF vpLocalCurrentWindowKFs 待优化的KF + * @param vpFixedKF vpMergeConnectedKFs 固定的KF + * @param pbStopFlag false + */ +void Optimizer::LocalBundleAdjustment( + KeyFrame *pMainKF, vector vpAdjustKF, vector vpFixedKF, bool *pbStopFlag) +{ + bool bShowImages = false; + + vector vpMPs; + + // 1. 构建g2o优化器 + g2o::SparseOptimizer optimizer; + g2o::BlockSolver_6_3::LinearSolverType *linearSolver; + + linearSolver = new g2o::LinearSolverEigen(); + + g2o::BlockSolver_6_3 *solver_ptr = new g2o::BlockSolver_6_3(linearSolver); + + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + optimizer.setAlgorithm(solver); + + optimizer.setVerbose(false); + + if (pbStopFlag) + optimizer.setForceStopFlag(pbStopFlag); + + long unsigned int maxKFid = 0; + set spKeyFrameBA; // 存放关键帧，包含固定的与不固定的 + + Map *pCurrentMap = pMainKF->GetMap(); + + // Set fixed KeyFrame vertices + // 2. 构建固定关键帧的节点，并储存对应的MP + int numInsertedPoints = 0; + for (KeyFrame *pKFi : vpFixedKF) + { + if (pKFi->isBad() || pKFi->GetMap() != pCurrentMap) + { + Verbose::PrintMess("ERROR LBA: KF is bad or is not in the current map", Verbose::VERBOSITY_NORMAL); + continue; + } + + pKFi->mnBALocalForMerge = pMainKF->mnId; // 防止重复添加 + + g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap(); + Sophus::SE3 Tcw = pKFi->GetPose(); + vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(), Tcw.translation().cast())); + vSE3->setId(pKFi->mnId); + vSE3->setFixed(true); + optimizer.addVertex(vSE3); + if (pKFi->mnId > maxKFid) + maxKFid = pKFi->mnId; + + set spViewMPs = pKFi->GetMapPoints(); + for (MapPoint *pMPi : spViewMPs) + { + if (pMPi) + if (!pMPi->isBad() && pMPi->GetMap() == pCurrentMap) + + if (pMPi->mnBALocalForMerge != pMainKF->mnId) + { + vpMPs.push_back(pMPi); + pMPi->mnBALocalForMerge = pMainKF->mnId; + numInsertedPoints++; + } + } + + spKeyFrameBA.insert(pKFi); + } + + // Set non fixed Keyframe vertices + // 3. 构建不固定关键帧的节点，并储存对应的MP + set spAdjustKF(vpAdjustKF.begin(), vpAdjustKF.end()); + numInsertedPoints = 0; + for (KeyFrame *pKFi : vpAdjustKF) + { + if (pKFi->isBad() || pKFi->GetMap() != pCurrentMap) + continue; + + pKFi->mnBALocalForMerge = pMainKF->mnId; // 防止重复添加 + + g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap(); + Sophus::SE3 Tcw = pKFi->GetPose(); + vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(), Tcw.translation().cast())); + vSE3->setId(pKFi->mnId); + optimizer.addVertex(vSE3); + if (pKFi->mnId > maxKFid) + maxKFid = pKFi->mnId; + + set spViewMPs = pKFi->GetMapPoints(); + for (MapPoint *pMPi : spViewMPs) + { + if (pMPi) + { + if (!pMPi->isBad() && pMPi->GetMap() == pCurrentMap) + { + if (pMPi->mnBALocalForMerge != pMainKF->mnId) + { + vpMPs.push_back(pMPi); + pMPi->mnBALocalForMerge = pMainKF->mnId; + numInsertedPoints++; + } + } + } + } + + spKeyFrameBA.insert(pKFi); + } + + // 准备存放边的vector + const int nExpectedSize = (vpAdjustKF.size() + vpFixedKF.size()) * vpMPs.size(); + + vector vpEdgesMono; + vpEdgesMono.reserve(nExpectedSize); + + vector vpEdgeKFMono; + vpEdgeKFMono.reserve(nExpectedSize); + + vector vpMapPointEdgeMono; + vpMapPointEdgeMono.reserve(nExpectedSize); + + vector vpEdgesStereo; + vpEdgesStereo.reserve(nExpectedSize); + + vector vpEdgeKFStereo; + vpEdgeKFStereo.reserve(nExpectedSize); + + vector vpMapPointEdgeStereo; + vpMapPointEdgeStereo.reserve(nExpectedSize); + + const float thHuber2D = sqrt(5.99); + const float thHuber3D = sqrt(7.815); + + // Set MapPoint vertices + map mpObsKFs; // 统计每个关键帧对应的MP点数，调试输出用 + map mpObsFinalKFs; // 统计每个MP对应的关键帧数，调试输出用 + map mpObsMPs; // 统计每个MP被观测的图片数，双目就是两个，调试输出用 + + // 4. 确定MP节点与边的连接 + for (unsigned int i = 0; i < vpMPs.size(); ++i) + { + MapPoint *pMPi = vpMPs[i]; + if (pMPi->isBad()) + continue; + + g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ(); + vPoint->setEstimate(pMPi->GetWorldPos().cast()); + const int id = pMPi->mnId + maxKFid + 1; + vPoint->setId(id); + vPoint->setMarginalized(true); + optimizer.addVertex(vPoint); + + const map> observations = pMPi->GetObservations(); + int nEdges = 0; + // SET EDGES + for (map>::const_iterator mit = observations.begin(); mit != observations.end(); mit++) + { + KeyFrame *pKF = mit->first; + // 跳过的条件 + // 1. 帧坏了 + // 2. 帧靠后 + // 3. 不在参与优化帧的里面 + // 4. 在左相机上不存在这个三维点 + if (pKF->isBad() || pKF->mnId > maxKFid || pKF->mnBALocalForMerge != pMainKF->mnId || !pKF->GetMapPoint(get<0>(mit->second))) + continue; + + nEdges++; + + const cv::KeyPoint &kpUn = pKF->mvKeysUn[get<0>(mit->second)]; + + // 注意！！！这里没有考虑相机2 + if (pKF->mvuRight[get<0>(mit->second)] < 0) // Monocular + { + mpObsMPs[pMPi]++; + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y; + + ORB_SLAM3::EdgeSE3ProjectXYZ *e = new ORB_SLAM3::EdgeSE3ProjectXYZ(); + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKF->mnId))); + e->setMeasurement(obs); + const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave]; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuber2D); + + e->pCamera = pKF->mpCamera; + + optimizer.addEdge(e); + + vpEdgesMono.push_back(e); + vpEdgeKFMono.push_back(pKF); + vpMapPointEdgeMono.push_back(pMPi); + + mpObsKFs[pKF]++; + } + else // RGBD or Stereo + { + mpObsMPs[pMPi] += 2; + Eigen::Matrix obs; + const float kp_ur = pKF->mvuRight[get<0>(mit->second)]; + obs << kpUn.pt.x, kpUn.pt.y, kp_ur; + + g2o::EdgeStereoSE3ProjectXYZ *e = new g2o::EdgeStereoSE3ProjectXYZ(); + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKF->mnId))); + e->setMeasurement(obs); + const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave]; + Eigen::Matrix3d Info = Eigen::Matrix3d::Identity() * invSigma2; + e->setInformation(Info); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuber3D); + + e->fx = pKF->fx; + e->fy = pKF->fy; + e->cx = pKF->cx; + e->cy = pKF->cy; + e->bf = pKF->mbf; + + optimizer.addEdge(e); + + vpEdgesStereo.push_back(e); + vpEdgeKFStereo.push_back(pKF); + vpMapPointEdgeStereo.push_back(pMPi); + + mpObsKFs[pKF]++; + } + } + } + + if (pbStopFlag) + if (*pbStopFlag) + return; + // 5. 优化 + optimizer.initializeOptimization(); + optimizer.optimize(5); + + bool bDoMore = true; + + if (pbStopFlag) + if (*pbStopFlag) + bDoMore = false; + // 6. 剔除误差大的边 + map mWrongObsKF; + if (bDoMore) + { + // Check inlier observations + int badMonoMP = 0, badStereoMP = 0; + for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) + { + ORB_SLAM3::EdgeSE3ProjectXYZ *e = vpEdgesMono[i]; + MapPoint *pMP = vpMapPointEdgeMono[i]; + + if (pMP->isBad()) + continue; + + if (e->chi2() > 5.991 || !e->isDepthPositive()) + { + e->setLevel(1); + badMonoMP++; + } + e->setRobustKernel(0); + } + + for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++) + { + g2o::EdgeStereoSE3ProjectXYZ *e = vpEdgesStereo[i]; + MapPoint *pMP = vpMapPointEdgeStereo[i]; + + if (pMP->isBad()) + continue; + + if (e->chi2() > 7.815 || !e->isDepthPositive()) + { + e->setLevel(1); + badStereoMP++; + } + + e->setRobustKernel(0); + } + Verbose::PrintMess("[BA]: First optimization(Huber), there are " + to_string(badMonoMP) + " monocular and " + to_string(badStereoMP) + " stereo bad edges", Verbose::VERBOSITY_DEBUG); + + optimizer.initializeOptimization(0); + optimizer.optimize(10); + } + + // 7. 删除误差大的点与帧的连接关系 + vector> vToErase; + vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size()); + set spErasedMPs; + set spErasedKFs; + + // Check inlier observations + int badMonoMP = 0, badStereoMP = 0; + for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) + { + ORB_SLAM3::EdgeSE3ProjectXYZ *e = vpEdgesMono[i]; + MapPoint *pMP = vpMapPointEdgeMono[i]; + + if (pMP->isBad()) + continue; + + if (e->chi2() > 5.991 || !e->isDepthPositive()) + { + KeyFrame *pKFi = vpEdgeKFMono[i]; + vToErase.push_back(make_pair(pKFi, pMP)); + mWrongObsKF[pKFi->mnId]++; + badMonoMP++; + + spErasedMPs.insert(pMP); + spErasedKFs.insert(pKFi); + } + } + + for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++) + { + g2o::EdgeStereoSE3ProjectXYZ *e = vpEdgesStereo[i]; + MapPoint *pMP = vpMapPointEdgeStereo[i]; + + if (pMP->isBad()) + continue; + + if (e->chi2() > 7.815 || !e->isDepthPositive()) + { + KeyFrame *pKFi = vpEdgeKFStereo[i]; + vToErase.push_back(make_pair(pKFi, pMP)); + mWrongObsKF[pKFi->mnId]++; + badStereoMP++; + + spErasedMPs.insert(pMP); + spErasedKFs.insert(pKFi); + } + } + + Verbose::PrintMess("[BA]: Second optimization, there are " + to_string(badMonoMP) + " monocular and " + to_string(badStereoMP) + " sterero bad edges", Verbose::VERBOSITY_DEBUG); + + // Get Map Mutex + unique_lock lock(pMainKF->GetMap()->mMutexMapUpdate); + + if (!vToErase.empty()) + { + // 剔除链接关系，其他均为调试 + for (size_t i = 0; i < vToErase.size(); i++) + { + KeyFrame *pKFi = vToErase[i].first; + MapPoint *pMPi = vToErase[i].second; + pKFi->EraseMapPointMatch(pMPi); + pMPi->EraseObservation(pKFi); + } + } + // 没用，调试的 + for (unsigned int i = 0; i < vpMPs.size(); ++i) + { + MapPoint *pMPi = vpMPs[i]; + if (pMPi->isBad()) + continue; + + const map> observations = pMPi->GetObservations(); + for (map>::const_iterator mit = observations.begin(); mit != observations.end(); mit++) + { + KeyFrame *pKF = mit->first; + if (pKF->isBad() || pKF->mnId > maxKFid || pKF->mnBALocalForKF != pMainKF->mnId || !pKF->GetMapPoint(get<0>(mit->second))) + continue; + + if (pKF->mvuRight[get<0>(mit->second)] < 0) // Monocular + { + mpObsFinalKFs[pKF]++; + } + else // RGBD or Stereo + { + mpObsFinalKFs[pKF]++; + } + } + } + + // Recover optimized data + // 8. 取出结果 + // Keyframes + for (KeyFrame *pKFi : vpAdjustKF) + { + if (pKFi->isBad()) + continue; + // 8.1 取出对应位姿 + g2o::VertexSE3Expmap *vSE3 = static_cast(optimizer.vertex(pKFi->mnId)); + g2o::SE3Quat SE3quat = vSE3->estimate(); + Sophus::SE3f Tiw(SE3quat.rotation().cast(), SE3quat.translation().cast()); + + // 统计调试用 + int numMonoBadPoints = 0, numMonoOptPoints = 0; + int numStereoBadPoints = 0, numStereoOptPoints = 0; + vector vpMonoMPsOpt, vpStereoMPsOpt; // 存放mp内点 + vector vpMonoMPsBad, vpStereoMPsBad; // 存放mp外点 + // 8.2 卡方检验，调试用的貌似没别的作用 + for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) + { + ORB_SLAM3::EdgeSE3ProjectXYZ *e = vpEdgesMono[i]; + MapPoint *pMP = vpMapPointEdgeMono[i]; + KeyFrame *pKFedge = vpEdgeKFMono[i]; + + if (pKFi != pKFedge) + { + continue; + } + + if (pMP->isBad()) + continue; + + if (e->chi2() > 5.991 || !e->isDepthPositive()) + { + numMonoBadPoints++; + vpMonoMPsBad.push_back(pMP); + } + else + { + numMonoOptPoints++; + vpMonoMPsOpt.push_back(pMP); + } + } + + for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++) + { + g2o::EdgeStereoSE3ProjectXYZ *e = vpEdgesStereo[i]; + MapPoint *pMP = vpMapPointEdgeStereo[i]; + KeyFrame *pKFedge = vpEdgeKFMono[i]; + + if (pKFi != pKFedge) + { + continue; + } + + if (pMP->isBad()) + continue; + + if (e->chi2() > 7.815 || !e->isDepthPositive()) + { + numStereoBadPoints++; + vpStereoMPsBad.push_back(pMP); + } + else + { + numStereoOptPoints++; + vpStereoMPsOpt.push_back(pMP); + } + } + + pKFi->SetPose(Tiw); + } + + // 更新点的坐标 + for (MapPoint *pMPi : vpMPs) + { + if (pMPi->isBad()) + continue; + + g2o::VertexSBAPointXYZ *vPoint = static_cast(optimizer.vertex(pMPi->mnId + maxKFid + 1)); + pMPi->SetWorldPos(vPoint->estimate().cast()); + pMPi->UpdateNormalAndDepth(); + } +} + +/** + * @brief LoopClosing::MergeLocal() 融合地图时使用，优化当前帧没有参与融合的元素，本质图优化 + * 优化目标： 1. vpNonFixedKFs; 2.vpNonCorrectedMPs + * @param pCurKF mpCurrentKF 融合时当前关键帧 + * @param vpFixedKFs vpMergeConnectedKFs 融合地图中的关键帧，也就是上面函数中的 vpFixedKF + * @param vpFixedCorrectedKFs vpLocalCurrentWindowKFs 当前地图中经过矫正的关键帧，也就是Optimizer::LocalBundleAdjustment中优化过的vpAdjustKF + * @param vpNonFixedKFs vpCurrentMapKFs 当前地图中剩余的关键帧，待优化 + * @param vpNonCorrectedMPs vpCurrentMapMPs 当前地图中剩余的MP点，待优化 + */ +void Optimizer::OptimizeEssentialGraph( + KeyFrame *pCurKF, vector &vpFixedKFs, vector &vpFixedCorrectedKFs, + vector &vpNonFixedKFs, vector &vpNonCorrectedMPs) +{ + // 1. 优化器构建 + g2o::SparseOptimizer optimizer; + optimizer.setVerbose(false); + g2o::BlockSolver_7_3::LinearSolverType *linearSolver = + new g2o::LinearSolverEigen(); + g2o::BlockSolver_7_3 *solver_ptr = new g2o::BlockSolver_7_3(linearSolver); + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + + solver->setUserLambdaInit(1e-16); + optimizer.setAlgorithm(solver); + + Map *pMap = pCurKF->GetMap(); + const unsigned int nMaxKFid = pMap->GetMaxKFid(); + + vector> vScw(nMaxKFid + 1); // 存放每一帧优化前的sim3 + vector> vCorrectedSwc(nMaxKFid + 1); // 存放每一帧优化后的sim3，调试输出用，没有实际作用 + vector vpVertices(nMaxKFid + 1); // 存放节点，没用，还占地方 + + vector vpGoodPose(nMaxKFid + 1); + vector vpBadPose(nMaxKFid + 1); + + const int minFeat = 100; // pKFi->GetCovisiblesByWeight(minFeat); essentialgraph 阈值就是共视大于100 + + // 2. 确定固定关键帧的节点 + // vpMergeConnectedKFs 融合地图中的关键帧 + for (KeyFrame *pKFi : vpFixedKFs) + { + if (pKFi->isBad()) + continue; + + g2o::VertexSim3Expmap *VSim3 = new g2o::VertexSim3Expmap(); + + const int nIDi = pKFi->mnId; + + Sophus::SE3d Tcw = pKFi->GetPose().cast(); + g2o::Sim3 Siw(Tcw.unit_quaternion(), Tcw.translation(), 1.0); + + vCorrectedSwc[nIDi] = Siw.inverse(); + VSim3->setEstimate(Siw); + + VSim3->setFixed(true); + + VSim3->setId(nIDi); + VSim3->setMarginalized(false); + VSim3->_fix_scale = true; + + optimizer.addVertex(VSim3); + + vpVertices[nIDi] = VSim3; + + vpGoodPose[nIDi] = true; + vpBadPose[nIDi] = false; + } + Verbose::PrintMess("Opt_Essential: vpFixedKFs loaded", Verbose::VERBOSITY_DEBUG); + + set sIdKF; + // vpLocalCurrentWindowKFs 当前地图中经过矫正的关键帧 + for (KeyFrame *pKFi : vpFixedCorrectedKFs) + { + if (pKFi->isBad()) + continue; + + g2o::VertexSim3Expmap *VSim3 = new g2o::VertexSim3Expmap(); + + const int nIDi = pKFi->mnId; + + Sophus::SE3d Tcw = pKFi->GetPose().cast(); + g2o::Sim3 Siw(Tcw.unit_quaternion(), Tcw.translation(), 1.0); + + vCorrectedSwc[nIDi] = Siw.inverse(); + VSim3->setEstimate(Siw); + + Sophus::SE3d Tcw_bef = pKFi->mTcwBefMerge.cast(); + vScw[nIDi] = g2o::Sim3(Tcw_bef.unit_quaternion(), Tcw_bef.translation(), 1.0); + + VSim3->setFixed(true); + + VSim3->setId(nIDi); + VSim3->setMarginalized(false); + + optimizer.addVertex(VSim3); + + vpVertices[nIDi] = VSim3; + + sIdKF.insert(nIDi); + + vpGoodPose[nIDi] = true; + vpBadPose[nIDi] = true; + } + + // 3. 确定待优化的关键帧节点 + for (KeyFrame *pKFi : vpNonFixedKFs) + { + if (pKFi->isBad()) + continue; + + const int nIDi = pKFi->mnId; + + if (sIdKF.count(nIDi)) // It has already added in the corrected merge KFs + continue; + + g2o::VertexSim3Expmap *VSim3 = new g2o::VertexSim3Expmap(); + + Sophus::SE3d Tcw = pKFi->GetPose().cast(); + g2o::Sim3 Siw(Tcw.unit_quaternion(), Tcw.translation(), 1.0); + + vScw[nIDi] = Siw; + VSim3->setEstimate(Siw); + + VSim3->setFixed(false); + + VSim3->setId(nIDi); + VSim3->setMarginalized(false); + + optimizer.addVertex(VSim3); + + vpVertices[nIDi] = VSim3; + + sIdKF.insert(nIDi); + + vpGoodPose[nIDi] = false; + vpBadPose[nIDi] = true; + } + + vector vpKFs; + vpKFs.reserve(vpFixedKFs.size() + vpFixedCorrectedKFs.size() + vpNonFixedKFs.size()); + vpKFs.insert(vpKFs.end(), vpFixedKFs.begin(), vpFixedKFs.end()); + vpKFs.insert(vpKFs.end(), vpFixedCorrectedKFs.begin(), vpFixedCorrectedKFs.end()); + vpKFs.insert(vpKFs.end(), vpNonFixedKFs.begin(), vpNonFixedKFs.end()); + set spKFs(vpKFs.begin(), vpKFs.end()); + + const Eigen::Matrix matLambda = Eigen::Matrix::Identity(); + + // 4. 遍历所有帧 + for (KeyFrame *pKFi : vpKFs) + { + int num_connections = 0; // 统计与pKFi连接的数量 + const int nIDi = pKFi->mnId; + + g2o::Sim3 correctedSwi; + g2o::Sim3 Swi; + + if (vpGoodPose[nIDi]) + correctedSwi = vCorrectedSwc[nIDi]; + if (vpBadPose[nIDi]) + Swi = vScw[nIDi].inverse(); + + KeyFrame *pParentKFi = pKFi->GetParent(); + + // Spanning tree edge + // 4.1 找到pKFi的父帧且在这批关键帧里面，添加与其关联的sim3边，相对约束 + if (pParentKFi && spKFs.find(pParentKFi) != spKFs.end()) + { + int nIDj = pParentKFi->mnId; + + g2o::Sim3 Sjw; + bool bHasRelation = false; + + if (vpGoodPose[nIDi] && vpGoodPose[nIDj]) + { + Sjw = vCorrectedSwc[nIDj].inverse(); + bHasRelation = true; + } + else if (vpBadPose[nIDi] && vpBadPose[nIDj]) + { + Sjw = vScw[nIDj]; + bHasRelation = true; + } + + if (bHasRelation) + { + g2o::Sim3 Sji = Sjw * Swi; + + g2o::EdgeSim3 *e = new g2o::EdgeSim3(); + e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); + e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + e->setMeasurement(Sji); + + e->information() = matLambda; + optimizer.addEdge(e); + num_connections++; + } + } + + // Loop edges + // 4.2 添加回环的边 + const set sLoopEdges = pKFi->GetLoopEdges(); + for (set::const_iterator sit = sLoopEdges.begin(), send = sLoopEdges.end(); sit != send; sit++) + { + KeyFrame *pLKF = *sit; + if (spKFs.find(pLKF) != spKFs.end() && pLKF->mnId < pKFi->mnId) + { + g2o::Sim3 Slw; + bool bHasRelation = false; + + if (vpGoodPose[nIDi] && vpGoodPose[pLKF->mnId]) + { + Slw = vCorrectedSwc[pLKF->mnId].inverse(); + bHasRelation = true; + } + else if (vpBadPose[nIDi] && vpBadPose[pLKF->mnId]) + { + Slw = vScw[pLKF->mnId]; + bHasRelation = true; + } + + if (bHasRelation) + { + g2o::Sim3 Sli = Slw * Swi; + g2o::EdgeSim3 *el = new g2o::EdgeSim3(); + el->setVertex(1, dynamic_cast(optimizer.vertex(pLKF->mnId))); + el->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + el->setMeasurement(Sli); + el->information() = matLambda; + optimizer.addEdge(el); + num_connections++; + } + } + } + + // Covisibility graph edges + // 4.3 建立essentialgraph（共视边） + const vector vpConnectedKFs = pKFi->GetCovisiblesByWeight(minFeat); + for (vector::const_iterator vit = vpConnectedKFs.begin(); vit != vpConnectedKFs.end(); vit++) + { + KeyFrame *pKFn = *vit; + // 1.这个帧存在且不是pKFi的父帧，防止重复添加 + // 2.pKFn不为pKFi的子帧 + // 3.pKFn不为回环帧 + // 4.pKFn要在这批关键帧里面 + // 防止重复添加 + if (pKFn && pKFn != pParentKFi && !pKFi->hasChild(pKFn) && !sLoopEdges.count(pKFn) && spKFs.find(pKFn) != spKFs.end()) + { + if (!pKFn->isBad() && pKFn->mnId < pKFi->mnId) + { + + g2o::Sim3 Snw = vScw[pKFn->mnId]; + bool bHasRelation = false; + + if (vpGoodPose[nIDi] && vpGoodPose[pKFn->mnId]) + { + Snw = vCorrectedSwc[pKFn->mnId].inverse(); + bHasRelation = true; + } + else if (vpBadPose[nIDi] && vpBadPose[pKFn->mnId]) + { + Snw = vScw[pKFn->mnId]; + bHasRelation = true; + } + + if (bHasRelation) + { + g2o::Sim3 Sni = Snw * Swi; + + g2o::EdgeSim3 *en = new g2o::EdgeSim3(); + en->setVertex(1, dynamic_cast(optimizer.vertex(pKFn->mnId))); + en->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + en->setMeasurement(Sni); + en->information() = matLambda; + optimizer.addEdge(en); + num_connections++; + } + } + } + } + + if (num_connections == 0) + { + Verbose::PrintMess("Opt_Essential: KF " + to_string(pKFi->mnId) + " has 0 connections", Verbose::VERBOSITY_DEBUG); + } + } + + // Optimize! + // 5. 开始优化 + optimizer.initializeOptimization(); + optimizer.optimize(20); + + unique_lock lock(pMap->mMutexMapUpdate); + + // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1] + // 6. 取出结果 + for (KeyFrame *pKFi : vpNonFixedKFs) + { + if (pKFi->isBad()) + continue; + + const int nIDi = pKFi->mnId; + + g2o::VertexSim3Expmap *VSim3 = static_cast(optimizer.vertex(nIDi)); + g2o::Sim3 CorrectedSiw = VSim3->estimate(); + vCorrectedSwc[nIDi] = CorrectedSiw.inverse(); + double s = CorrectedSiw.scale(); + Sophus::SE3d Tiw(CorrectedSiw.rotation(), CorrectedSiw.translation() / s); + + pKFi->mTcwBefMerge = pKFi->GetPose(); + pKFi->mTwcBefMerge = pKFi->GetPoseInverse(); + pKFi->SetPose(Tiw.cast()); + } + + // Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose + for (MapPoint *pMPi : vpNonCorrectedMPs) + { + if (pMPi->isBad()) + continue; + + KeyFrame *pRefKF = pMPi->GetReferenceKeyFrame(); + while (pRefKF->isBad()) + { + if (!pRefKF) + { + Verbose::PrintMess("MP " + to_string(pMPi->mnId) + " without a valid reference KF", Verbose::VERBOSITY_DEBUG); + break; + } + + pMPi->EraseObservation(pRefKF); + pRefKF = pMPi->GetReferenceKeyFrame(); + } + + if (vpBadPose[pRefKF->mnId]) + { + // 流程就是转到相机坐标系，在用新的rt转到世界 + Sophus::SE3f TNonCorrectedwr = pRefKF->mTwcBefMerge; + Sophus::SE3f Twr = pRefKF->GetPoseInverse(); + + Eigen::Vector3f eigCorrectedP3Dw = Twr * TNonCorrectedwr.inverse() * pMPi->GetWorldPos(); + pMPi->SetWorldPos(eigCorrectedP3Dw); + + pMPi->UpdateNormalAndDepth(); + } + else + { + cout << "ERROR: MapPoint has a reference KF from another map" << endl; + } + } +} + +/** + * @brief 这里面进行visual inertial ba + * LoopClosing::MergeLocal2 中用到 + * 优化目标：相关帧的位姿，速度，偏置，还有涉及点的坐标，可以理解为跨地图的局部窗口优化 + * @param[in] pCurrKF 当前关键帧 + * @param[in] pMergeKF 融合帧 + * @param[in] pbStopFlag 是否优化 + * @param[in] pMap 当前地图 + * @param[out] corrPoses 所有的Sim3 矫正 + */ +void Optimizer::MergeInertialBA( + KeyFrame *pCurrKF, KeyFrame *pMergeKF, bool *pbStopFlag, Map *pMap, LoopClosing::KeyFrameAndPose &corrPoses) +{ + const int Nd = 6; + const unsigned long maxKFid = pCurrKF->mnId; + // 1. 准备所有被优化的关键帧, 完全固定的帧 + // 被优化的帧, 当前帧和融合匹配帧加起来一共12个 + vector vpOptimizableKFs; + vpOptimizableKFs.reserve(2 * Nd); + + // For cov KFS, inertial parameters are not optimized + // 共视帧, 只优化位姿，不优化速度与偏置 + const int maxCovKF = 30; + vector vpOptimizableCovKFs; + vpOptimizableCovKFs.reserve(maxCovKF); + + // Add sliding window for current KF + // 当前关键帧先加到容器中 + vpOptimizableKFs.push_back(pCurrKF); + // 后面用这个变量避免重复 + pCurrKF->mnBALocalForKF = pCurrKF->mnId; + // 添加5个与当前关键帧相连的时序上相连的关键帧进容器 + // 1.1 一直放一直放，一直放到没有上一个关键帧，这里面包含了pCurrKF最近的6帧，从晚到早排列，不过有可能存不满 + for (int i = 1; i < Nd; i++) + { + if (vpOptimizableKFs.back()->mPrevKF) + { + // 用mPrevKF访问时序上的上一个关键帧 + vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mPrevKF); + vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId; + } + else + break; + } + + // 1.2 如果 vpOptimizableKFs 中最早的一帧前面还有，往不更新惯导参数的序列中添加 + // 否则把最后一个取出来放到不更新惯导参数的序列中 + if (vpOptimizableKFs.back()->mPrevKF) + { + vpOptimizableCovKFs.push_back(vpOptimizableKFs.back()->mPrevKF); + vpOptimizableKFs.back()->mPrevKF->mnBALocalForKF = pCurrKF->mnId; + } + else + { + // 如果没找到时序相连的帧,把被优化的窗口缩减一个给到固定的变量 + vpOptimizableCovKFs.push_back(vpOptimizableKFs.back()); + vpOptimizableKFs.pop_back(); + } + + // Add temporal neighbours to merge KF (previous and next KFs) + // 2. 同样, 对于融合帧也把它和时序上的几个邻居加到可优化的容器里 + // 2.1 把匹配的融合关键帧也放进来准备一起优化 + vpOptimizableKFs.push_back(pMergeKF); + pMergeKF->mnBALocalForKF = pCurrKF->mnId; + + // Previous KFs + // 把融合帧时序上的邻居添加到可优化的容器里, 因为融合帧左右都有时序上的邻居,所以这里先取一半 Nd/2 + // 2.2 再放进来3个pMergeKF之前的关键帧，有可能放不满 + for (int i = 1; i < (Nd / 2); i++) + { + if (vpOptimizableKFs.back()->mPrevKF) + { + vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mPrevKF); + vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId; + } + else + break; + } + + // We fix just once the old map + // 记录与融合帧窗口时序上紧邻的帧为完全固定的帧 + // 2.3 类似于上面的做法如果有前一个关键帧放入lFixedKeyFrames，否则从vpOptimizableKFs取出，注意这里防止重复添加的标识又多了一个变量 + // 记录完全固定的帧 + list lFixedKeyFrames; + if (vpOptimizableKFs.back()->mPrevKF) + { + lFixedKeyFrames.push_back(vpOptimizableKFs.back()->mPrevKF); + vpOptimizableKFs.back()->mPrevKF->mnBAFixedForKF = pCurrKF->mnId; + } + // 如果没找到,则从窗口内拿出一帧给到完全固定的帧 + else + { + vpOptimizableKFs.back()->mnBALocalForKF = 0; + vpOptimizableKFs.back()->mnBAFixedForKF = pCurrKF->mnId; + lFixedKeyFrames.push_back(vpOptimizableKFs.back()); + vpOptimizableKFs.pop_back(); + } + + // Next KFs + // 2.4 再添加一个pMergeKF的下一个关键帧 + if (pMergeKF->mNextKF) + { + vpOptimizableKFs.push_back(pMergeKF->mNextKF); + vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId; + } + // 2.5 数量不够时，添加最后一个的下一帧。继续添加直到达到2Nd个可优化关键帧,或没有新的可以添加了 + while (vpOptimizableKFs.size() < (2 * Nd)) + { + if (vpOptimizableKFs.back()->mNextKF) + { + vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mNextKF); + vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId; + } + else + break; + } + + int N = vpOptimizableKFs.size(); + + // Optimizable points seen by optimizable keyframes + // 3. 帧弄完了该弄点了，将优化的帧的点存入lLocalMapPoints (所有被可优化关键帧看到的点) + + // 添加用来优化的地图点 + list lLocalMapPoints; + + // 统计了在这些帧中点被观测的次数 + map mLocalObs; + // 遍历所有可优化的关键帧 + for (int i = 0; i < N; i++) + { + vector vpMPs = vpOptimizableKFs[i]->GetMapPointMatches(); + // 遍历每个关键帧所有的地图点 + for (vector::iterator vit = vpMPs.begin(), vend = vpMPs.end(); vit != vend; vit++) + { + // Using mnBALocalForKF we avoid redundance here, one MP can not be added several times to lLocalMapPoints + MapPoint *pMP = *vit; + if (pMP) + if (!pMP->isBad()) + // 用这个变量记录是否重复添加 + if (pMP->mnBALocalForKF != pCurrKF->mnId) + { + mLocalObs[pMP] = 1; + lLocalMapPoints.push_back(pMP); + pMP->mnBALocalForKF = pCurrKF->mnId; // 防止重复添加 + } + else + { + mLocalObs[pMP]++; + } + } + } + + std::vector> pairs; + pairs.reserve(mLocalObs.size()); + for (auto itr = mLocalObs.begin(); itr != mLocalObs.end(); ++itr) + pairs.push_back(*itr); + sort(pairs.begin(), pairs.end(), sortByVal); + + // 4. 把pCurrKF的共视帧加进来 只优化位姿，不优化速度与偏置 + // 固定所有观察到地图点,但没有被加到优化变量中的关键帧 + // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes + int i = 0; + // 遍历所有的pair<地图点指针,观测次数> + for (vector>::iterator lit = pairs.begin(), lend = pairs.end(); lit != lend; lit++, i++) + { + map> observations = lit->first->GetObservations(); + if (i >= maxCovKF) + break; + for (map>::iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++) + { + KeyFrame *pKFi = mit->first; + + if (pKFi->mnBALocalForKF != pCurrKF->mnId && pKFi->mnBAFixedForKF != pCurrKF->mnId) // If optimizable or already included... + { + pKFi->mnBALocalForKF = pCurrKF->mnId; + if (!pKFi->isBad()) + { + vpOptimizableCovKFs.push_back(pKFi); + break; + } + } + } + } + + // 5. 构建优化器 + g2o::SparseOptimizer optimizer; + g2o::BlockSolverX::LinearSolverType *linearSolver; + linearSolver = new g2o::LinearSolverEigen(); + + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); + + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + + solver->setUserLambdaInit(1e3); + + optimizer.setAlgorithm(solver); + optimizer.setVerbose(false); + + // Set Local KeyFrame vertices + N = vpOptimizableKFs.size(); + // 5.1 设置所有的可优化关键帧顶点 + for (int i = 0; i < N; i++) + { + KeyFrame *pKFi = vpOptimizableKFs[i]; + + VertexPose *VP = new VertexPose(pKFi); + VP->setId(pKFi->mnId); + VP->setFixed(false); + // 位姿 + optimizer.addVertex(VP); + + if (pKFi->bImu) + { + VertexVelocity *VV = new VertexVelocity(pKFi); + VV->setId(maxKFid + 3 * (pKFi->mnId) + 1); + VV->setFixed(false); + optimizer.addVertex(VV); + VertexGyroBias *VG = new VertexGyroBias(pKFi); + VG->setId(maxKFid + 3 * (pKFi->mnId) + 2); + VG->setFixed(false); + optimizer.addVertex(VG); + VertexAccBias *VA = new VertexAccBias(pKFi); + VA->setId(maxKFid + 3 * (pKFi->mnId) + 3); + VA->setFixed(false); + optimizer.addVertex(VA); + } + } + + // Set Local cov keyframes vertices + // 5.2 优化位姿，速度与偏置 + int Ncov = vpOptimizableCovKFs.size(); + for (int i = 0; i < Ncov; i++) + { + KeyFrame *pKFi = vpOptimizableCovKFs[i]; + + VertexPose *VP = new VertexPose(pKFi); + VP->setId(pKFi->mnId); + VP->setFixed(false); + optimizer.addVertex(VP); + + if (pKFi->bImu) + { + VertexVelocity *VV = new VertexVelocity(pKFi); + VV->setId(maxKFid + 3 * (pKFi->mnId) + 1); + VV->setFixed(false); + optimizer.addVertex(VV); + VertexGyroBias *VG = new VertexGyroBias(pKFi); + VG->setId(maxKFid + 3 * (pKFi->mnId) + 2); + VG->setFixed(false); + optimizer.addVertex(VG); + VertexAccBias *VA = new VertexAccBias(pKFi); + VA->setId(maxKFid + 3 * (pKFi->mnId) + 3); + VA->setFixed(false); + optimizer.addVertex(VA); + } + } + + // Set Fixed KeyFrame vertices + // 5.3 设置所有完全固定的关键帧顶点 + for (list::iterator lit = lFixedKeyFrames.begin(), lend = lFixedKeyFrames.end(); lit != lend; lit++) + { + KeyFrame *pKFi = *lit; + VertexPose *VP = new VertexPose(pKFi); + VP->setId(pKFi->mnId); + VP->setFixed(true); + optimizer.addVertex(VP); + + if (pKFi->bImu) + { + VertexVelocity *VV = new VertexVelocity(pKFi); + VV->setId(maxKFid + 3 * (pKFi->mnId) + 1); + VV->setFixed(true); + optimizer.addVertex(VV); + VertexGyroBias *VG = new VertexGyroBias(pKFi); + VG->setId(maxKFid + 3 * (pKFi->mnId) + 2); + VG->setFixed(true); + optimizer.addVertex(VG); + VertexAccBias *VA = new VertexAccBias(pKFi); + VA->setId(maxKFid + 3 * (pKFi->mnId) + 3); + VA->setFixed(true); + optimizer.addVertex(VA); + } + } + + // 6 设置所有的inertial的边 + // Create intertial constraints + // 预积分边 + vector vei(N, (EdgeInertial *)NULL); + // 角速度bias边 + vector vegr(N, (EdgeGyroRW *)NULL); + // 加速地bias边 + vector vear(N, (EdgeAccRW *)NULL); + + // 6.1 第一阶段优化vpOptimizableKFs里面的帧，遍历所有可优化的关键帧 + for (int i = 0; i < N; i++) + { + // cout << "inserting inertial edge " << i << endl; + KeyFrame *pKFi = vpOptimizableKFs[i]; + + if (!pKFi->mPrevKF) + { + Verbose::PrintMess("NOT INERTIAL LINK TO PREVIOUS FRAME!!!!", Verbose::VERBOSITY_NORMAL); + continue; + } + if (pKFi->bImu && pKFi->mPrevKF->bImu && pKFi->mpImuPreintegrated) + { + pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias()); + // 位姿 + g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId); + // 速度 + g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 1); + // 角速度bias + g2o::HyperGraph::Vertex *VG1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 2); + // 加速度bias + g2o::HyperGraph::Vertex *VA1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 3); + // 同上 + g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId); + g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1); + g2o::HyperGraph::Vertex *VG2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2); + g2o::HyperGraph::Vertex *VA2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3); + + if (!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2 || !VG2 || !VA2) + { + cerr << "Error " << VP1 << ", " << VV1 << ", " << VG1 << ", " << VA1 << ", " << VP2 << ", " << VV2 << ", " << VG2 << ", " << VA2 << endl; + continue; + } + + vei[i] = new EdgeInertial(pKFi->mpImuPreintegrated); + // 设置顶点 2*Pose + 2*Velocity + 1 角速度bias + 1 加速度bias + vei[i]->setVertex(0, dynamic_cast(VP1)); + vei[i]->setVertex(1, dynamic_cast(VV1)); + vei[i]->setVertex(2, dynamic_cast(VG1)); + vei[i]->setVertex(3, dynamic_cast(VA1)); + vei[i]->setVertex(4, dynamic_cast(VP2)); + vei[i]->setVertex(5, dynamic_cast(VV2)); + + // TODO Uncomment + // 设置优化参数 + g2o::RobustKernelHuber *rki = new g2o::RobustKernelHuber; + vei[i]->setRobustKernel(rki); + rki->setDelta(sqrt(16.92)); + // 添加边 + optimizer.addEdge(vei[i]); + + // 角速度bias的边 + vegr[i] = new EdgeGyroRW(); + // 设置顶点两个角速度bias + vegr[i]->setVertex(0, VG1); + vegr[i]->setVertex(1, VG2); + // 设置infomation matrix + Eigen::Matrix3d InfoG = pKFi->mpImuPreintegrated->C.block<3, 3>(9, 9).cast().inverse(); + vegr[i]->setInformation(InfoG); + optimizer.addEdge(vegr[i]); + // 设置加速度的边 + vear[i] = new EdgeAccRW(); + vear[i]->setVertex(0, VA1); + vear[i]->setVertex(1, VA2); + // 设置information matrix + Eigen::Matrix3d InfoA = pKFi->mpImuPreintegrated->C.block<3, 3>(12, 12).cast().inverse(); + vear[i]->setInformation(InfoA); + optimizer.addEdge(vear[i]); + } + else + Verbose::PrintMess("ERROR building inertial edge", Verbose::VERBOSITY_NORMAL); + } + + Verbose::PrintMess("end inserting inertial edges", Verbose::VERBOSITY_NORMAL); + + // Set MapPoint vertices + // 6.2 设置所有地图的顶点 + // 设置地图点顶点 + const int nExpectedSize = (N + Ncov + lFixedKeyFrames.size()) * lLocalMapPoints.size(); + + // 对于双目单目设置不同 + // Mono + vector vpEdgesMono; + vpEdgesMono.reserve(nExpectedSize); + + vector vpEdgeKFMono; + vpEdgeKFMono.reserve(nExpectedSize); + + vector vpMapPointEdgeMono; + vpMapPointEdgeMono.reserve(nExpectedSize); + + // Stereo + vector vpEdgesStereo; + vpEdgesStereo.reserve(nExpectedSize); + + vector vpEdgeKFStereo; + vpEdgeKFStereo.reserve(nExpectedSize); + + vector vpMapPointEdgeStereo; + vpMapPointEdgeStereo.reserve(nExpectedSize); + + const float thHuberMono = sqrt(5.991); + const float chi2Mono2 = 5.991; + const float thHuberStereo = sqrt(7.815); + const float chi2Stereo2 = 7.815; + + const unsigned long iniMPid = maxKFid * 5; + + // 遍历所有被可优化关键帧观测到的的地图点 + // 7. 添加重投影的边 + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) + { + MapPoint *pMP = *lit; + if (!pMP) + continue; + + g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ(); + vPoint->setEstimate(pMP->GetWorldPos().cast()); + + unsigned long id = pMP->mnId + iniMPid + 1; + vPoint->setId(id); + vPoint->setMarginalized(true); + // 添加顶点 + optimizer.addVertex(vPoint); + + const map> observations = pMP->GetObservations(); + + // Create visual constraints + // 添加重投影边 + for (map>::const_iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++) + { + KeyFrame *pKFi = mit->first; + + if (!pKFi) + continue; + + if ((pKFi->mnBALocalForKF != pCurrKF->mnId) && (pKFi->mnBAFixedForKF != pCurrKF->mnId)) + continue; + + if (pKFi->mnId > maxKFid) + { + continue; + } + + if (optimizer.vertex(id) == NULL || optimizer.vertex(pKFi->mnId) == NULL) + continue; + + if (!pKFi->isBad()) + { + // 3D点的观测 + const cv::KeyPoint &kpUn = pKFi->mvKeysUn[get<0>(mit->second)]; + // 如果是单目观测 + if (pKFi->mvuRight[get<0>(mit->second)] < 0) // Monocular observation + { + // 投影 + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y; + + EdgeMono *e = new EdgeMono(); + // 设置边的顶点 + // 3D点 + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + // 关键帧位姿 + e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); + e->setMeasurement(obs); + const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; + // 设置信息矩阵 + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberMono); + // 添加边 + optimizer.addEdge(e); + vpEdgesMono.push_back(e); + vpEdgeKFMono.push_back(pKFi); + vpMapPointEdgeMono.push_back(pMP); + } + // 双目 + else // stereo observation + { + const float kp_ur = pKFi->mvuRight[get<0>(mit->second)]; + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y, kp_ur; + + EdgeStereo *e = new EdgeStereo(); + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); + e->setMeasurement(obs); + const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; + e->setInformation(Eigen::Matrix3d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberStereo); + + optimizer.addEdge(e); + vpEdgesStereo.push_back(e); + vpEdgeKFStereo.push_back(pKFi); + vpMapPointEdgeStereo.push_back(pMP); + } + } + } + } + // 如果要停止就直接返回 + if (pbStopFlag) + if (*pbStopFlag) + return; + // 8. 开始优化 + optimizer.initializeOptimization(); + optimizer.optimize(8); + + // 更具优化结果挑选删除外点 + vector> vToErase; + vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size()); + + // Check inlier observations + // Mono + + // 9. 处理外点 + // 更具卡方检测来记录要删除的外点 + for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) + { + EdgeMono *e = vpEdgesMono[i]; + MapPoint *pMP = vpMapPointEdgeMono[i]; + + if (pMP->isBad()) + continue; + + if (e->chi2() > chi2Mono2) + { + KeyFrame *pKFi = vpEdgeKFMono[i]; + vToErase.push_back(make_pair(pKFi, pMP)); + } + } + + // Stereo + for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++) + { + EdgeStereo *e = vpEdgesStereo[i]; + MapPoint *pMP = vpMapPointEdgeStereo[i]; + + if (pMP->isBad()) + continue; + + if (e->chi2() > chi2Stereo2) + { + KeyFrame *pKFi = vpEdgeKFStereo[i]; + vToErase.push_back(make_pair(pKFi, pMP)); + } + } + + // Get Map Mutex and erase outliers + // 移除外点 + unique_lock lock(pMap->mMutexMapUpdate); + if (!vToErase.empty()) + { + for (size_t i = 0; i < vToErase.size(); i++) + { + KeyFrame *pKFi = vToErase[i].first; + MapPoint *pMPi = vToErase[i].second; + pKFi->EraseMapPointMatch(pMPi); + pMPi->EraseObservation(pKFi); + } + } + + // Recover optimized data + // 10. 取数 + // 根据优化的结果,修改每个被优化的变量 + // Keyframes + // 对于每个可优化关键帧 + for (int i = 0; i < N; i++) + { + KeyFrame *pKFi = vpOptimizableKFs[i]; + + // 修改位姿 + VertexPose *VP = static_cast(optimizer.vertex(pKFi->mnId)); + Sophus::SE3f Tcw(VP->estimate().Rcw[0].cast(), VP->estimate().tcw[0].cast()); + pKFi->SetPose(Tcw); + + // 在corrPoses记录每个关键帧融合后的位姿 + Sophus::SE3d Tiw = pKFi->GetPose().cast(); + g2o::Sim3 g2oSiw(Tiw.unit_quaternion(), Tiw.translation(), 1.0); + corrPoses[pKFi] = g2oSiw; + + if (pKFi->bImu) + { + // 速度 + VertexVelocity *VV = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1)); + // 修改速度 + pKFi->SetVelocity(VV->estimate().cast()); + // 陀螺仪bias + VertexGyroBias *VG = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2)); + // 加速度bias + VertexAccBias *VA = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3)); + Vector6d b; + // 修改bias + b << VG->estimate(), VA->estimate(); + pKFi->SetNewBias(IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2])); + } + } + // 遍历所有的共视帧 + for (int i = 0; i < Ncov; i++) + { + KeyFrame *pKFi = vpOptimizableCovKFs[i]; + + VertexPose *VP = static_cast(optimizer.vertex(pKFi->mnId)); + Sophus::SE3f Tcw(VP->estimate().Rcw[0].cast(), VP->estimate().tcw[0].cast()); + pKFi->SetPose(Tcw); + + Sophus::SE3d Tiw = pKFi->GetPose().cast(); + g2o::Sim3 g2oSiw(Tiw.unit_quaternion(), Tiw.translation(), 1.0); + corrPoses[pKFi] = g2oSiw; + + if (pKFi->bImu) + { + VertexVelocity *VV = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1)); + pKFi->SetVelocity(VV->estimate().cast()); + VertexGyroBias *VG = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2)); + VertexAccBias *VA = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3)); + Vector6d b; + b << VG->estimate(), VA->estimate(); + pKFi->SetNewBias(IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2])); + } + } + + // Points + // 对于所有的地图点 + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) + { + // 跟新位置和normal等信息 + MapPoint *pMP = *lit; + g2o::VertexSBAPointXYZ *vPoint = static_cast(optimizer.vertex(pMP->mnId + iniMPid + 1)); + pMP->SetWorldPos(vPoint->estimate().cast()); + pMP->UpdateNormalAndDepth(); + } + + pMap->IncreaseChangeIndex(); +} + +} // namespace ORB_SLAM diff --git a/ORB-SLAM3-UESTC/Workspace/src/PnPsolver.cc b/ORB-SLAM3-UESTC/Workspace/src/PnPsolver.cc new file mode 100644 index 0000000..b70bb30 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/PnPsolver.cc @@ -0,0 +1,1407 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +/** +* Copyright (c) 2009, V. Lepetit, EPFL +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +* +* The views and conclusions contained in the software and documentation are those +* of the authors and should not be interpreted as representing official policies, +* either expressed or implied, of the FreeBSD Project +*/ + +#include + +#include "PnPsolver.h" + +#include +#include +#include +#include "Thirdparty/DBoW2/DUtils/Random.h" +#include + +using namespace std; + +namespace ORB_SLAM3 +{ + +// 在大体的pipeline上和Sim3Solver差不多,都是 构造->设置RANSAC参数->外部调用迭代函数,进行计算->得到计算的结果 + +// pcs表示3D点在camera坐标系下的坐标 +// pws表示3D点在世界坐标系下的坐标 +// us表示图像坐标系下的2D点坐标 +// alphas为真实3D点用4个虚拟控制点表达时的系数 +// 构造函数 +PnPsolver::PnPsolver(const Frame &F, const vector &vpMapPointMatches): + pws(0), us(0), alphas(0), pcs(0), maximum_number_of_correspondences(0), number_of_correspondences(0), mnInliersi(0), + mnIterations(0), mnBestInliers(0), N(0) +{ + // 根据点数初始化容器的大小 + mvpMapPointMatches = vpMapPointMatches; //匹配关系 + mvP2D.reserve(F.mvpMapPoints.size()); //2D特征点 + mvSigma2.reserve(F.mvpMapPoints.size()); //特征点金字塔层级 + mvP3Dw.reserve(F.mvpMapPoints.size()); //世界坐标系下的3D点 + mvKeyPointIndices.reserve(F.mvpMapPoints.size()); //记录被使用特征点在原始特征点容器中的索引，因为有些3D点不一定存在，所以索引是不连续的 + mvAllIndices.reserve(F.mvpMapPoints.size()); //记录被使用特征点的索引，是连续的 + + // 生成地图点、对应2D特征点，记录一些索引坐标 + int idx=0; + // 遍历给出的每一个地图点 + for(size_t i=0, iend=vpMapPointMatches.size(); iisBad()) + { + const cv::KeyPoint &kp = F.mvKeysUn[i];//得到2维特征点, 将KeyPoint类型变为Point2f + + mvP2D.push_back(kp.pt); //存放2维特征点 + mvSigma2.push_back(F.mvLevelSigma2[kp.octave]); //记录特征点是在哪一层提取出来的 + + cv::Mat Pos = pMP->GetWorldPos(); //世界坐标系下的3D点 + mvP3Dw.push_back(cv::Point3f(Pos.at(0),Pos.at(1), Pos.at(2))); + + mvKeyPointIndices.push_back(i); //记录被使用特征点在原始特征点容器中的索引, mvKeyPointIndices是跳跃的 + mvAllIndices.push_back(idx); //记录被使用特征点的索引, mvAllIndices是连续的 + + idx++; + } + } + } // 遍历给出的每一个地图点 + + // Set camera calibration parameters + fu = F.fx; + fv = F.fy; + uc = F.cx; + vc = F.cy; + + // 设置默认的RANSAC参数,这个和Sim3Solver中的操作是相同的 + SetRansacParameters(); +} + +// 析构函数 +PnPsolver::~PnPsolver() +{ + // 释放堆内存 + delete [] pws; + delete [] us; + delete [] alphas; + delete [] pcs; +} + + +/** + * @brief 设置RANSAC迭代的参数 + * @param[in] probability 用于计算RANSAC理论迭代次数所用的概率 + * @param[in] minInliers 退出RANSAC所需要的最小内点个数, 注意这个只是给定值,最终迭代的时候不一定按照这个来 + * @param[in] maxIterations 设定的最大RANSAC迭代次数 + * @param[in] minSet 表示求解这个问题所需要的最小的样本数目,简称最小集;参与到最小内点数的确定过程中，默认是4 + * @param[in] epsilon 希望得到的 内点数/总体数 的比值,参与到最小内点数的确定过程中 + * @param[in] th2 内外点判定时的距离的baseline(程序中还会根据特征点所在的图层对这个阈值进行缩放的) + */ +void PnPsolver::SetRansacParameters(double probability, int minInliers, int maxIterations, int minSet, float epsilon, float th2) +{ + // 注意这次里在每一次采样的过程中,需要采样四个点,即最小集应该设置为4 + + // Step 1 获取给定的参数 + mRansacProb = probability; + mRansacMinInliers = minInliers; + mRansacMaxIts = maxIterations; + mRansacEpsilon = epsilon; + mRansacMinSet = minSet; + + + // Step 2 计算理论内点数,并且选 min(给定内点数,最小集,理论内点数) 作为最终在迭代过程中使用的最小内点数 + N = mvP2D.size(); // number of correspondences, 所有二维特征点个数 + + mvbInliersi.resize(N);// inlier index, mvbInliersi记录每次迭代inlier的点 + + // Adjust Parameters according to number of correspondences + // 再根据 epsilon 来计算理论上的内点数; + // NOTICE 实际在计算的过程中使用的 mRansacMinInliers = min(给定内点数,最小集,理论内点数) + int nMinInliers = N*mRansacEpsilon; + if(nMinInliers &vbInliers, int &nInliers) +{ + bool bFlag; + return iterate(mRansacMaxIts,bFlag,vbInliers,nInliers); +} + + +/** + * @brief EPnP迭代计算 + * + * @param[in] nIterations 迭代次数 + * @param[in] bNoMore 达到最大迭代次数的标志 + * @param[in] vbInliers 内点的标记 + * @param[in] nInliers 总共内点数 + * @return cv::Mat 计算出来的位姿 + */ +cv::Mat PnPsolver::iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers) +{ + bNoMore = false; //已经达到最大迭代次数的标志 + vbInliers.clear(); + nInliers=0; // 当前次迭代时的内点数 + + // mRansacMinSet 为每次RANSAC需要的特征点数，默认为4组3D-2D对应点 + set_maximum_number_of_correspondences(mRansacMinSet); + + // 如果已有匹配点数目比要求的内点数目还少，直接退出 + // N为所有2D点的个数, mRansacMinInliers 为正常退出RANSAC迭代过程中最少的inlier数 + if(N vAvailableIndices; + + // 当前的迭代次数id + int nCurrentIterations = 0; + + // 进行迭代的条件: + // 条件1: 历史进行的迭代次数少于最大迭代值 + // 条件2: 当前进行的迭代次数少于当前函数给定的最大迭代值 + while(mnIterations=mRansacMinInliers) + { + // If it is the best solution so far, save it + // 更新最佳的计算结果 + if(mnInliersi>mnBestInliers) + { + mvbBestInliers = mvbInliersi; + mnBestInliers = mnInliersi; + + cv::Mat Rcw(3,3,CV_64F,mRi); + cv::Mat tcw(3,1,CV_64F,mti); + Rcw.convertTo(Rcw,CV_32F); + tcw.convertTo(tcw,CV_32F); + mBestTcw = cv::Mat::eye(4,4,CV_32F); + Rcw.copyTo(mBestTcw.rowRange(0,3).colRange(0,3)); + tcw.copyTo(mBestTcw.rowRange(0,3).col(3)); + } // 更新最佳的计算结果 + + // 还要求精 + if(Refine()) // 如果求精成功(即表示求精之后的结果能够满足退出RANSAC迭代的内点数条件了) + { + nInliers = mnRefinedInliers; + // 转录,作为计算结果 + vbInliers = vector(mvpMapPointMatches.size(),false); + for(int i=0; i=mRansacMaxIts) + { + // 没有更多的允许迭代次数了 + bNoMore=true; + // 但是如果我们目前得到的最好结果看上去还不错的话 + if(mnBestInliers>=mRansacMinInliers) + { + // 返回计算结果 + nInliers=mnBestInliers; + vbInliers = vector(mvpMapPointMatches.size(),false); + for(int i=0; i vIndices; + vIndices.reserve(mvbBestInliers.size()); + + for(size_t i=0; imRansacMinInliers) + { + // 各种参数各种设置 + cv::Mat Rcw(3,3,CV_64F,mRi); + cv::Mat tcw(3,1,CV_64F,mti); + Rcw.convertTo(Rcw,CV_32F); + tcw.convertTo(tcw,CV_32F); + mRefinedTcw = cv::Mat::eye(4,4,CV_32F); + Rcw.copyTo(mRefinedTcw.rowRange(0,3).colRange(0,3)); + tcw.copyTo(mRefinedTcw.rowRange(0,3).col(3)); + return true; + } + + return false; +} + +/** + * @brief 通过之前求解的位姿来进行3D-2D投影，统计内点数目 + * + */ +void PnPsolver::CheckInliers() +{ + mnInliersi=0; + + // 遍历当前帧中所有的匹配点 + for(int i=0; idata.db[3 * i + j] = pws[3 * i + j] - cws[0][j]; + + // Step 2.2：利用特征值分解得到三个主方向 + // PW0^T * PW0 + // cvMulTransposed(A_src,Res_dst,order, delta=null,scale=1): + // Calculates Res=(A-delta)*(A-delta)^T (order=0) or (A-delta)^T*(A-delta) (order=1) + cvMulTransposed(PW0, &PW0tPW0, 1); + + // 这里实际是特征值分解 + cvSVD(&PW0tPW0, // A + &DC, // W，实际是特征值 + &UCt, // U，实际是特征向量 + 0, // V + CV_SVD_MODIFY_A | CV_SVD_U_T); // flags + + cvReleaseMat(&PW0); + + // Step 2.3：得到C1, C2, C3三个3D控制点，最后加上之前减掉的第一个控制点这个偏移量 + // ?这里的循环次数不应写成4,而应该是变量 number_of_correspondences + for(int i = 1; i < 4; i++) { + // 这里只需要遍历后面3个控制点 + double k = sqrt(dc[i - 1] / number_of_correspondences); + for(int j = 0; j < 3; j++) + cws[i][j] = cws[0][j] + k * uct[3 * (i - 1) + j]; + } +} + +/** + * @brief 求解世界坐标系下四个控制点的系数alphas，在相机坐标系下系数不变 + * + */ +void PnPsolver::compute_barycentric_coordinates(void) +{ + // pws为世界坐标系下3D参考点的坐标 + // cws1 cws2 cws3 cws4为世界坐标系下四个控制点的坐标 + // alphas 四个控制点的系数，每一个pws，都有一组alphas与之对应 + double cc[3 * 3], cc_inv[3 * 3]; + CvMat CC = cvMat(3, 3, CV_64F, cc); // 除第1个控制点外，另外3个控制点在控制点坐标系下的坐标 + CvMat CC_inv = cvMat(3, 3, CV_64F, cc_inv); // 上面这个矩阵的逆矩阵 + + // Step 1：第一个控制点在质心的位置，后面三个控制点减去第一个控制点的坐标（以第一个控制点为原点） + // 减去质心后得到x y z轴 + // + // cws的排列 |cws1_x cws1_y cws1_z| ---> |cws1| + // |cws2_x cws2_y cws2_z| |cws2| + // |cws3_x cws3_y cws3_z| |cws3| + // |cws4_x cws4_y cws4_z| |cws4| + // + // cc的排列 |cc2_x cc3_x cc4_x| --->|cc2 cc3 cc4| + // |cc2_y cc3_y cc4_y| + // |cc2_z cc3_z cc4_z| + + // 将后面3个控制点cws 去重心后 转化为 cc + for(int i = 0; i < 3; i++) // x y z 轴 + for(int j = 1; j < 4; j++) // 哪个控制点 + cc[3 * i + j - 1] = cws[j][i] - cws[0][i]; // 坐标索引中的-1是考虑到跳过了第1个控制点0 + + cvInvert(&CC, &CC_inv, CV_SVD); + double * ci = cc_inv; + for(int i = 0; i < number_of_correspondences; i++) { + double * pi = pws + 3 * i; // pi指向第i个3D点的首地址 + double * a = alphas + 4 * i; // a指向第i个控制点系数alphas的首地址 + + // pi[]-cws[0][]表示去质心 + // a0,a1,a2,a3 对应的是四个控制点的齐次重心坐标 + for(int j = 0; j < 3; j++) + // +1 是因为跳过了a0 + /* 这里的原理基本上是这个样子：(这里公式的下标和程序中的不一样，是从1开始的) + * cp=p_i-c1 + * cp=a1(c1-c1)+a2(c2-c1)+a3(c3-c1)+a4(c4-c1) + * => a2*cc2+a3*cc3+a4*cc4 + * [cc2 cc3 cc4] * [a2 a3 a4]^T = cp + * => [a2 a3 a4]^T = [cc2 cc3 cc4]^(-1) * cp + */ + a[1 + j] = ci[3 * j ] * (pi[0] - cws[0][0]) + + ci[3 * j + 1] * (pi[1] - cws[0][1]) + + ci[3 * j + 2] * (pi[2] - cws[0][2]); + // 最后计算用于进行归一化的a0 + a[0] = 1.0f - a[1] - a[2] - a[3]; + } +} + +/** + * @brief 根据提供的每一对点的数据来填充矩阵 M. 每对匹配点的数据可以填充两行 + * @param[in] M cvMat对应,存储矩阵M + * @param[in] row 开始填充数据的行 + * @param[in] alphas 世界坐标系下3D点用4个虚拟控制点表达时的4个系数 + * @param[in] u 2D点坐标u + * @param[in] v 2D点坐标v + */ +void PnPsolver::fill_M(CvMat * M, + const int row, const double * as, const double u, const double v) +{ + // 第一行起点 + double * M1 = M->data.db + row * 12; + // 第二行起点 + double * M2 = M1 + 12; + + // 对每一个参考点对： + // |ai1*fu, 0, ai1(uc-ui),| ai2*fu, 0, ai2(uc-ui),| ai3*fu, 0, ai3(uc-ui),| ai4*fu, 0, ai4(uc-ui)| + // |0, ai1*fv, ai1(vc-vi),| 0, ai2*fv, ai2(vc-vi),| 0, ai3*fv, ai3(vc-vi),| 0, ai4*fv, ai4(vc-vi)| + // 每一个特征点i有两行,每一行根据j=1,2,3,4可以分成四个部分,这也就是下面的for循环中所进行的工作 + for(int i = 0; i < 4; i++) { + M1[3 * i ] = as[i] * fu; + M1[3 * i + 1] = 0.0; + M1[3 * i + 2] = as[i] * (uc - u); + + M2[3 * i ] = 0.0; + M2[3 * i + 1] = as[i] * fv; + M2[3 * i + 2] = as[i] * (vc - v); + } +} + +/** + * @brief 通过给出的beta和vi,计算控制点在相机坐标系下的坐标 + * @param[in] betas beta + * @param[in] ut 其实是vi + */ +void PnPsolver::compute_ccs(const double * betas, const double * ut) +{ + // Step 1 赋值初始值为0 + for(int i = 0; i < 4; i++) + ccs[i][0] = ccs[i][1] = ccs[i][2] = 0.0f; + + // Step 2 + for(int i = 0; i < 4; i++) { + // 注意这里传过来的向量ut中,最后的部分才是v,依次是 x x x ... x v4 v3 v2 v1 + // 这里就是在最后面一次取出 v1 ~ v4 + const double * v = ut + 12 * (11 - i); + + for(int j = 0; j < 4; j++) // j表示当前计算的是第几个控制点 + for(int k = 0; k < 3; k++) // k表示当前计算的是控制点的哪个坐标 + ccs[j][k] += betas[i] * v[3 * j + k]; + } +} + +/** + * @brief 根据相机坐标系下控制点坐标ccs 和控制点系数 alphas（通过世界坐标系下3D点计算得到），得到相机坐标系下3D点坐标 pcs + * 过程可以参考 https://blog.csdn.net/jessecw79/article/details/82945918 + */ +void PnPsolver::compute_pcs(void) +{ + // 遍历所有的空间点 + for(int i = 0; i < number_of_correspondences; i++) { + // 定位 + double * a = alphas + 4 * i; + double * pc = pcs + 3 * i; + + // 计算 + for(int j = 0; j < 3; j++) + pc[j] = a[0] * ccs[0][j] + a[1] * ccs[1][j] + a[2] * ccs[2][j] + a[3] * ccs[3][j]; + } +} + +/** + * @brief 使用EPnP算法计算相机的位姿.其中匹配点的信息由类的成员函数给定 + * @param[out] R 旋转 + * @param[out] T 平移 + * @return double 使用这对旋转和平移的时候, 匹配点对的平均重投影误差 + */ +double PnPsolver::compute_pose(double R[3][3], double t[3]) +{ + // Step 1：获得EPnP算法中的四个控制点 + choose_control_points(); + + // Step 2：计算世界坐标系下每个3D点用4个控制点线性表达时的系数alphas + compute_barycentric_coordinates(); + + // Step 3：构造M矩阵，EPnP原始论文中公式(3)(4)-->(5)(6)(7); 矩阵的大小为 2n*12 ,n 为使用的匹配点的对数 + CvMat * M = cvCreateMat(2 * number_of_correspondences, 12, CV_64F); + + // 根据每一对匹配点的数据来填充矩阵M中的数据 + // alphas: 世界坐标系下3D点用4个虚拟控制点表达时的系数 + // us: 图像坐标系下的2D点坐标 + for(int i = 0; i < number_of_correspondences; i++) + fill_M(M, 2 * i, alphas + 4 * i, us[2 * i], us[2 * i + 1]); + + double mtm[12 * 12], d[12], ut[12 * 12]; + CvMat MtM = cvMat(12, 12, CV_64F, mtm); + CvMat D = cvMat(12, 1, CV_64F, d); // 这里实际是特征值 + CvMat Ut = cvMat(12, 12, CV_64F, ut); // 这里实际是特征向量 + + // Step 4：求解Mx = 0 + + // Step 4.1 先计算其中的特征向量vi + // 求M'M + cvMulTransposed(M, &MtM, 1); + // 该函数实际是特征值分解，得到特征值D，特征向量ut，对应EPnP论文式(8)中的vi + cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T); + cvReleaseMat(&M); + + // Step 4.2 计算分情况讨论的时候需要用到的矩阵L和\rho + // EPnP论文中式13中的L和\rho + double l_6x10[6 * 10], rho[6]; + CvMat L_6x10 = cvMat(6, 10, CV_64F, l_6x10); + CvMat Rho = cvMat(6, 1, CV_64F, rho); + + // 计算这两个量,6x10是先准备按照EPnP论文中的N=4来计算的 + compute_L_6x10(ut, l_6x10); + compute_rho(rho); + + + // Step 4.3 分情况计算N=2,3,4时能够求解得到的相机位姿R,t并且得到平均重投影误差 + double Betas[4][4], // 本质上就四个beta1~4,但是这里有四种情况(第一维度表示) + rep_errors[4]; // 重投影误差 + double Rs[4][3][3], //每一种情况迭代优化后得到的旋转矩阵 + ts[4][3]; //每一种情况迭代优化后得到的平移向量 + + // 不管什么情况，都假设论文中N=4，并求解部分betas（如果全求解出来会有冲突） + // 通过优化得到剩下的 betas + // 最后计算R t + + + // 求解近似解：N=4的情况 + find_betas_approx_1(&L_6x10, &Rho, Betas[1]); + // 高斯牛顿法迭代优化得到 beta + gauss_newton(&L_6x10, &Rho, Betas[1]); + rep_errors[1] = compute_R_and_t(ut, Betas[1], Rs[1], ts[1]); // 注意是每对匹配点的平均的重投影误差 + + // 求解近似解：N=2的情况 + find_betas_approx_2(&L_6x10, &Rho, Betas[2]); + gauss_newton(&L_6x10, &Rho, Betas[2]); + rep_errors[2] = compute_R_and_t(ut, Betas[2], Rs[2], ts[2]); + + // 求解近似解：N=3的情况 + find_betas_approx_3(&L_6x10, &Rho, Betas[3]); + gauss_newton(&L_6x10, &Rho, Betas[3]); + rep_errors[3] = compute_R_and_t(ut, Betas[3], Rs[3], ts[3]); + + // Step 5 看看哪种情况得到的效果最好,然后就选哪个 + int N = 1; // trick , 这样可以减少一种情况的计算 + if (rep_errors[2] < rep_errors[1]) N = 2; + if (rep_errors[3] < rep_errors[N]) N = 3; + + // Step 6 将最佳计算结果保存到返回计算结果用的变量中 + copy_R_and_t(Rs[N], ts[N], R, t); + + // Step 7 并且返回平均匹配点对的重投影误差,作为对相机位姿估计的评价 + return rep_errors[N]; +} + +/** + * @brief 复制计算得到的位姿到另外的一组变量中 + * @param[in] R_dst + * @param[in] t_dst + * @param[out] R_src + * @param[out] t_src + */ +void PnPsolver::copy_R_and_t(const double R_src[3][3], const double t_src[3], + double R_dst[3][3], double t_dst[3]) +{ + for(int i = 0; i < 3; i++) { + for(int j = 0; j < 3; j++) + R_dst[i][j] = R_src[i][j]; + + // trick 放在这里用来尽可能减少计算的复杂度 + t_dst[i] = t_src[i]; + } +} + +// 计算两个三维向量所表示的空间点的欧式距离的平方 +double PnPsolver::dist2(const double * p1, const double * p2) +{ + return + (p1[0] - p2[0]) * (p1[0] - p2[0]) + + (p1[1] - p2[1]) * (p1[1] - p2[1]) + + (p1[2] - p2[2]) * (p1[2] - p2[2]); +} + +// 计算两个三维向量的点乘 +double PnPsolver::dot(const double * v1, const double * v2) +{ + return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]; +} + +/** + * @brief 计算在给定位姿的时候的3D点投影误差 + * @param[in] R 给定旋转 + * @param[in] t 给定平移 + * @return double 重投影误差,是平均到每一对匹配点上的误差 + */ +double PnPsolver::reprojection_error(const double R[3][3], const double t[3]) +{ + // 统计其实并不是平方 + double sum2 = 0.0; + + // 遍历每个3D点 + for(int i = 0; i < number_of_correspondences; i++) { + // 指针定位 + double * pw = pws + 3 * i; + // 计算这个3D点在相机坐标系下的坐标,逆深度表示 + double Xc = dot(R[0], pw) + t[0]; + double Yc = dot(R[1], pw) + t[1]; + double inv_Zc = 1.0 / (dot(R[2], pw) + t[2]); + // 计算投影点 + double ue = uc + fu * Xc * inv_Zc; + double ve = vc + fv * Yc * inv_Zc; + // 计算投影点与匹配2D点的欧氏距离的平方 + double u = us[2 * i], v = us[2 * i + 1]; + // 得到其欧式距离并累加 + sum2 += sqrt( (u - ue) * (u - ue) + (v - ve) * (v - ve) ); + } + // 返回平均误差 + return sum2 / number_of_correspondences; +} + +/** + * @brief 用3D点在世界坐标系和相机坐标系下对应的坐标，用ICP求取R t + * @param[out] R 旋转 + * @param[out] t 平移 + */ +void PnPsolver::estimate_R_and_t(double R[3][3], double t[3]) +{ + // Step 1 计算3D点的质心 + double pc0[3], //3D点世界坐标系下坐标的质心 + pw0[3]; //3D点相机坐标系下坐标的质心 + // 初始化这两个质心 + pc0[0] = pc0[1] = pc0[2] = 0.0; + pw0[0] = pw0[1] = pw0[2] = 0.0; + + // 然后累加求质心 + for(int i = 0; i < number_of_correspondences; i++) { + const double * pc = pcs + 3 * i; + const double * pw = pws + 3 * i; + + for(int j = 0; j < 3; j++) { + pc0[j] += pc[j]; + pw0[j] += pw[j]; + } + } + for(int j = 0; j < 3; j++) { + pc0[j] /= number_of_correspondences; + pw0[j] /= number_of_correspondences; + } + + // 准备构造矩阵A,B以及B^T*A的SVD分解的值 + double abt[3 * 3], abt_d[3], abt_u[3 * 3], abt_v[3 * 3]; + CvMat ABt = cvMat(3, 3, CV_64F, abt); // H=B^T*A + CvMat ABt_D = cvMat(3, 1, CV_64F, abt_d); // 奇异值分解得到的特征值 + CvMat ABt_U = cvMat(3, 3, CV_64F, abt_u); // 奇异值分解得到的左特征矩阵 + CvMat ABt_V = cvMat(3, 3, CV_64F, abt_v); // 奇异值分解得到的右特征矩阵 + + // Step 2 构造矩阵H=B^T*A,不过这里是隐含的构造 + cvSetZero(&ABt); + // 遍历每一个3D点 + for(int i = 0; i < number_of_correspondences; i++) { + // 定位 + double * pc = pcs + 3 * i; + double * pw = pws + 3 * i; + + // 计算H=B^T*A,其中的两个矩阵构造和相乘的操作被融合在一起了 + for(int j = 0; j < 3; j++) { + abt[3 * j ] += (pc[j] - pc0[j]) * (pw[0] - pw0[0]); + abt[3 * j + 1] += (pc[j] - pc0[j]) * (pw[1] - pw0[1]); + abt[3 * j + 2] += (pc[j] - pc0[j]) * (pw[2] - pw0[2]); + } + } + + // Step 3 对得到的H矩阵进行奇异值分解 + cvSVD(&ABt, &ABt_D, &ABt_U, &ABt_V, CV_SVD_MODIFY_A); + + // Step 4 R=U*V^T, 并且进行合法性检查 + for(int i = 0; i < 3; i++) + for(int j = 0; j < 3; j++) + R[i][j] = dot(abt_u + 3 * i, abt_v + 3 * j); + + // 注意在得到了R以后,需要保证 det(R)=1>0 + const double det = + R[0][0] * R[1][1] * R[2][2] + R[0][1] * R[1][2] * R[2][0] + R[0][2] * R[1][0] * R[2][1] - + R[0][2] * R[1][1] * R[2][0] - R[0][1] * R[1][0] * R[2][2] - R[0][0] * R[1][2] * R[2][1]; + // 如果小于0那么就要这样做 + if (det < 0) { + R[2][0] = -R[2][0]; + R[2][1] = -R[2][1]; + R[2][2] = -R[2][2]; + } + + // Step 5 根据R计算t + t[0] = pc0[0] - dot(R[0], pw0); + t[1] = pc0[1] - dot(R[1], pw0); + t[2] = pc0[2] - dot(R[2], pw0); +} + +// DEPRECATED 输出位姿,不过目测这个函数应该是在ORB中被废弃了(因为根本用不到啊) +void PnPsolver::print_pose(const double R[3][3], const double t[3]) +{ + cout << R[0][0] << " " << R[0][1] << " " << R[0][2] << " " << t[0] << endl; + cout << R[1][0] << " " << R[1][1] << " " << R[1][2] << " " << t[1] << endl; + cout << R[2][0] << " " << R[2][1] << " " << R[2][2] << " " << t[2] << endl; +} + +// 保持所有点在相机坐标系下的深度为正,调整符号 +void PnPsolver::solve_for_sign(void) +{ + // 根据第一个3D点在当前相机坐标系下的深度,调整所有的3D点的深度为正(因为正常地来讲,这些3D点都应该是在相机前面的) + // 如果第一个点的深度是负的话 + if (pcs[2] < 0.0) { + // 先调整控制点的坐标 + for(int i = 0; i < 4; i++) + for(int j = 0; j < 3; j++) + ccs[i][j] = -ccs[i][j]; + + // 然后调整3D点的坐标 + for(int i = 0; i < number_of_correspondences; i++) { + pcs[3 * i ] = -pcs[3 * i]; + pcs[3 * i + 1] = -pcs[3 * i + 1]; + pcs[3 * i + 2] = -pcs[3 * i + 2]; + } + } +} + + +/** + * @brief 根据已经得到的控制点在当前相机坐标系下的坐标来恢复出相机的位姿 + * @param[in] ut vi + * @param[in] betas betas + * @param[out] R 计算得到的相机旋转R + * @param[out] t 计算得到的相机位置t + * @return double 使用这个位姿,所得到的重投影误差 + */ +double PnPsolver::compute_R_and_t(const double * ut, const double * betas, + double R[3][3], double t[3]) +{ + // Step 1 根据前面的计算结果来"组装"得到控制点在当前相机坐标系下的坐标 + compute_ccs(betas, ut); + // Step 2 将世界坐标系下的3D点的坐标转换到控制点的坐标系下 + compute_pcs(); + // Step 3 调整点坐标的符号,来保证在相机坐标系下点的深度为正 + solve_for_sign(); + + // Step 4 ICP计算R和t + estimate_R_and_t(R, t); + + // Step 5 计算使用这个位姿,所得到的每对点平均的重投影误差,作为返回值 + return reprojection_error(R, t); +} + +/** + * @brief 计算N=4时候的粗糙近似解，暴力将其他量置为0 + * + * @param[in] L_6x10 矩阵L + * @param[in] Rho 非齐次项 \rho, 列向量 + * @param[out] betas 计算得到的beta + */ +void PnPsolver::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, + double * betas) +{ + // 计算N=4时候的粗糙近似解，暴力将其他量置为0 + // betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44] -- L_6x10中每一行的内容 + // betas_approx_1 = [B11 B12 B13 B14 ] -- L_6x4 中一行提取出来的内容 + + double l_6x4[6 * 4], b4[4]; + CvMat L_6x4 = cvMat(6, 4, CV_64F, l_6x4); + CvMat B4 = cvMat(4, 1, CV_64F, b4); + + // 提取L_6x10矩阵中每行的第0,1,3,6个元素，得到L_6x4 + for(int i = 0; i < 6; i++) { + cvmSet(&L_6x4, i, 0, cvmGet(L_6x10, i, 0)); //将L_6x10的第i行的第0个元素设置为L_6x4的第i行的第0个元素 + cvmSet(&L_6x4, i, 1, cvmGet(L_6x10, i, 1)); + cvmSet(&L_6x4, i, 2, cvmGet(L_6x10, i, 3)); + cvmSet(&L_6x4, i, 3, cvmGet(L_6x10, i, 6)); + } + + // SVD方式求解方程组 L_6x4 * B4 = Rho + cvSolve(&L_6x4, Rho, &B4, CV_SVD); + // 得到的解是 b00 b01 b02 b03 因此解出来b00即可 + if (b4[0] < 0) { + betas[0] = sqrt(-b4[0]); + betas[1] = -b4[1] / betas[0]; + betas[2] = -b4[2] / betas[0]; + betas[3] = -b4[3] / betas[0]; + } else { + betas[0] = sqrt(b4[0]); + betas[1] = b4[1] / betas[0]; + betas[2] = b4[2] / betas[0]; + betas[3] = b4[3] / betas[0]; + } +} + +/** + * @brief 计算N=2时候的粗糙近似解，暴力将其他量置为0 + * + * @param[in] L_6x10 矩阵L + * @param[in] Rho 非齐次项 \rho, 列向量 + * @param[out] betas 计算得到的beta + */ +void PnPsolver::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, + double * betas) +{ + // betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44] + // betas_approx_2 = [B11 B12 B22 ] + double l_6x3[6 * 3], b3[3]; + CvMat L_6x3 = cvMat(6, 3, CV_64F, l_6x3); + CvMat B3 = cvMat(3, 1, CV_64F, b3); + + // 提取 + for(int i = 0; i < 6; i++) { + cvmSet(&L_6x3, i, 0, cvmGet(L_6x10, i, 0)); + cvmSet(&L_6x3, i, 1, cvmGet(L_6x10, i, 1)); + cvmSet(&L_6x3, i, 2, cvmGet(L_6x10, i, 2)); + } + + // 求解方程组 + cvSolve(&L_6x3, Rho, &B3, CV_SVD); + + // 从b11 b12 b22 中恢复 b1 b2 + if (b3[0] < 0) { + betas[0] = sqrt(-b3[0]); + betas[1] = (b3[2] < 0) ? sqrt(-b3[2]) : 0.0; + } else { + betas[0] = sqrt(b3[0]); + betas[1] = (b3[2] > 0) ? sqrt(b3[2]) : 0.0; + } + + if (b3[1] < 0) betas[0] = -betas[0]; + + // 这俩没有使用到 + betas[2] = 0.0; + betas[3] = 0.0; +} + + +/** + * @brief 计算N=3时候的粗糙近似解，暴力将其他量置为0 + * + * @param[in] L_6x10 矩阵L + * @param[in] Rho 非齐次项 \rho, 列向量 + * @param[out] betas 计算得到的beta + */ +void PnPsolver::find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, + double * betas) +{ + // betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44] + // betas_approx_3 = [B11 B12 B22 B13 B23 ] + double l_6x5[6 * 5], b5[5]; + CvMat L_6x5 = cvMat(6, 5, CV_64F, l_6x5); + CvMat B5 = cvMat(5, 1, CV_64F, b5); + + // 获取并构造矩阵 + for(int i = 0; i < 6; i++) { + cvmSet(&L_6x5, i, 0, cvmGet(L_6x10, i, 0)); + cvmSet(&L_6x5, i, 1, cvmGet(L_6x10, i, 1)); + cvmSet(&L_6x5, i, 2, cvmGet(L_6x10, i, 2)); + cvmSet(&L_6x5, i, 3, cvmGet(L_6x10, i, 3)); + cvmSet(&L_6x5, i, 4, cvmGet(L_6x10, i, 4)); + } + + // 求解这个方程组 + cvSolve(&L_6x5, Rho, &B5, CV_SVD); + + // 从 B11 B12 B22 B13 B23 中恢复出 B1 B2 B3 + if (b5[0] < 0) { + betas[0] = sqrt(-b5[0]); + betas[1] = (b5[2] < 0) ? sqrt(-b5[2]) : 0.0; + } else { + betas[0] = sqrt(b5[0]); + betas[1] = (b5[2] > 0) ? sqrt(b5[2]) : 0.0; + } + if (b5[1] < 0) betas[0] = -betas[0]; + betas[2] = b5[3] / betas[0]; + + // N=3的时候没有B4 + betas[3] = 0.0; +} + +/** + * @brief 计算矩阵L,论文式13中的L矩阵,不过这里的是按照N=4的时候计算的 + * + * @param[in] ut 特征值分解之后得到的12x12特征矩阵 + * @param[out] l_6x10 计算的L矩阵结果，维度6x10 + */ +void PnPsolver::compute_L_6x10(const double * ut, double * l_6x10) +{ + // Step 1 获取最后4个零特征值对应的4个12x1的特征向量 + const double * v[4]; + + // 对应EPnP里N=4的情况。直接取特征向量的最后4行 + // 以这里的v[0]为例，它是12x1的向量，会拆成4个3x1的向量v[0]^[0]，v[0]^[1]，v[0]^[1]，v[0]^[3]，对应4个相机坐标系控制点 + v[0] = ut + 12 * 11; // v[0] : v[0][0]~v[0][2] => v[0]^[0] , * \beta_0 = c0 (理论上) + // v[0][3]~v[0][5] => v[0]^[1] , * \beta_0 = c1 + // v[0][6]~v[0][8] => v[0]^[2] , * \beta_0 = c2 + // v[0][9]~v[0][11] => v[0]^[3] , * \beta_0 = c3 + v[1] = ut + 12 * 10; + v[2] = ut + 12 * 9; + v[3] = ut + 12 * 8; + + // Step 2 提前计算中间变量dv + // dv表示中间变量，是difference-vector的缩写 + // 4 表示N=4时对应的4个12x1的向量v, 6 表示4对点一共有6种两两组合的方式，3 表示v^[i]是一个3维的列向量 + double dv[4][6][3]; + + + // N=4时候的情况. 控制第一个下标的就是a,第二个下标的就是b,不过下面的循环中下标都是从0开始的 + for(int i = 0; i < 4; i++) { + // 每一个向量v[i]可以提供四个控制点的"雏形"v[i]^[0]~v[i]^[3] + // 这四个"雏形"两两组合一共有六种组合方式: + // 下面的a变量就是前面的那个id,b就是后面的那个id + int a = 0, b = 1; + for(int j = 0; j < 6; j++) { + // dv[i][j]=v[i]^[a]-v[i]^[b] + // a,b的取值有6种组合 0-1 0-2 0-3 1-2 1-3 2-3 + dv[i][j][0] = v[i][3 * a ] - v[i][3 * b ]; + dv[i][j][1] = v[i][3 * a + 1] - v[i][3 * b + 1]; + dv[i][j][2] = v[i][3 * a + 2] - v[i][3 * b + 2]; + + b++; + if (b > 3) { + a++; + b = a + 1; + } + } + } + + // Step 3 用前面计算的dv生成L矩阵 + // 这里的6代表前面每个12x1维向量v的4个3x1子向量v^[i]对应的6种组合 + for(int i = 0; i < 6; i++) { + double * row = l_6x10 + 10 * i; + // 计算每一行中的每一个元素,总共是10个元素 // 对应的\beta列向量 + row[0] = dot(dv[0][i], dv[0][i]); //*b11 + row[1] = 2.0f * dot(dv[0][i], dv[1][i]); //*b12 + row[2] = dot(dv[1][i], dv[1][i]); //*b22 + row[3] = 2.0f * dot(dv[0][i], dv[2][i]); //*b13 + row[4] = 2.0f * dot(dv[1][i], dv[2][i]); //*b23 + row[5] = dot(dv[2][i], dv[2][i]); //*b33 + row[6] = 2.0f * dot(dv[0][i], dv[3][i]); //*b14 + row[7] = 2.0f * dot(dv[1][i], dv[3][i]); //*b24 + row[8] = 2.0f * dot(dv[2][i], dv[3][i]); //*b34 + row[9] = dot(dv[3][i], dv[3][i]); //*b44 + } +} + +/** + * @brief 计算四个控制点任意两点间的距离，总共6个距离，对应论文式13中的向量\rho + * @param[in] rho 计算结果 + */ +void PnPsolver::compute_rho(double * rho) +{ + // 四个点两两组合一共有6中组合方式: 01 02 03 12 13 23 + rho[0] = dist2(cws[0], cws[1]); + rho[1] = dist2(cws[0], cws[2]); + rho[2] = dist2(cws[0], cws[3]); + rho[3] = dist2(cws[1], cws[2]); + rho[4] = dist2(cws[1], cws[3]); + rho[5] = dist2(cws[2], cws[3]); +} + +/** + * @brief 计算高斯牛顿法优化时,增量方程中的系数矩阵和非齐次项 + * @param[in] l_6x10 L矩阵 + * @param[in] rho Rho矩向量 + * @param[in] cb 当前次迭代得到的beta1~beta4 + * @param[out] A 计算得到的增量方程中的系数矩阵 + * @param[out] b 计算得到的增量方程中的非齐次项 + */ +void PnPsolver::compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho, + double betas[4], CvMat * A, CvMat * b) +{ + // 一共有六个方程组, 对每一行(也就是每一个方程展开遍历); + // 从优化目标函数的概念出发,其中的每一行的约束均由一对点来提供,因此不同行之间其实并无关系,可以相互独立地计算 + for(int i = 0; i < 6; i++) { + // 获得矩阵L中的行指针 + const double * rowL = l_6x10 + i * 10; + double * rowA = A->data.db + i * 4; + + // Step 1: 计算当前行的雅克比 + rowA[0] = 2 * rowL[0] * betas[0] + rowL[1] * betas[1] + rowL[3] * betas[2] + rowL[6] * betas[3]; + rowA[1] = rowL[1] * betas[0] + 2 * rowL[2] * betas[1] + rowL[4] * betas[2] + rowL[7] * betas[3]; + rowA[2] = rowL[3] * betas[0] + rowL[4] * betas[1] + 2 * rowL[5] * betas[2] + rowL[8] * betas[3]; + rowA[3] = rowL[6] * betas[0] + rowL[7] * betas[1] + rowL[8] * betas[2] + 2 * rowL[9] * betas[3]; + + // Step 2: 计算当前行的非齐次项 + cvmSet(b, i, 0, rho[i] - + ( // 从0开始的下标 | 从1开始的下标 + rowL[0] * betas[0] * betas[0] + //b00 b11 + rowL[1] * betas[0] * betas[1] + //b01 b12 + rowL[2] * betas[1] * betas[1] + //b11 b22 + rowL[3] * betas[0] * betas[2] + //b02 b13 + rowL[4] * betas[1] * betas[2] + //b12 b23 + rowL[5] * betas[2] * betas[2] + //b22 b33 + rowL[6] * betas[0] * betas[3] + //b03 b14 + rowL[7] * betas[1] * betas[3] + //b13 b24 + rowL[8] * betas[2] * betas[3] + //b23 b34 + rowL[9] * betas[3] * betas[3] //b33 b44 + )); + } +} + +// 对计算出来的Beta结果进行高斯牛顿法优化,求精. 过程参考EPnP论文中式(15) +void PnPsolver::gauss_newton(const CvMat * L_6x10, const CvMat * Rho, + double betas[4]) +{ + // 只进行5次迭代 + const int iterations_number = 5; + + double a[6*4], b[6], x[4]; + CvMat A = cvMat(6, 4, CV_64F, a); // 系数矩阵 + CvMat B = cvMat(6, 1, CV_64F, b); // 非齐次项 + CvMat X = cvMat(4, 1, CV_64F, x); // 增量，待求量 + + // 对于每次迭代过程 + for(int k = 0; k < iterations_number; k++) { + // 计算增量方程的系数矩阵和非齐次项 + compute_A_and_b_gauss_newton(L_6x10->data.db, Rho->data.db, + betas, &A, &B); + // 使用QR分解来求解增量方程,解得当前次迭代的增量X + qr_solve(&A, &B, &X); + + // 应用增量,对估计值进行更新;估计值是beta1~beta4组成的向量 + for(int i = 0; i < 4; i++) + betas[i] += x[i]; + } +} + +/** + * @brief 使用QR分解来求解增量方程 + * @param[in] A 洗漱矩阵 + * @param[in] b 非齐次项 + * @param[out] X 增量 + */ +void PnPsolver::qr_solve(CvMat * A, CvMat * b, CvMat * X) +{ + static int max_nr = 0; + static double * A1, * A2; + + const int nr = A->rows; + const int nc = A->cols; + + if (max_nr != 0 && max_nr < nr) { + delete [] A1; + delete [] A2; + } + if (max_nr < nr) { + max_nr = nr; + A1 = new double[nr]; + A2 = new double[nr]; + } + + double * pA = A->data.db, * ppAkk = pA; + for(int k = 0; k < nc; k++) { + double * ppAik = ppAkk, eta = fabs(*ppAik); + for(int i = k + 1; i < nr; i++) { + double elt = fabs(*ppAik); + if (eta < elt) eta = elt; + ppAik += nc; + } + + if (eta == 0) { + A1[k] = A2[k] = 0.0; + cerr << "God damnit, A is singular, this shouldn't happen." << endl; + return; + } else { + double * ppAik = ppAkk, sum = 0.0, inv_eta = 1. / eta; + for(int i = k; i < nr; i++) { + *ppAik *= inv_eta; + sum += *ppAik * *ppAik; + ppAik += nc; + } + double sigma = sqrt(sum); + if (*ppAkk < 0) + sigma = -sigma; + *ppAkk += sigma; + A1[k] = sigma * *ppAkk; + A2[k] = -eta * sigma; + for(int j = k + 1; j < nc; j++) { + double * ppAik = ppAkk, sum = 0; + for(int i = k; i < nr; i++) { + sum += *ppAik * ppAik[j - k]; + ppAik += nc; + } + double tau = sum / A1[k]; + ppAik = ppAkk; + for(int i = k; i < nr; i++) { + ppAik[j - k] -= tau * *ppAik; + ppAik += nc; + } + } + } + ppAkk += nc + 1; + } + + // b <- Qt b + double * ppAjj = pA, * pb = b->data.db; + for(int j = 0; j < nc; j++) { + double * ppAij = ppAjj, tau = 0; + for(int i = j; i < nr; i++) { + tau += *ppAij * pb[i]; + ppAij += nc; + } + tau /= A1[j]; + ppAij = ppAjj; + for(int i = j; i < nr; i++) { + pb[i] -= tau * *ppAij; + ppAij += nc; + } + ppAjj += nc + 1; + } + + // X = R-1 b + double * pX = X->data.db; + pX[nc - 1] = pb[nc - 1] / A2[nc - 1]; + for(int i = nc - 2; i >= 0; i--) { + double * ppAij = pA + i * nc + (i + 1), sum = 0; + + for(int j = i + 1; j < nc; j++) { + sum += *ppAij * pX[j]; + ppAij++; + } + pX[i] = (pb[i] - sum) / A2[i]; + } +} + + + +void PnPsolver::relative_error(double & rot_err, double & transl_err, + const double Rtrue[3][3], const double ttrue[3], + const double Rest[3][3], const double test[3]) +{ + double qtrue[4], qest[4]; + + mat_to_quat(Rtrue, qtrue); + mat_to_quat(Rest, qest); + + double rot_err1 = sqrt((qtrue[0] - qest[0]) * (qtrue[0] - qest[0]) + + (qtrue[1] - qest[1]) * (qtrue[1] - qest[1]) + + (qtrue[2] - qest[2]) * (qtrue[2] - qest[2]) + + (qtrue[3] - qest[3]) * (qtrue[3] - qest[3]) ) / + sqrt(qtrue[0] * qtrue[0] + qtrue[1] * qtrue[1] + qtrue[2] * qtrue[2] + qtrue[3] * qtrue[3]); + + double rot_err2 = sqrt((qtrue[0] + qest[0]) * (qtrue[0] + qest[0]) + + (qtrue[1] + qest[1]) * (qtrue[1] + qest[1]) + + (qtrue[2] + qest[2]) * (qtrue[2] + qest[2]) + + (qtrue[3] + qest[3]) * (qtrue[3] + qest[3]) ) / + sqrt(qtrue[0] * qtrue[0] + qtrue[1] * qtrue[1] + qtrue[2] * qtrue[2] + qtrue[3] * qtrue[3]); + + rot_err = min(rot_err1, rot_err2); + + transl_err = + sqrt((ttrue[0] - test[0]) * (ttrue[0] - test[0]) + + (ttrue[1] - test[1]) * (ttrue[1] - test[1]) + + (ttrue[2] - test[2]) * (ttrue[2] - test[2])) / + sqrt(ttrue[0] * ttrue[0] + ttrue[1] * ttrue[1] + ttrue[2] * ttrue[2]); +} + +void PnPsolver::mat_to_quat(const double R[3][3], double q[4]) +{ + double tr = R[0][0] + R[1][1] + R[2][2]; + double n4; + + if (tr > 0.0f) { + q[0] = R[1][2] - R[2][1]; + q[1] = R[2][0] - R[0][2]; + q[2] = R[0][1] - R[1][0]; + q[3] = tr + 1.0f; + n4 = q[3]; + } else if ( (R[0][0] > R[1][1]) && (R[0][0] > R[2][2]) ) { + q[0] = 1.0f + R[0][0] - R[1][1] - R[2][2]; + q[1] = R[1][0] + R[0][1]; + q[2] = R[2][0] + R[0][2]; + q[3] = R[1][2] - R[2][1]; + n4 = q[0]; + } else if (R[1][1] > R[2][2]) { + q[0] = R[1][0] + R[0][1]; + q[1] = 1.0f + R[1][1] - R[0][0] - R[2][2]; + q[2] = R[2][1] + R[1][2]; + q[3] = R[2][0] - R[0][2]; + n4 = q[1]; + } else { + q[0] = R[2][0] + R[0][2]; + q[1] = R[2][1] + R[1][2]; + q[2] = 1.0f + R[2][2] - R[0][0] - R[1][1]; + q[3] = R[0][1] - R[1][0]; + n4 = q[2]; + } + double scale = 0.5f / double(sqrt(n4)); + + q[0] *= scale; + q[1] *= scale; + q[2] *= scale; + q[3] *= scale; +} + +} //namespace ORB_SLAM diff --git a/ORB-SLAM3-UESTC/Workspace/src/Settings.cc b/ORB-SLAM3-UESTC/Workspace/src/Settings.cc new file mode 100644 index 0000000..9114299 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/Settings.cc @@ -0,0 +1,731 @@ +/** + * This file is part of ORB-SLAM3 + * + * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * + * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public + * License as published by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even + * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with ORB-SLAM3. + * If not, see . + */ + +#include "Settings.h" + +#include "CameraModels/Pinhole.h" +#include "CameraModels/KannalaBrandt8.h" + +#include "System.h" + +#include +#include + +#include + +using namespace std; + +namespace ORB_SLAM3 +{ + +template <> +float Settings::readParameter(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required) +{ + cv::FileNode node = fSettings[name]; + if (node.empty()) + { + if (required) + { + std::cerr << name << " required parameter does not exist, aborting..." << std::endl; + exit(-1); + } + else + { + std::cerr << name << " optional parameter does not exist..." << std::endl; + found = false; + return 0.0f; + } + } + else if (!node.isReal()) + { + std::cerr << name << " parameter must be a real number, aborting..." << std::endl; + exit(-1); + } + else + { + found = true; + return node.real(); + } +} + +template <> +int Settings::readParameter(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required) +{ + cv::FileNode node = fSettings[name]; + if (node.empty()) + { + if (required) + { + std::cerr << name << " required parameter does not exist, aborting..." << std::endl; + exit(-1); + } + else + { + std::cerr << name << " optional parameter does not exist..." << std::endl; + found = false; + return 0; + } + } + else if (!node.isInt()) + { + std::cerr << name << " parameter must be an integer number, aborting..." << std::endl; + exit(-1); + } + else + { + found = true; + return node.operator int(); + } +} + +template <> +string Settings::readParameter(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required) +{ + cv::FileNode node = fSettings[name]; + if (node.empty()) + { + if (required) + { + std::cerr << name << " required parameter does not exist, aborting..." << std::endl; + exit(-1); + } + else + { + std::cerr << name << " optional parameter does not exist..." << std::endl; + found = false; + return string(); + } + } + else if (!node.isString()) + { + std::cerr << name << " parameter must be a string, aborting..." << std::endl; + exit(-1); + } + else + { + found = true; + return node.string(); + } +} + +template <> +cv::Mat Settings::readParameter(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required) +{ + cv::FileNode node = fSettings[name]; + if (node.empty()) + { + if (required) + { + std::cerr << name << " required parameter does not exist, aborting..." << std::endl; + exit(-1); + } + else + { + std::cerr << name << " optional parameter does not exist..." << std::endl; + found = false; + return cv::Mat(); + } + } + else + { + found = true; + return node.mat(); + } +} + +Settings::Settings(const std::string &configFile, const int &sensor) : bNeedToUndistort_(false), bNeedToRectify_(false), bNeedToResize1_(false), bNeedToResize2_(false) +{ + sensor_ = sensor; + + // Open settings file + cv::FileStorage fSettings(configFile, cv::FileStorage::READ); + if (!fSettings.isOpened()) + { + cerr << "[ERROR]: could not open configuration file at: " << configFile << endl; + cerr << "Aborting..." << endl; + + exit(-1); + } + else + { + cout << "Loading settings from " << configFile << endl; + } + + // Read first camera + readCamera1(fSettings); + cout << "\t-Loaded camera 1" << endl; + + // Read second camera if stereo (not rectified) + if (sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) + { + readCamera2(fSettings); + cout << "\t-Loaded camera 2" << endl; + } + + // Read image info + readImageInfo(fSettings); + cout << "\t-Loaded image info" << endl; + + if (sensor_ == System::IMU_MONOCULAR || sensor_ == System::IMU_STEREO || sensor_ == System::IMU_RGBD) + { + readIMU(fSettings); + cout << "\t-Loaded IMU calibration" << endl; + } + + if (sensor_ == System::RGBD || sensor_ == System::IMU_RGBD) + { + readRGBD(fSettings); + cout << "\t-Loaded RGB-D calibration" << endl; + } + + readORB(fSettings); + cout << "\t-Loaded ORB settings" << endl; + readViewer(fSettings); + cout << "\t-Loaded viewer settings" << endl; + readLoadAndSave(fSettings); + cout << "\t-Loaded Atlas settings" << endl; + readOtherParameters(fSettings); + cout << "\t-Loaded misc parameters" << endl; + + if (bNeedToRectify_) + { + precomputeRectificationMaps(); + cout << "\t-Computed rectification maps" << endl; + } + + cout << "----------------------------------" << endl; +} + +void Settings::readCamera1(cv::FileStorage &fSettings) +{ + bool found; + + // Read camera model + string cameraModel = readParameter(fSettings, "Camera.type", found); + + vector vCalibration; + if (cameraModel == "PinHole") + { + cameraType_ = PinHole; + + // Read intrinsic parameters + float fx = readParameter(fSettings, "Camera1.fx", found); + float fy = readParameter(fSettings, "Camera1.fy", found); + float cx = readParameter(fSettings, "Camera1.cx", found); + float cy = readParameter(fSettings, "Camera1.cy", found); + + vCalibration = {fx, fy, cx, cy}; + + calibration1_ = new Pinhole(vCalibration); + originalCalib1_ = new Pinhole(vCalibration); + + // Check if it is a distorted PinHole + readParameter(fSettings, "Camera1.k1", found, false); + if (found) + { + readParameter(fSettings, "Camera1.k3", found, false); + if (found) + { + vPinHoleDistorsion1_.resize(5); + vPinHoleDistorsion1_[4] = readParameter(fSettings, "Camera1.k3", found); + } + else + { + vPinHoleDistorsion1_.resize(4); + } + vPinHoleDistorsion1_[0] = readParameter(fSettings, "Camera1.k1", found); + vPinHoleDistorsion1_[1] = readParameter(fSettings, "Camera1.k2", found); + vPinHoleDistorsion1_[2] = readParameter(fSettings, "Camera1.p1", found); + vPinHoleDistorsion1_[3] = readParameter(fSettings, "Camera1.p2", found); + } + + // Check if we need to correct distortion from the images + if ( + (sensor_ == System::MONOCULAR || + sensor_ == System::IMU_MONOCULAR || + sensor_ == System::RGBD || + sensor_ == System::IMU_RGBD) && + vPinHoleDistorsion1_.size() != 0) + { + bNeedToUndistort_ = true; + } + } + else if (cameraModel == "Rectified") + { + cameraType_ = Rectified; + + // Read intrinsic parameters + float fx = readParameter(fSettings, "Camera1.fx", found); + float fy = readParameter(fSettings, "Camera1.fy", found); + float cx = readParameter(fSettings, "Camera1.cx", found); + float cy = readParameter(fSettings, "Camera1.cy", found); + + vCalibration = {fx, fy, cx, cy}; + + calibration1_ = new Pinhole(vCalibration); + originalCalib1_ = new Pinhole(vCalibration); + + // Rectified images are assumed to be ideal PinHole images (no distortion) + } + else if (cameraModel == "KannalaBrandt8") + { + cameraType_ = KannalaBrandt; + + // Read intrinsic parameters + float fx = readParameter(fSettings, "Camera1.fx", found); + float fy = readParameter(fSettings, "Camera1.fy", found); + float cx = readParameter(fSettings, "Camera1.cx", found); + float cy = readParameter(fSettings, "Camera1.cy", found); + + float k0 = readParameter(fSettings, "Camera1.k1", found); + float k1 = readParameter(fSettings, "Camera1.k2", found); + float k2 = readParameter(fSettings, "Camera1.k3", found); + float k3 = readParameter(fSettings, "Camera1.k4", found); + + vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3}; + + calibration1_ = new KannalaBrandt8(vCalibration); + originalCalib1_ = new KannalaBrandt8(vCalibration); + + if (sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) + { + int colBegin = readParameter(fSettings, "Camera1.overlappingBegin", found); + int colEnd = readParameter(fSettings, "Camera1.overlappingEnd", found); + vector vOverlapping = {colBegin, colEnd}; + + static_cast(calibration1_)->mvLappingArea = vOverlapping; + } + } + else + { + cerr << "Error: " << cameraModel << " not known" << endl; + exit(-1); + } +} + +void Settings::readCamera2(cv::FileStorage &fSettings) +{ + bool found; + vector vCalibration; + if (cameraType_ == PinHole) + { + bNeedToRectify_ = true; + + // Read intrinsic parameters + float fx = readParameter(fSettings, "Camera2.fx", found); + float fy = readParameter(fSettings, "Camera2.fy", found); + float cx = readParameter(fSettings, "Camera2.cx", found); + float cy = readParameter(fSettings, "Camera2.cy", found); + + vCalibration = {fx, fy, cx, cy}; + + calibration2_ = new Pinhole(vCalibration); + originalCalib2_ = new Pinhole(vCalibration); + + // Check if it is a distorted PinHole + readParameter(fSettings, "Camera2.k1", found, false); + if (found) + { + readParameter(fSettings, "Camera2.k3", found, false); + if (found) + { + vPinHoleDistorsion2_.resize(5); + vPinHoleDistorsion2_[4] = readParameter(fSettings, "Camera2.k3", found); + } + else + { + vPinHoleDistorsion2_.resize(4); + } + vPinHoleDistorsion2_[0] = readParameter(fSettings, "Camera2.k1", found); + vPinHoleDistorsion2_[1] = readParameter(fSettings, "Camera2.k2", found); + vPinHoleDistorsion2_[2] = readParameter(fSettings, "Camera2.p1", found); + vPinHoleDistorsion2_[3] = readParameter(fSettings, "Camera2.p2", found); + } + } + else if (cameraType_ == KannalaBrandt) + { + // Read intrinsic parameters + float fx = readParameter(fSettings, "Camera2.fx", found); + float fy = readParameter(fSettings, "Camera2.fy", found); + float cx = readParameter(fSettings, "Camera2.cx", found); + float cy = readParameter(fSettings, "Camera2.cy", found); + + float k0 = readParameter(fSettings, "Camera1.k1", found); + float k1 = readParameter(fSettings, "Camera1.k2", found); + float k2 = readParameter(fSettings, "Camera1.k3", found); + float k3 = readParameter(fSettings, "Camera1.k4", found); + + vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3}; + + calibration2_ = new KannalaBrandt8(vCalibration); + originalCalib2_ = new KannalaBrandt8(vCalibration); + + int colBegin = readParameter(fSettings, "Camera2.overlappingBegin", found); + int colEnd = readParameter(fSettings, "Camera2.overlappingEnd", found); + vector vOverlapping = {colBegin, colEnd}; + + static_cast(calibration2_)->mvLappingArea = vOverlapping; + } + + // Load stereo extrinsic calibration + if (cameraType_ == Rectified) + { + b_ = readParameter(fSettings, "Stereo.b", found); + bf_ = b_ * calibration1_->getParameter(0); + } + else + { + cv::Mat cvTlr = readParameter(fSettings, "Stereo.T_c1_c2", found); + Tlr_ = Converter::toSophus(cvTlr); + + // TODO: also search for Trl and invert if necessary + + b_ = Tlr_.translation().norm(); + bf_ = b_ * calibration1_->getParameter(0); + } + + thDepth_ = readParameter(fSettings, "Stereo.ThDepth", found); +} + +void Settings::readImageInfo(cv::FileStorage &fSettings) +{ + bool found; + // Read original and desired image dimensions + int originalRows = readParameter(fSettings, "Camera.height", found); + int originalCols = readParameter(fSettings, "Camera.width", found); + originalImSize_.width = originalCols; + originalImSize_.height = originalRows; + + newImSize_ = originalImSize_; + int newHeigh = readParameter(fSettings, "Camera.newHeight", found, false); + if (found) + { + bNeedToResize1_ = true; + newImSize_.height = newHeigh; + + if (!bNeedToRectify_) + { + // Update calibration + float scaleRowFactor = (float)newImSize_.height / (float)originalImSize_.height; + calibration1_->setParameter(calibration1_->getParameter(1) * scaleRowFactor, 1); + calibration1_->setParameter(calibration1_->getParameter(3) * scaleRowFactor, 3); + + if ((sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) && cameraType_ != Rectified) + { + calibration2_->setParameter(calibration2_->getParameter(1) * scaleRowFactor, 1); + calibration2_->setParameter(calibration2_->getParameter(3) * scaleRowFactor, 3); + } + } + } + + int newWidth = readParameter(fSettings, "Camera.newWidth", found, false); + if (found) + { + bNeedToResize1_ = true; + newImSize_.width = newWidth; + + if (!bNeedToRectify_) + { + // Update calibration + float scaleColFactor = (float)newImSize_.width / (float)originalImSize_.width; + calibration1_->setParameter(calibration1_->getParameter(0) * scaleColFactor, 0); + calibration1_->setParameter(calibration1_->getParameter(2) * scaleColFactor, 2); + + if ((sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) && cameraType_ != Rectified) + { + calibration2_->setParameter(calibration2_->getParameter(0) * scaleColFactor, 0); + calibration2_->setParameter(calibration2_->getParameter(2) * scaleColFactor, 2); + + if (cameraType_ == KannalaBrandt) + { + static_cast(calibration1_)->mvLappingArea[0] *= scaleColFactor; + static_cast(calibration1_)->mvLappingArea[1] *= scaleColFactor; + + static_cast(calibration2_)->mvLappingArea[0] *= scaleColFactor; + static_cast(calibration2_)->mvLappingArea[1] *= scaleColFactor; + } + } + } + } + + fps_ = readParameter(fSettings, "Camera.fps", found); + bRGB_ = (bool)readParameter(fSettings, "Camera.RGB", found); +} + +void Settings::readIMU(cv::FileStorage &fSettings) +{ + bool found; + noiseGyro_ = readParameter(fSettings, "IMU.NoiseGyro", found); + noiseAcc_ = readParameter(fSettings, "IMU.NoiseAcc", found); + gyroWalk_ = readParameter(fSettings, "IMU.GyroWalk", found); + accWalk_ = readParameter(fSettings, "IMU.AccWalk", found); + imuFrequency_ = readParameter(fSettings, "IMU.Frequency", found); + + cv::Mat cvTbc = readParameter(fSettings, "IMU.T_b_c1", found); + Tbc_ = Converter::toSophus(cvTbc); + + readParameter(fSettings, "IMU.InsertKFsWhenLost", found, false); + if (found) + { + insertKFsWhenLost_ = (bool)readParameter(fSettings, "IMU.InsertKFsWhenLost", found, false); + } + else + { + insertKFsWhenLost_ = true; + } +} + +void Settings::readRGBD(cv::FileStorage &fSettings) +{ + bool found; + + depthMapFactor_ = readParameter(fSettings, "RGBD.DepthMapFactor", found); + thDepth_ = readParameter(fSettings, "Stereo.ThDepth", found); + b_ = readParameter(fSettings, "Stereo.b", found); + bf_ = b_ * calibration1_->getParameter(0); +} + +void Settings::readORB(cv::FileStorage &fSettings) +{ + bool found; + + nFeatures_ = readParameter(fSettings, "ORBextractor.nFeatures", found); + scaleFactor_ = readParameter(fSettings, "ORBextractor.scaleFactor", found); + nLevels_ = readParameter(fSettings, "ORBextractor.nLevels", found); + initThFAST_ = readParameter(fSettings, "ORBextractor.iniThFAST", found); + minThFAST_ = readParameter(fSettings, "ORBextractor.minThFAST", found); +} + +void Settings::readViewer(cv::FileStorage &fSettings) +{ + bool found; + + keyFrameSize_ = readParameter(fSettings, "Viewer.KeyFrameSize", found); + keyFrameLineWidth_ = readParameter(fSettings, "Viewer.KeyFrameLineWidth", found); + graphLineWidth_ = readParameter(fSettings, "Viewer.GraphLineWidth", found); + pointSize_ = readParameter(fSettings, "Viewer.PointSize", found); + cameraSize_ = readParameter(fSettings, "Viewer.CameraSize", found); + cameraLineWidth_ = readParameter(fSettings, "Viewer.CameraLineWidth", found); + viewPointX_ = readParameter(fSettings, "Viewer.ViewpointX", found); + viewPointY_ = readParameter(fSettings, "Viewer.ViewpointY", found); + viewPointZ_ = readParameter(fSettings, "Viewer.ViewpointZ", found); + viewPointF_ = readParameter(fSettings, "Viewer.ViewpointF", found); + imageViewerScale_ = readParameter(fSettings, "Viewer.imageViewScale", found, false); + + if (!found) + imageViewerScale_ = 1.0f; +} + +void Settings::readLoadAndSave(cv::FileStorage &fSettings) +{ + bool found; + + sLoadFrom_ = readParameter(fSettings, "System.LoadAtlasFromFile", found, false); + sSaveto_ = readParameter(fSettings, "System.SaveAtlasToFile", found, false); +} + +void Settings::readOtherParameters(cv::FileStorage &fSettings) +{ + bool found; + + thFarPoints_ = readParameter(fSettings, "System.thFarPoints", found, false); +} + +void Settings::precomputeRectificationMaps() +{ + // Precompute rectification maps, new calibrations, ... + cv::Mat K1 = static_cast(calibration1_)->toK(); + K1.convertTo(K1, CV_64F); + cv::Mat K2 = static_cast(calibration2_)->toK(); + K2.convertTo(K2, CV_64F); + + cv::Mat cvTlr; + cv::eigen2cv(Tlr_.inverse().matrix3x4(), cvTlr); + cv::Mat R12 = cvTlr.rowRange(0, 3).colRange(0, 3); + R12.convertTo(R12, CV_64F); + cv::Mat t12 = cvTlr.rowRange(0, 3).col(3); + t12.convertTo(t12, CV_64F); + + cv::Mat R_r1_u1, R_r2_u2; + cv::Mat P1, P2, Q; + + cv::stereoRectify(K1, camera1DistortionCoef(), K2, camera2DistortionCoef(), newImSize_, + R12, t12, + R_r1_u1, R_r2_u2, P1, P2, Q, + cv::CALIB_ZERO_DISPARITY, -1, newImSize_); + cv::initUndistortRectifyMap(K1, camera1DistortionCoef(), R_r1_u1, P1.rowRange(0, 3).colRange(0, 3), + newImSize_, CV_32F, M1l_, M2l_); + cv::initUndistortRectifyMap(K2, camera2DistortionCoef(), R_r2_u2, P2.rowRange(0, 3).colRange(0, 3), + newImSize_, CV_32F, M1r_, M2r_); + + // Update calibration + calibration1_->setParameter(P1.at(0, 0), 0); + calibration1_->setParameter(P1.at(1, 1), 1); + calibration1_->setParameter(P1.at(0, 2), 2); + calibration1_->setParameter(P1.at(1, 2), 3); + + // Update bf + bf_ = b_ * P1.at(0, 0); + + // Update relative pose between camera 1 and IMU if necessary + if (sensor_ == System::IMU_STEREO) + { + Eigen::Matrix3f eigenR_r1_u1; + cv::cv2eigen(R_r1_u1, eigenR_r1_u1); + Sophus::SE3f T_r1_u1(eigenR_r1_u1, Eigen::Vector3f::Zero()); + Tbc_ = Tbc_ * T_r1_u1.inverse(); + } +} + +ostream &operator<<(std::ostream &output, const Settings &settings) +{ + output << "SLAM settings: " << endl; + + output << "\t-Camera 1 parameters ("; + if (settings.cameraType_ == Settings::PinHole || settings.cameraType_ == Settings::Rectified) + { + output << "Pinhole"; + } + else + { + output << "Kannala-Brandt"; + } + output << ")" + << ": ["; + for (size_t i = 0; i < settings.originalCalib1_->size(); i++) + { + output << " " << settings.originalCalib1_->getParameter(i); + } + output << " ]" << endl; + + if (!settings.vPinHoleDistorsion1_.empty()) + { + output << "\t-Camera 1 distortion parameters: [ "; + for (float d : settings.vPinHoleDistorsion1_) + { + output << " " << d; + } + output << " ]" << endl; + } + + if (settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO) + { + if (settings.cameraType_ != Settings::Rectified) + { + output << "\t-Camera 2 parameters ("; + if (settings.cameraType_ == Settings::PinHole) + { + output << "Pinhole"; + } + else + { + output << "Kannala-Brandt"; + } + output << "" + << ": ["; + for (size_t i = 0; i < settings.originalCalib2_->size(); i++) + { + output << " " << settings.originalCalib2_->getParameter(i); + } + output << " ]" << endl; + + if (!settings.vPinHoleDistorsion2_.empty()) + { + output << "\t-Camera 1 distortion parameters: [ "; + for (float d : settings.vPinHoleDistorsion2_) + { + output << " " << d; + } + output << " ]" << endl; + } + } + } + + output << "\t-Original image size: [ " << settings.originalImSize_.width << " , " << settings.originalImSize_.height << " ]" << endl; + output << "\t-Current image size: [ " << settings.newImSize_.width << " , " << settings.newImSize_.height << " ]" << endl; + + if (settings.bNeedToRectify_) + { + output << "\t-Camera 1 parameters after rectification: [ "; + for (size_t i = 0; i < settings.calibration1_->size(); i++) + { + output << " " << settings.calibration1_->getParameter(i); + } + output << " ]" << endl; + } + else if (settings.bNeedToResize1_) + { + output << "\t-Camera 1 parameters after resize: [ "; + for (size_t i = 0; i < settings.calibration1_->size(); i++) + { + output << " " << settings.calibration1_->getParameter(i); + } + output << " ]" << endl; + + if ((settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO) && + settings.cameraType_ == Settings::KannalaBrandt) + { + output << "\t-Camera 2 parameters after resize: [ "; + for (size_t i = 0; i < settings.calibration2_->size(); i++) + { + output << " " << settings.calibration2_->getParameter(i); + } + output << " ]" << endl; + } + } + + output << "\t-Sequence FPS: " << settings.fps_ << endl; + + // Stereo stuff + if (settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO) + { + output << "\t-Stereo baseline: " << settings.b_ << endl; + output << "\t-Stereo depth threshold : " << settings.thDepth_ << endl; + + if (settings.cameraType_ == Settings::KannalaBrandt) + { + auto vOverlapping1 = static_cast(settings.calibration1_)->mvLappingArea; + auto vOverlapping2 = static_cast(settings.calibration2_)->mvLappingArea; + output << "\t-Camera 1 overlapping area: [ " << vOverlapping1[0] << " , " << vOverlapping1[1] << " ]" << endl; + output << "\t-Camera 2 overlapping area: [ " << vOverlapping2[0] << " , " << vOverlapping2[1] << " ]" << endl; + } + } + + if (settings.sensor_ == System::IMU_MONOCULAR || settings.sensor_ == System::IMU_STEREO || settings.sensor_ == System::IMU_RGBD) + { + output << "\t-Gyro noise: " << settings.noiseGyro_ << endl; + output << "\t-Accelerometer noise: " << settings.noiseAcc_ << endl; + output << "\t-Gyro walk: " << settings.gyroWalk_ << endl; + output << "\t-Accelerometer walk: " << settings.accWalk_ << endl; + output << "\t-IMU frequency: " << settings.imuFrequency_ << endl; + } + + if (settings.sensor_ == System::RGBD || settings.sensor_ == System::IMU_RGBD) + { + output << "\t-RGB-D depth map factor: " << settings.depthMapFactor_ << endl; + } + + output << "\t-Features per image: " << settings.nFeatures_ << endl; + output << "\t-ORB scale factor: " << settings.scaleFactor_ << endl; + output << "\t-ORB number of scales: " << settings.nLevels_ << endl; + output << "\t-Initial FAST threshold: " << settings.initThFAST_ << endl; + output << "\t-Min FAST threshold: " << settings.minThFAST_ << endl; + + return output; +} +}; diff --git a/ORB-SLAM3-UESTC/Workspace/src/Sim3Solver.cc b/ORB-SLAM3-UESTC/Workspace/src/Sim3Solver.cc new file mode 100644 index 0000000..b8d3abf --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/Sim3Solver.cc @@ -0,0 +1,489 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#include "Sim3Solver.h" + +#include +#include +#include + +#include "KeyFrame.h" +#include "ORBmatcher.h" + +#include "Thirdparty/DBoW2/DUtils/Random.h" + +namespace ORB_SLAM3 +{ + + +Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector &vpMatched12, const bool bFixScale, + vector vpKeyFrameMatchedMP): + mnIterations(0), mnBestInliers(0), mbFixScale(bFixScale), + pCamera1(pKF1->mpCamera), pCamera2(pKF2->mpCamera) +{ + bool bDifferentKFs = false; + if(vpKeyFrameMatchedMP.empty()) + { + bDifferentKFs = true; + vpKeyFrameMatchedMP = vector(vpMatched12.size(), pKF2); + } + + mpKF1 = pKF1; + mpKF2 = pKF2; + + vector vpKeyFrameMP1 = pKF1->GetMapPointMatches(); + + mN1 = vpMatched12.size(); + + mvpMapPoints1.reserve(mN1); + mvpMapPoints2.reserve(mN1); + mvpMatches12 = vpMatched12; + mvnIndices1.reserve(mN1); + mvX3Dc1.reserve(mN1); + mvX3Dc2.reserve(mN1); + + Eigen::Matrix3f Rcw1 = pKF1->GetRotation(); + Eigen::Vector3f tcw1 = pKF1->GetTranslation(); + Eigen::Matrix3f Rcw2 = pKF2->GetRotation(); + Eigen::Vector3f tcw2 = pKF2->GetTranslation(); + + mvAllIndices.reserve(mN1); + + size_t idx=0; + + KeyFrame* pKFm = pKF2; //Default variable + for(int i1=0; i1isBad() || pMP2->isBad()) + continue; + + if(bDifferentKFs) + pKFm = vpKeyFrameMatchedMP[i1]; + + int indexKF1 = get<0>(pMP1->GetIndexInKeyFrame(pKF1)); + int indexKF2 = get<0>(pMP2->GetIndexInKeyFrame(pKFm)); + + if(indexKF1<0 || indexKF2<0) + continue; + + const cv::KeyPoint &kp1 = pKF1->mvKeysUn[indexKF1]; + const cv::KeyPoint &kp2 = pKFm->mvKeysUn[indexKF2]; + + const float sigmaSquare1 = pKF1->mvLevelSigma2[kp1.octave]; + const float sigmaSquare2 = pKFm->mvLevelSigma2[kp2.octave]; + + mvnMaxError1.push_back(9.210*sigmaSquare1); + mvnMaxError2.push_back(9.210*sigmaSquare2); + + mvpMapPoints1.push_back(pMP1); + mvpMapPoints2.push_back(pMP2); + mvnIndices1.push_back(i1); + + Eigen::Vector3f X3D1w = pMP1->GetWorldPos(); + mvX3Dc1.push_back(Rcw1*X3D1w+tcw1); + + Eigen::Vector3f X3D2w = pMP2->GetWorldPos(); + mvX3Dc2.push_back(Rcw2*X3D2w+tcw2); + + mvAllIndices.push_back(idx); + idx++; + } + } + + FromCameraToImage(mvX3Dc1,mvP1im1,pCamera1); + FromCameraToImage(mvX3Dc2,mvP2im2,pCamera2); + + SetRansacParameters(); +} + +void Sim3Solver::SetRansacParameters(double probability, int minInliers, int maxIterations) +{ + mRansacProb = probability; + mRansacMinInliers = minInliers; + mRansacMaxIts = maxIterations; + + N = mvpMapPoints1.size(); // number of correspondences + + mvbInliersi.resize(N); + + // Adjust Parameters according to number of correspondences + float epsilon = (float)mRansacMinInliers/N; + + // Set RANSAC iterations according to probability, epsilon, and max iterations + int nIterations; + + if(mRansacMinInliers==N) + nIterations=1; + else + nIterations = ceil(log(1-mRansacProb)/log(1-pow(epsilon,3))); + + mRansacMaxIts = max(1,min(nIterations,mRansacMaxIts)); + + mnIterations = 0; +} + +Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers) +{ + bNoMore = false; + vbInliers = vector(mN1,false); + nInliers=0; + + if(N vAvailableIndices; + + Eigen::Matrix3f P3Dc1i; + Eigen::Matrix3f P3Dc2i; + + int nCurrentIterations = 0; + while(mnIterations=mnBestInliers) + { + mvbBestInliers = mvbInliersi; + mnBestInliers = mnInliersi; + mBestT12 = mT12i; + mBestRotation = mR12i; + mBestTranslation = mt12i; + mBestScale = ms12i; + + if(mnInliersi>mRansacMinInliers) + { + nInliers = mnInliersi; + for(int i=0; i=mRansacMaxIts) + bNoMore=true; + + return Eigen::Matrix4f::Identity(); +} + +Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers, bool &bConverge) +{ + bNoMore = false; + bConverge = false; + vbInliers = vector(mN1,false); + nInliers=0; + + if(N vAvailableIndices; + + Eigen::Matrix3f P3Dc1i; + Eigen::Matrix3f P3Dc2i; + + int nCurrentIterations = 0; + + Eigen::Matrix4f bestSim3; + + while(mnIterations=mnBestInliers) + { + mvbBestInliers = mvbInliersi; + mnBestInliers = mnInliersi; + mBestT12 = mT12i; + mBestRotation = mR12i; + mBestTranslation = mt12i; + mBestScale = ms12i; + + if(mnInliersi>mRansacMinInliers) + { + nInliers = mnInliersi; + for(int i=0; i=mRansacMaxIts) + bNoMore=true; + + return bestSim3; +} + +Eigen::Matrix4f Sim3Solver::find(vector &vbInliers12, int &nInliers) +{ + bool bFlag; + return iterate(mRansacMaxIts,bFlag,vbInliers12,nInliers); +} + +void Sim3Solver::ComputeCentroid(Eigen::Matrix3f &P, Eigen::Matrix3f &Pr, Eigen::Vector3f &C) +{ + C = P.rowwise().sum(); + C = C / P.cols(); + for(int i=0; i eigSolver; + eigSolver.compute(N); + + Eigen::Vector4f eval = eigSolver.eigenvalues().real(); + Eigen::Matrix4f evec = eigSolver.eigenvectors().real(); //evec[0] is the quaternion of the desired rotation + + int maxIndex; // should be zero + eval.maxCoeff(&maxIndex); + + Eigen::Vector3f vec = evec.block<3,1>(1,maxIndex); //extract imaginary part of the quaternion (sin*axis) + + // Rotation angle. sin is the norm of the imaginary part, cos is the real part + double ang=atan2(vec.norm(),evec(0,maxIndex)); + + vec = 2*ang*vec/vec.norm(); //Angle-axis representation. quaternion angle is the half + mR12i = Sophus::SO3f::exp(vec).matrix(); + + // Step 5: Rotate set 2 + Eigen::Matrix3f P3 = mR12i*Pr2; + + // Step 6: Scale + + if(!mbFixScale) + { + double cvnom = Converter::toCvMat(Pr1).dot(Converter::toCvMat(P3)); + double nom = (Pr1.array() * P3.array()).sum(); + if (abs(nom-cvnom)>1e-3) + std::cout << "sim3 solver: " << abs(nom-cvnom) << std::endl << nom << std::endl; + Eigen::Array aux_P3; + aux_P3 = P3.array() * P3.array(); + double den = aux_P3.sum(); + + ms12i = nom/den; + } + else + ms12i = 1.0f; + + // Step 7: Translation + mt12i = O1 - ms12i * mR12i * O2; + + // Step 8: Transformation + + // Step 8.1 T12 + mT12i.setIdentity(); + + Eigen::Matrix3f sR = ms12i*mR12i; + mT12i.block<3,3>(0,0) = sR; + mT12i.block<3,1>(0,3) = mt12i; + + + // Step 8.2 T21 + mT21i.setIdentity(); + Eigen::Matrix3f sRinv = (1.0/ms12i)*mR12i.transpose(); + + // sRinv.copyTo(mT21i.rowRange(0,3).colRange(0,3)); + mT21i.block<3,3>(0,0) = sRinv; + + Eigen::Vector3f tinv = -sRinv * mt12i; + mT21i.block<3,1>(0,3) = tinv; +} + + +void Sim3Solver::CheckInliers() +{ + vector vP1im2, vP2im1; + Project(mvX3Dc2,vP2im1,mT12i,pCamera1); + Project(mvX3Dc1,vP1im2,mT21i,pCamera2); + + mnInliersi=0; + + for(size_t i=0; i &vP3Dw, vector &vP2D, Eigen::Matrix4f Tcw, GeometricCamera* pCamera) +{ + Eigen::Matrix3f Rcw = Tcw.block<3,3>(0,0); + Eigen::Vector3f tcw = Tcw.block<3,1>(0,3); + + vP2D.clear(); + vP2D.reserve(vP3Dw.size()); + + for(size_t i=0, iend=vP3Dw.size(); iproject(P3Dc); + vP2D.push_back(pt2D); + } +} + +void Sim3Solver::FromCameraToImage(const vector &vP3Dc, vector &vP2D, GeometricCamera* pCamera) +{ + vP2D.clear(); + vP2D.reserve(vP3Dc.size()); + + for(size_t i=0, iend=vP3Dc.size(); iproject(vP3Dc[i]); + vP2D.push_back(pt2D); + } +} + +} //namespace ORB_SLAM diff --git a/ORB-SLAM3-UESTC/Workspace/src/System.cc b/ORB-SLAM3-UESTC/Workspace/src/System.cc new file mode 100644 index 0000000..1569f8d --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/System.cc @@ -0,0 +1,1626 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + + +#include "System.h" +#include "Converter.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ORB_SLAM3 +{ + +Verbose::eLevel Verbose::th = Verbose::VERBOSITY_NORMAL; + +/** + * @brief 系统的构造函数，将会启动其他的线程 + * @param strVocFile 词袋文件所在路径 + * @param strSettingsFile 配置文件所在路径 + * @param sensor 传感器类型 + * @param bUseViewer 是否使用可视化界面 + * @param initFr initFr表示初始化帧的id,开始设置为0 + * @param strSequence 序列名,在跟踪线程和局部建图线程用得到 + */ +System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, + const bool bUseViewer, const int initFr, const string &strSequence): + mSensor(sensor), mpViewer(static_cast(NULL)), mbReset(false), mbResetActiveMap(false), + mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false), mbShutDown(false) +{ + // Output welcome message + cout << endl << + "ORB-SLAM3 Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza." << endl << + "ORB-SLAM2 Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza." << endl << + "This program comes with ABSOLUTELY NO WARRANTY;" << endl << + "This is free software, and you are welcome to redistribute it" << endl << + "under certain conditions. See LICENSE.txt." << endl << endl; + + cout << "Input sensor was set to: "; + + if(mSensor==MONOCULAR) + cout << "Monocular" << endl; // 单目 + else if(mSensor==STEREO) + cout << "Stereo" << endl; // 双目 + else if(mSensor==RGBD) + cout << "RGB-D" << endl; // RGBD相机 + else if(mSensor==IMU_MONOCULAR) + cout << "Monocular-Inertial" << endl; // 单目 + imu + else if(mSensor==IMU_STEREO) + cout << "Stereo-Inertial" << endl; // 双目 + imu + else if(mSensor==IMU_RGBD) + cout << "RGB-D-Inertial" << endl; // RGBD相机 + imu + + //Check settings file + // Step 2 读取配置文件 + cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ); + // 如果打开失败，就输出错误信息 + if(!fsSettings.isOpened()) + { + cerr << "Failed to open settings file at: " << strSettingsFile << endl; + exit(-1); + } + + // 查看配置文件版本，不同版本有不同处理方法 + cv::FileNode node = fsSettings["File.version"]; + if(!node.empty() && node.isString() && node.string() == "1.0") + { + settings_ = new Settings(strSettingsFile,mSensor); + + // 保存及加载地图的名字 + mStrLoadAtlasFromFile = settings_->atlasLoadFile(); + mStrSaveAtlasToFile = settings_->atlasSaveFile(); + + cout << (*settings_) << endl; + } + else + { + settings_ = nullptr; + cv::FileNode node = fsSettings["System.LoadAtlasFromFile"]; + if(!node.empty() && node.isString()) + { + mStrLoadAtlasFromFile = (string)node; + } + + node = fsSettings["System.SaveAtlasToFile"]; + if(!node.empty() && node.isString()) + { + mStrSaveAtlasToFile = (string)node; + } + } + + // 是否激活回环，默认是开着的 + node = fsSettings["loopClosing"]; + bool activeLC = true; + if(!node.empty()) + { + activeLC = static_cast(fsSettings["loopClosing"]) != 0; + } + + mStrVocabularyFilePath = strVocFile; + + // ORBSLAM3新加的多地图管理功能，这里加载Atlas标识符 + bool loadedAtlas = false; + + if(mStrLoadAtlasFromFile.empty()) + { + //Load ORB Vocabulary + cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl; + + // 建立一个新的ORB字典 + mpVocabulary = new ORBVocabulary(); + // 读取预训练好的ORB字典并返回成功/失败标志 + bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); + // 如果加载失败，就输出错误信息 + if(!bVocLoad) + { + cerr << "Wrong path to vocabulary. " << endl; + cerr << "Falied to open at: " << strVocFile << endl; + exit(-1); + } + cout << "Vocabulary loaded!" << endl << endl; + + //Create KeyFrame Database + // Step 4 创建关键帧数据库 + mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary); + + //Create the Atlas + // Step 5 创建多地图，参数0表示初始化关键帧id为0 + cout << "Initialization of Atlas from scratch " << endl; + mpAtlas = new Atlas(0); + } + else + { + //Load ORB Vocabulary + cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl; + + mpVocabulary = new ORBVocabulary(); + bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); + if(!bVocLoad) + { + cerr << "Wrong path to vocabulary. " << endl; + cerr << "Falied to open at: " << strVocFile << endl; + exit(-1); + } + cout << "Vocabulary loaded!" << endl << endl; + + //Create KeyFrame Database + mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary); + + cout << "Load File" << endl; + + // Load the file with an earlier session + //clock_t start = clock(); + cout << "Initialization of Atlas from file: " << mStrLoadAtlasFromFile << endl; + bool isRead = LoadAtlas(FileType::BINARY_FILE); + + if(!isRead) + { + cout << "Error to load the file, please try with other session file or vocabulary file" << endl; + exit(-1); + } + //mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary); + + + //cout << "KF in DB: " << mpKeyFrameDatabase->mnNumKFs << "; words: " << mpKeyFrameDatabase->mnNumWords << endl; + + loadedAtlas = true; + + mpAtlas->CreateNewMap(); + + //clock_t timeElapsed = clock() - start; + //unsigned msElapsed = timeElapsed / (CLOCKS_PER_SEC / 1000); + //cout << "Binary file read in " << msElapsed << " ms" << endl; + + //usleep(10*1000*1000); + } + + // 如果是有imu的传感器类型，设置mbIsInertial = true;以后的跟踪和预积分将和这个标志有关 + if (mSensor==IMU_STEREO || mSensor==IMU_MONOCULAR || mSensor==IMU_RGBD) + mpAtlas->SetInertialSensor(); + + // Step 6 依次创建跟踪、局部建图、闭环、显示线程 + //Create Drawers. These are used by the Viewer + // 创建用于显示帧和地图的类，由Viewer调用 + mpFrameDrawer = new FrameDrawer(mpAtlas); + mpMapDrawer = new MapDrawer(mpAtlas, strSettingsFile, settings_); + + //Initialize the Tracking thread + //(it will live in the main thread of execution, the one that called this constructor) + // 创建跟踪线程（主线程）,不会立刻开启,会在对图像和imu预处理后在main主线程种执行 + cout << "Seq. Name: " << strSequence << endl; + mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, + mpAtlas, mpKeyFrameDatabase, strSettingsFile, mSensor, settings_, strSequence); + + //Initialize the Local Mapping thread and launch + //创建并开启local mapping线程 + mpLocalMapper = new LocalMapping(this, mpAtlas, mSensor==MONOCULAR || mSensor==IMU_MONOCULAR, + mSensor==IMU_MONOCULAR || mSensor==IMU_STEREO || mSensor==IMU_RGBD, strSequence); + mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run,mpLocalMapper); + mpLocalMapper->mInitFr = initFr; + + // 设置最远3D地图点的深度值，如果超过阈值，说明可能三角化不太准确，丢弃 + if(settings_) + mpLocalMapper->mThFarPoints = settings_->thFarPoints(); + else + mpLocalMapper->mThFarPoints = fsSettings["thFarPoints"]; + // ? 这里有个疑问,C++中浮点型跟0比较是否用精确? + if(mpLocalMapper->mThFarPoints!=0) + { + cout << "Discard points further than " << mpLocalMapper->mThFarPoints << " m from current camera" << endl; + mpLocalMapper->mbFarPoints = true; + } + else + mpLocalMapper->mbFarPoints = false; + + //Initialize the Loop Closing thread and launch + // mSensor!=MONOCULAR && mSensor!=IMU_MONOCULAR + // 创建并开启闭环线程 + mpLoopCloser = new LoopClosing(mpAtlas, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR, activeLC); // mSensor!=MONOCULAR); + mptLoopClosing = new thread(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser); + + //Set pointers between threads + // 设置线程间的指针 + mpTracker->SetLocalMapper(mpLocalMapper); + mpTracker->SetLoopClosing(mpLoopCloser); + + mpLocalMapper->SetTracker(mpTracker); + mpLocalMapper->SetLoopCloser(mpLoopCloser); + + mpLoopCloser->SetTracker(mpTracker); + mpLoopCloser->SetLocalMapper(mpLocalMapper); + + //usleep(10*1000*1000); + + //Initialize the Viewer thread and launch + // 创建并开启显示线程 + if(bUseViewer) + //if(false) // TODO + { + mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile,settings_); + mptViewer = new thread(&Viewer::Run, mpViewer); + mpTracker->SetViewer(mpViewer); + mpLoopCloser->mpViewer = mpViewer; + mpViewer->both = mpFrameDrawer->both; + } + + // Fix verbosity + // 打印输出中间的信息，设置为安静模式 + Verbose::SetTh(Verbose::VERBOSITY_QUIET); + +} + +Sophus::SE3f System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp, const vector& vImuMeas, string filename) +{ + if(mSensor!=STEREO && mSensor!=IMU_STEREO) + { + cerr << "ERROR: you called TrackStereo but input sensor was not set to Stereo nor Stereo-Inertial." << endl; + exit(-1); + } + + cv::Mat imLeftToFeed, imRightToFeed; + if(settings_ && settings_->needToRectify()){ + cv::Mat M1l = settings_->M1l(); + cv::Mat M2l = settings_->M2l(); + cv::Mat M1r = settings_->M1r(); + cv::Mat M2r = settings_->M2r(); + + cv::remap(imLeft, imLeftToFeed, M1l, M2l, cv::INTER_LINEAR); + cv::remap(imRight, imRightToFeed, M1r, M2r, cv::INTER_LINEAR); + } + else if(settings_ && settings_->needToResize()){ + cv::resize(imLeft,imLeftToFeed,settings_->newImSize()); + cv::resize(imRight,imRightToFeed,settings_->newImSize()); + } + else{ + imLeftToFeed = imLeft.clone(); + imRightToFeed = imRight.clone(); + } + + // Check mode change + { + unique_lock lock(mMutexMode); + if(mbActivateLocalizationMode) + { + mpLocalMapper->RequestStop(); + + // Wait until Local Mapping has effectively stopped + while(!mpLocalMapper->isStopped()) + { + usleep(1000); + } + + mpTracker->InformOnlyTracking(true); + mbActivateLocalizationMode = false; + } + if(mbDeactivateLocalizationMode) + { + mpTracker->InformOnlyTracking(false); + mpLocalMapper->Release(); + mbDeactivateLocalizationMode = false; + } + } + + // Check reset + { + unique_lock lock(mMutexReset); + if(mbReset) + { + mpTracker->Reset(); + mbReset = false; + mbResetActiveMap = false; + } + else if(mbResetActiveMap) + { + mpTracker->ResetActiveMap(); + mbResetActiveMap = false; + } + } + + if (mSensor == System::IMU_STEREO) + for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++) + mpTracker->GrabImuData(vImuMeas[i_imu]); + + // std::cout << "start GrabImageStereo" << std::endl; + Sophus::SE3f Tcw = mpTracker->GrabImageStereo(imLeftToFeed,imRightToFeed,timestamp,filename); + + // std::cout << "out grabber" << std::endl; + + unique_lock lock2(mMutexState); + mTrackingState = mpTracker->mState; + mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; + mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn; + + return Tcw; +} + +Sophus::SE3f System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp, const vector& vImuMeas, string filename) +{ + if(mSensor!=RGBD && mSensor!=IMU_RGBD) + { + cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD." << endl; + exit(-1); + } + + cv::Mat imToFeed = im.clone(); + cv::Mat imDepthToFeed = depthmap.clone(); + if(settings_ && settings_->needToResize()){ + cv::Mat resizedIm; + cv::resize(im,resizedIm,settings_->newImSize()); + imToFeed = resizedIm; + + cv::resize(depthmap,imDepthToFeed,settings_->newImSize()); + } + + // Check mode change + { + unique_lock lock(mMutexMode); + if(mbActivateLocalizationMode) + { + mpLocalMapper->RequestStop(); + + // Wait until Local Mapping has effectively stopped + while(!mpLocalMapper->isStopped()) + { + usleep(1000); + } + + mpTracker->InformOnlyTracking(true); + mbActivateLocalizationMode = false; + } + if(mbDeactivateLocalizationMode) + { + mpTracker->InformOnlyTracking(false); + mpLocalMapper->Release(); + mbDeactivateLocalizationMode = false; + } + } + + // Check reset + { + unique_lock lock(mMutexReset); + if(mbReset) + { + mpTracker->Reset(); + mbReset = false; + mbResetActiveMap = false; + } + else if(mbResetActiveMap) + { + mpTracker->ResetActiveMap(); + mbResetActiveMap = false; + } + } + + if (mSensor == System::IMU_RGBD) + for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++) + mpTracker->GrabImuData(vImuMeas[i_imu]); + + Sophus::SE3f Tcw = mpTracker->GrabImageRGBD(imToFeed,imDepthToFeed,timestamp,filename); + + unique_lock lock2(mMutexState); + mTrackingState = mpTracker->mState; + mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; + mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn; + return Tcw; +} + +/** + * @brief 单目/单目VIO跟踪 + * + * @param[in] im 灰度图像 + * @param[in] timestamp 图像时间戳 + * @param[in] vImuMeas 上一帧到当前帧图像之间的IMU测量值 + * @param[in] filename 调试用的文件名 + * @return Sophus::SE3f 当前帧位姿Tcw + */ +Sophus::SE3f System::TrackMonocular(const cv::Mat &im, const double ×tamp, const vector& vImuMeas, string filename) +{ + + { + unique_lock lock(mMutexReset); + if(mbShutDown) + return Sophus::SE3f(); + } + // 确保是单目或单目VIO模式 + if(mSensor!=MONOCULAR && mSensor!=IMU_MONOCULAR) + { + cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular nor Monocular-Inertial." << endl; + exit(-1); + } + + cv::Mat imToFeed = im.clone(); + if(settings_ && settings_->needToResize()){ + cv::Mat resizedIm; + cv::resize(im,resizedIm,settings_->newImSize()); + imToFeed = resizedIm; + } + + // Check mode change + { + // 独占锁，主要是为了mbActivateLocalizationMode和mbDeactivateLocalizationMode不会发生混乱 + unique_lock lock(mMutexMode); + // mbActivateLocalizationMode为true会关闭局部地图线程，仅跟踪模式 + if(mbActivateLocalizationMode) + { + mpLocalMapper->RequestStop(); + + // Wait until Local Mapping has effectively stopped + while(!mpLocalMapper->isStopped()) + { + usleep(1000); + } + // 局部地图关闭以后，只进行追踪的线程，只计算相机的位姿，没有对局部地图进行更新 + mpTracker->InformOnlyTracking(true); + // 关闭线程可以使得别的线程得到更多的资源 + mbActivateLocalizationMode = false; + } + if(mbDeactivateLocalizationMode) + { + mpTracker->InformOnlyTracking(false); + mpLocalMapper->Release(); + mbDeactivateLocalizationMode = false; + } + } + + // Check reset + { + unique_lock lock(mMutexReset); + if(mbReset) + { + mpTracker->Reset(); + mbReset = false; + mbResetActiveMap = false; + } + // 如果检测到重置活动地图的标志为true,将重置地图 + else if(mbResetActiveMap) + { + cout << "SYSTEM-> Reseting active map in monocular case" << endl; + mpTracker->ResetActiveMap(); + mbResetActiveMap = false; + } + } + // 如果是单目VIO模式，把IMU数据存储到队列mlQueueImuData + if (mSensor == System::IMU_MONOCULAR) + for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++) + mpTracker->GrabImuData(vImuMeas[i_imu]); + + // 计算相机位姿 + Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed,timestamp,filename); + + // 更新跟踪状态和参数 + unique_lock lock2(mMutexState); + mTrackingState = mpTracker->mState; + mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; + mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn; + + return Tcw; +} + + + +void System::ActivateLocalizationMode() +{ + unique_lock lock(mMutexMode); + mbActivateLocalizationMode = true; +} + +void System::DeactivateLocalizationMode() +{ + unique_lock lock(mMutexMode); + mbDeactivateLocalizationMode = true; +} + +bool System::MapChanged() +{ + static int n=0; + int curn = mpAtlas->GetLastBigChangeIdx(); + if(n lock(mMutexReset); + mbReset = true; +} + +void System::ResetActiveMap() +{ + unique_lock lock(mMutexReset); + mbResetActiveMap = true; +} + +void System::Shutdown() +{ + { + unique_lock lock(mMutexReset); + mbShutDown = true; + } + + cout << "Shutdown" << endl; + + if (mpViewer) + { + mpViewer->RequestFinish(); + while (!mpViewer->isFinished()) + usleep(5000); + } + + mpLocalMapper->RequestFinish(); + mpLoopCloser->RequestFinish(); + /*if(mpViewer) + { + mpViewer->RequestFinish(); + while(!mpViewer->isFinished()) + usleep(5000); + }*/ + + // Wait until all thread have effectively stopped + // 源代码这里注释掉了，但是不执行会有锁报错 + while(!mpLocalMapper->isFinished() || !mpLoopCloser->isFinished() || mpLoopCloser->isRunningGBA()) + { + // if(!mpLocalMapper->isFinished()) + // cout << "mpLocalMapper is not finished" << endl; + // if(!mpLoopCloser->isFinished()) + // cout << "mpLoopCloser is not finished" << endl; + // if(mpLoopCloser->isRunningGBA()){ + // cout << "mpLoopCloser is running GBA" << endl; + // cout << "break anyway..." << endl; + // break; + // } + usleep(5000); + } + + + if(!mStrSaveAtlasToFile.empty()) + { + std::cout << "开始保存地图" << std::endl; + Verbose::PrintMess("Atlas saving to file " + mStrSaveAtlasToFile, Verbose::VERBOSITY_NORMAL); + SaveAtlas(FileType::BINARY_FILE); + } + + /*if(mpViewer) + pangolin::BindToContext("ORB-SLAM2: Map Viewer");*/ + +#ifdef REGISTER_TIMES + mpTracker->PrintTimeStats(); +#endif + + +} + +bool System::isShutDown() +{ + unique_lock lock(mMutexReset); + return mbShutDown; +} + +void System::SaveTrajectoryTUM(const string &filename) +{ + cout << endl << "Saving camera trajectory to " << filename << " ..." << endl; + if(mSensor==MONOCULAR) + { + cerr << "ERROR: SaveTrajectoryTUM cannot be used for monocular." << endl; + return; + } + + vector vpKFs = mpAtlas->GetAllKeyFrames(); + sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + Sophus::SE3f Two = vpKFs[0]->GetPoseInverse(); + + ofstream f; + f.open(filename.c_str()); + f << fixed; + + // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph). + // We need to get first the keyframe pose and then concatenate the relative transformation. + // Frames not localized (tracking failure) are not saved. + + // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag + // which is true when tracking failed (lbL). + list::iterator lRit = mpTracker->mlpReferences.begin(); + list::iterator lT = mpTracker->mlFrameTimes.begin(); + list::iterator lbL = mpTracker->mlbLost.begin(); + for(list::iterator lit=mpTracker->mlRelativeFramePoses.begin(), + lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++) + { + if(*lbL) + continue; + + KeyFrame* pKF = *lRit; + + Sophus::SE3f Trw; + + // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe. + while(pKF->isBad()) + { + Trw = Trw * pKF->mTcp; + pKF = pKF->GetParent(); + } + + Trw = Trw * pKF->GetPose() * Two; + + Sophus::SE3f Tcw = (*lit) * Trw; + Sophus::SE3f Twc = Tcw.inverse(); + + Eigen::Vector3f twc = Twc.translation(); + Eigen::Quaternionf q = Twc.unit_quaternion(); + + f << setprecision(6) << *lT << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + } + f.close(); + // cout << endl << "trajectory saved!" << endl; +} + +void System::SaveKeyFrameTrajectoryTUM(const string &filename) +{ + cout << endl << "Saving keyframe trajectory to " << filename << " ..." << endl; + + vector vpKFs = mpAtlas->GetAllKeyFrames(); + sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + ofstream f; + f.open(filename.c_str()); + f << fixed; + + for(size_t i=0; iSetPose(pKF->GetPose()*Two); + + if(pKF->isBad()) + continue; + + Sophus::SE3f Twc = pKF->GetPoseInverse(); + Eigen::Quaternionf q = Twc.unit_quaternion(); + Eigen::Vector3f t = Twc.translation(); + f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << " " << t(0) << " " << t(1) << " " << t(2) + << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + + } + + f.close(); +} + +void System::SaveTrajectoryEuRoC(const string &filename) +{ + + cout << endl << "Saving trajectory to " << filename << " ..." << endl; + /*if(mSensor==MONOCULAR) + { + cerr << "ERROR: SaveTrajectoryEuRoC cannot be used for monocular." << endl; + return; + }*/ + + vector vpMaps = mpAtlas->GetAllMaps(); + int numMaxKFs = 0; + Map* pBiggerMap; + std::cout << "There are " << std::to_string(vpMaps.size()) << " maps in the atlas" << std::endl; + for(Map* pMap :vpMaps) + { + std::cout << " Map " << std::to_string(pMap->GetId()) << " has " << std::to_string(pMap->GetAllKeyFrames().size()) << " KFs" << std::endl; + if(pMap->GetAllKeyFrames().size() > numMaxKFs) + { + numMaxKFs = pMap->GetAllKeyFrames().size(); + pBiggerMap = pMap; + } + } + + vector vpKFs = pBiggerMap->GetAllKeyFrames(); + sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + Sophus::SE3f Twb; // Can be word to cam0 or world to b depending on IMU or not. + if (mSensor==IMU_MONOCULAR || mSensor==IMU_STEREO || mSensor==IMU_RGBD) + Twb = vpKFs[0]->GetImuPose(); + else + Twb = vpKFs[0]->GetPoseInverse(); + + ofstream f; + f.open(filename.c_str()); + // cout << "file open" << endl; + f << fixed; + + // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph). + // We need to get first the keyframe pose and then concatenate the relative transformation. + // Frames not localized (tracking failure) are not saved. + + // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag + // which is true when tracking failed (lbL). + list::iterator lRit = mpTracker->mlpReferences.begin(); + list::iterator lT = mpTracker->mlFrameTimes.begin(); + list::iterator lbL = mpTracker->mlbLost.begin(); + + //cout << "size mlpReferences: " << mpTracker->mlpReferences.size() << endl; + //cout << "size mlRelativeFramePoses: " << mpTracker->mlRelativeFramePoses.size() << endl; + //cout << "size mpTracker->mlFrameTimes: " << mpTracker->mlFrameTimes.size() << endl; + //cout << "size mpTracker->mlbLost: " << mpTracker->mlbLost.size() << endl; + + + for(auto lit=mpTracker->mlRelativeFramePoses.begin(), + lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++) + { + //cout << "1" << endl; + if(*lbL) + continue; + + + KeyFrame* pKF = *lRit; + //cout << "KF: " << pKF->mnId << endl; + + Sophus::SE3f Trw; + + // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe. + if (!pKF) + continue; + + //cout << "2.5" << endl; + + while(pKF->isBad()) + { + //cout << " 2.bad" << endl; + Trw = Trw * pKF->mTcp; + pKF = pKF->GetParent(); + //cout << "--Parent KF: " << pKF->mnId << endl; + } + + if(!pKF || pKF->GetMap() != pBiggerMap) + { + //cout << "--Parent KF is from another map" << endl; + continue; + } + + //cout << "3" << endl; + + Trw = Trw * pKF->GetPose()*Twb; // Tcp*Tpw*Twb0=Tcb0 where b0 is the new world reference + + // cout << "4" << endl; + + if (mSensor == IMU_MONOCULAR || mSensor == IMU_STEREO || mSensor==IMU_RGBD) + { + Sophus::SE3f Twb = (pKF->mImuCalib.mTbc * (*lit) * Trw).inverse(); + Eigen::Quaternionf q = Twb.unit_quaternion(); + Eigen::Vector3f twb = Twb.translation(); + f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + } + else + { + Sophus::SE3f Twc = ((*lit)*Trw).inverse(); + Eigen::Quaternionf q = Twc.unit_quaternion(); + Eigen::Vector3f twc = Twc.translation(); + f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + } + + // cout << "5" << endl; + } + //cout << "end saving trajectory" << endl; + f.close(); + cout << endl << "End of saving trajectory to " << filename << " ..." << endl; +} + +void System::SaveTrajectoryEuRoC(const string &filename, Map* pMap) +{ + + cout << endl << "Saving trajectory of map " << pMap->GetId() << " to " << filename << " ..." << endl; + /*if(mSensor==MONOCULAR) + { + cerr << "ERROR: SaveTrajectoryEuRoC cannot be used for monocular." << endl; + return; + }*/ + + int numMaxKFs = 0; + + vector vpKFs = pMap->GetAllKeyFrames(); + sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + Sophus::SE3f Twb; // Can be word to cam0 or world to b dependingo on IMU or not. + if (mSensor==IMU_MONOCULAR || mSensor==IMU_STEREO || mSensor==IMU_RGBD) + Twb = vpKFs[0]->GetImuPose(); + else + Twb = vpKFs[0]->GetPoseInverse(); + + ofstream f; + f.open(filename.c_str()); + // cout << "file open" << endl; + f << fixed; + + // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph). + // We need to get first the keyframe pose and then concatenate the relative transformation. + // Frames not localized (tracking failure) are not saved. + + // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag + // which is true when tracking failed (lbL). + list::iterator lRit = mpTracker->mlpReferences.begin(); + list::iterator lT = mpTracker->mlFrameTimes.begin(); + list::iterator lbL = mpTracker->mlbLost.begin(); + + //cout << "size mlpReferences: " << mpTracker->mlpReferences.size() << endl; + //cout << "size mlRelativeFramePoses: " << mpTracker->mlRelativeFramePoses.size() << endl; + //cout << "size mpTracker->mlFrameTimes: " << mpTracker->mlFrameTimes.size() << endl; + //cout << "size mpTracker->mlbLost: " << mpTracker->mlbLost.size() << endl; + + + for(auto lit=mpTracker->mlRelativeFramePoses.begin(), + lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++) + { + //cout << "1" << endl; + if(*lbL) + continue; + + + KeyFrame* pKF = *lRit; + //cout << "KF: " << pKF->mnId << endl; + + Sophus::SE3f Trw; + + // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe. + if (!pKF) + continue; + + //cout << "2.5" << endl; + + while(pKF->isBad()) + { + //cout << " 2.bad" << endl; + Trw = Trw * pKF->mTcp; + pKF = pKF->GetParent(); + //cout << "--Parent KF: " << pKF->mnId << endl; + } + + if(!pKF || pKF->GetMap() != pMap) + { + //cout << "--Parent KF is from another map" << endl; + continue; + } + + //cout << "3" << endl; + + Trw = Trw * pKF->GetPose()*Twb; // Tcp*Tpw*Twb0=Tcb0 where b0 is the new world reference + + // cout << "4" << endl; + + if (mSensor == IMU_MONOCULAR || mSensor == IMU_STEREO || mSensor==IMU_RGBD) + { + Sophus::SE3f Twb = (pKF->mImuCalib.mTbc * (*lit) * Trw).inverse(); + Eigen::Quaternionf q = Twb.unit_quaternion(); + Eigen::Vector3f twb = Twb.translation(); + f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + } + else + { + Sophus::SE3f Twc = ((*lit)*Trw).inverse(); + Eigen::Quaternionf q = Twc.unit_quaternion(); + Eigen::Vector3f twc = Twc.translation(); + f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + } + + // cout << "5" << endl; + } + //cout << "end saving trajectory" << endl; + f.close(); + cout << endl << "End of saving trajectory to " << filename << " ..." << endl; +} + +/*void System::SaveTrajectoryEuRoC(const string &filename) +{ + + cout << endl << "Saving trajectory to " << filename << " ..." << endl; + if(mSensor==MONOCULAR) + { + cerr << "ERROR: SaveTrajectoryEuRoC cannot be used for monocular." << endl; + return; + } + + vector vpMaps = mpAtlas->GetAllMaps(); + Map* pBiggerMap; + int numMaxKFs = 0; + for(Map* pMap :vpMaps) + { + if(pMap->GetAllKeyFrames().size() > numMaxKFs) + { + numMaxKFs = pMap->GetAllKeyFrames().size(); + pBiggerMap = pMap; + } + } + + vector vpKFs = pBiggerMap->GetAllKeyFrames(); + sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + Sophus::SE3f Twb; // Can be word to cam0 or world to b dependingo on IMU or not. + if (mSensor==IMU_MONOCULAR || mSensor==IMU_STEREO || mSensor==IMU_RGBD) + Twb = vpKFs[0]->GetImuPose_(); + else + Twb = vpKFs[0]->GetPoseInverse_(); + + ofstream f; + f.open(filename.c_str()); + // cout << "file open" << endl; + f << fixed; + + // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph). + // We need to get first the keyframe pose and then concatenate the relative transformation. + // Frames not localized (tracking failure) are not saved. + + // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag + // which is true when tracking failed (lbL). + list::iterator lRit = mpTracker->mlpReferences.begin(); + list::iterator lT = mpTracker->mlFrameTimes.begin(); + list::iterator lbL = mpTracker->mlbLost.begin(); + + //cout << "size mlpReferences: " << mpTracker->mlpReferences.size() << endl; + //cout << "size mlRelativeFramePoses: " << mpTracker->mlRelativeFramePoses.size() << endl; + //cout << "size mpTracker->mlFrameTimes: " << mpTracker->mlFrameTimes.size() << endl; + //cout << "size mpTracker->mlbLost: " << mpTracker->mlbLost.size() << endl; + + + for(list::iterator lit=mpTracker->mlRelativeFramePoses.begin(), + lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++) + { + //cout << "1" << endl; + if(*lbL) + continue; + + + KeyFrame* pKF = *lRit; + //cout << "KF: " << pKF->mnId << endl; + + Sophus::SE3f Trw; + + // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe. + if (!pKF) + continue; + + //cout << "2.5" << endl; + + while(pKF->isBad()) + { + //cout << " 2.bad" << endl; + Trw = Trw * pKF->mTcp; + pKF = pKF->GetParent(); + //cout << "--Parent KF: " << pKF->mnId << endl; + } + + if(!pKF || pKF->GetMap() != pBiggerMap) + { + //cout << "--Parent KF is from another map" << endl; + continue; + } + + //cout << "3" << endl; + + Trw = Trw * pKF->GetPose()*Twb; // Tcp*Tpw*Twb0=Tcb0 where b0 is the new world reference + + // cout << "4" << endl; + + + if (mSensor == IMU_MONOCULAR || mSensor == IMU_STEREO || mSensor==IMU_RGBD) + { + Sophus::SE3f Tbw = pKF->mImuCalib.Tbc_ * (*lit) * Trw; + Sophus::SE3f Twb = Tbw.inverse(); + + Eigen::Vector3f twb = Twb.translation(); + Eigen::Quaternionf q = Twb.unit_quaternion(); + f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + } + else + { + Sophus::SE3f Tcw = (*lit) * Trw; + Sophus::SE3f Twc = Tcw.inverse(); + + Eigen::Vector3f twc = Twc.translation(); + Eigen::Quaternionf q = Twc.unit_quaternion(); + f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + } + + // cout << "5" << endl; + } + //cout << "end saving trajectory" << endl; + f.close(); + cout << endl << "End of saving trajectory to " << filename << " ..." << endl; +}*/ + + +/*void System::SaveKeyFrameTrajectoryEuRoC_old(const string &filename) +{ + cout << endl << "Saving keyframe trajectory to " << filename << " ..." << endl; + + vector vpMaps = mpAtlas->GetAllMaps(); + Map* pBiggerMap; + int numMaxKFs = 0; + for(Map* pMap :vpMaps) + { + if(pMap->GetAllKeyFrames().size() > numMaxKFs) + { + numMaxKFs = pMap->GetAllKeyFrames().size(); + pBiggerMap = pMap; + } + } + + vector vpKFs = pBiggerMap->GetAllKeyFrames(); + sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + ofstream f; + f.open(filename.c_str()); + f << fixed; + + for(size_t i=0; iSetPose(pKF->GetPose()*Two); + + if(pKF->isBad()) + continue; + if (mSensor == IMU_MONOCULAR || mSensor == IMU_STEREO || mSensor==IMU_RGBD) + { + cv::Mat R = pKF->GetImuRotation().t(); + vector q = Converter::toQuaternion(R); + cv::Mat twb = pKF->GetImuPosition(); + f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << twb.at(0) << " " << twb.at(1) << " " << twb.at(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl; + + } + else + { + cv::Mat R = pKF->GetRotation(); + vector q = Converter::toQuaternion(R); + cv::Mat t = pKF->GetCameraCenter(); + f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << t.at(0) << " " << t.at(1) << " " << t.at(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl; + } + } + f.close(); +}*/ + +void System::SaveKeyFrameTrajectoryEuRoC(const string &filename) +{ + cout << endl << "Saving keyframe trajectory to " << filename << " ..." << endl; + + vector vpMaps = mpAtlas->GetAllMaps(); + Map* pBiggerMap; + int numMaxKFs = 0; + for(Map* pMap :vpMaps) + { + if(pMap && pMap->GetAllKeyFrames().size() > numMaxKFs) + { + numMaxKFs = pMap->GetAllKeyFrames().size(); + pBiggerMap = pMap; + } + } + + if(!pBiggerMap) + { + std::cout << "There is not a map!!" << std::endl; + return; + } + + vector vpKFs = pBiggerMap->GetAllKeyFrames(); + sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + ofstream f; + f.open(filename.c_str()); + f << fixed; + + for(size_t i=0; iSetPose(pKF->GetPose()*Two); + + if(!pKF || pKF->isBad()) + continue; + if (mSensor == IMU_MONOCULAR || mSensor == IMU_STEREO || mSensor==IMU_RGBD) + { + Sophus::SE3f Twb = pKF->GetImuPose(); + Eigen::Quaternionf q = Twb.unit_quaternion(); + Eigen::Vector3f twb = Twb.translation(); + f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + + } + else + { + Sophus::SE3f Twc = pKF->GetPoseInverse(); + Eigen::Quaternionf q = Twc.unit_quaternion(); + Eigen::Vector3f t = Twc.translation(); + f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << t(0) << " " << t(1) << " " << t(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + } + } + f.close(); +} + +void System::SaveKeyFrameTrajectoryEuRoC(const string &filename, Map* pMap) +{ + cout << endl << "Saving keyframe trajectory of map " << pMap->GetId() << " to " << filename << " ..." << endl; + + vector vpKFs = pMap->GetAllKeyFrames(); + sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + ofstream f; + f.open(filename.c_str()); + f << fixed; + + for(size_t i=0; iisBad()) + continue; + if (mSensor == IMU_MONOCULAR || mSensor == IMU_STEREO || mSensor==IMU_RGBD) + { + Sophus::SE3f Twb = pKF->GetImuPose(); + Eigen::Quaternionf q = Twb.unit_quaternion(); + Eigen::Vector3f twb = Twb.translation(); + f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + + } + else + { + Sophus::SE3f Twc = pKF->GetPoseInverse(); + Eigen::Quaternionf q = Twc.unit_quaternion(); + Eigen::Vector3f t = Twc.translation(); + f << setprecision(6) << 1e9*pKF->mTimeStamp << " " << setprecision(9) << t(0) << " " << t(1) << " " << t(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; + } + } + f.close(); +} + +/*void System::SaveTrajectoryKITTI(const string &filename) +{ + cout << endl << "Saving camera trajectory to " << filename << " ..." << endl; + if(mSensor==MONOCULAR) + { + cerr << "ERROR: SaveTrajectoryKITTI cannot be used for monocular." << endl; + return; + } + + vector vpKFs = mpAtlas->GetAllKeyFrames(); + sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + cv::Mat Two = vpKFs[0]->GetPoseInverse(); + + ofstream f; + f.open(filename.c_str()); + f << fixed; + + // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph). + // We need to get first the keyframe pose and then concatenate the relative transformation. + // Frames not localized (tracking failure) are not saved. + + // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag + // which is true when tracking failed (lbL). + list::iterator lRit = mpTracker->mlpReferences.begin(); + list::iterator lT = mpTracker->mlFrameTimes.begin(); + for(list::iterator lit=mpTracker->mlRelativeFramePoses.begin(), lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++) + { + ORB_SLAM3::KeyFrame* pKF = *lRit; + + cv::Mat Trw = cv::Mat::eye(4,4,CV_32F); + + while(pKF->isBad()) + { + Trw = Trw * Converter::toCvMat(pKF->mTcp.matrix()); + pKF = pKF->GetParent(); + } + + Trw = Trw * pKF->GetPoseCv() * Two; + + cv::Mat Tcw = (*lit)*Trw; + cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t(); + cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3); + + f << setprecision(9) << Rwc.at(0,0) << " " << Rwc.at(0,1) << " " << Rwc.at(0,2) << " " << twc.at(0) << " " << + Rwc.at(1,0) << " " << Rwc.at(1,1) << " " << Rwc.at(1,2) << " " << twc.at(1) << " " << + Rwc.at(2,0) << " " << Rwc.at(2,1) << " " << Rwc.at(2,2) << " " << twc.at(2) << endl; + } + f.close(); +}*/ + +void System::SaveTrajectoryKITTI(const string &filename) +{ + cout << endl << "Saving camera trajectory to " << filename << " ..." << endl; + if(mSensor==MONOCULAR) + { + cerr << "ERROR: SaveTrajectoryKITTI cannot be used for monocular." << endl; + return; + } + + vector vpKFs = mpAtlas->GetAllKeyFrames(); + sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + Sophus::SE3f Tow = vpKFs[0]->GetPoseInverse(); + + ofstream f; + f.open(filename.c_str()); + f << fixed; + + // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph). + // We need to get first the keyframe pose and then concatenate the relative transformation. + // Frames not localized (tracking failure) are not saved. + + // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag + // which is true when tracking failed (lbL). + list::iterator lRit = mpTracker->mlpReferences.begin(); + list::iterator lT = mpTracker->mlFrameTimes.begin(); + for(list::iterator lit=mpTracker->mlRelativeFramePoses.begin(), + lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++) + { + ORB_SLAM3::KeyFrame* pKF = *lRit; + + Sophus::SE3f Trw; + + if(!pKF) + continue; + + while(pKF->isBad()) + { + Trw = Trw * pKF->mTcp; + pKF = pKF->GetParent(); + } + + Trw = Trw * pKF->GetPose() * Tow; + + Sophus::SE3f Tcw = (*lit) * Trw; + Sophus::SE3f Twc = Tcw.inverse(); + Eigen::Matrix3f Rwc = Twc.rotationMatrix(); + Eigen::Vector3f twc = Twc.translation(); + + f << setprecision(9) << Rwc(0,0) << " " << Rwc(0,1) << " " << Rwc(0,2) << " " << twc(0) << " " << + Rwc(1,0) << " " << Rwc(1,1) << " " << Rwc(1,2) << " " << twc(1) << " " << + Rwc(2,0) << " " << Rwc(2,1) << " " << Rwc(2,2) << " " << twc(2) << endl; + } + f.close(); +} + + +void System::SaveDebugData(const int &initIdx) +{ + // 0. Save initialization trajectory + SaveTrajectoryEuRoC("init_FrameTrajectoy_" +to_string(mpLocalMapper->mInitSect)+ "_" + to_string(initIdx)+".txt"); + + // 1. Save scale + ofstream f; + f.open("init_Scale_" + to_string(mpLocalMapper->mInitSect) + ".txt", ios_base::app); + f << fixed; + f << mpLocalMapper->mScale << endl; + f.close(); + + // 2. Save gravity direction + f.open("init_GDir_" +to_string(mpLocalMapper->mInitSect)+ ".txt", ios_base::app); + f << fixed; + f << mpLocalMapper->mRwg(0,0) << "," << mpLocalMapper->mRwg(0,1) << "," << mpLocalMapper->mRwg(0,2) << endl; + f << mpLocalMapper->mRwg(1,0) << "," << mpLocalMapper->mRwg(1,1) << "," << mpLocalMapper->mRwg(1,2) << endl; + f << mpLocalMapper->mRwg(2,0) << "," << mpLocalMapper->mRwg(2,1) << "," << mpLocalMapper->mRwg(2,2) << endl; + f.close(); + + // 3. Save computational cost + f.open("init_CompCost_" +to_string(mpLocalMapper->mInitSect)+ ".txt", ios_base::app); + f << fixed; + f << mpLocalMapper->mCostTime << endl; + f.close(); + + // 4. Save biases + f.open("init_Biases_" +to_string(mpLocalMapper->mInitSect)+ ".txt", ios_base::app); + f << fixed; + f << mpLocalMapper->mbg(0) << "," << mpLocalMapper->mbg(1) << "," << mpLocalMapper->mbg(2) << endl; + f << mpLocalMapper->mba(0) << "," << mpLocalMapper->mba(1) << "," << mpLocalMapper->mba(2) << endl; + f.close(); + + // 5. Save covariance matrix + f.open("init_CovMatrix_" +to_string(mpLocalMapper->mInitSect)+ "_" +to_string(initIdx)+".txt", ios_base::app); + f << fixed; + for(int i=0; imcovInertial.rows(); i++) + { + for(int j=0; jmcovInertial.cols(); j++) + { + if(j!=0) + f << ","; + f << setprecision(15) << mpLocalMapper->mcovInertial(i,j); + } + f << endl; + } + f.close(); + + // 6. Save initialization time + f.open("init_Time_" +to_string(mpLocalMapper->mInitSect)+ ".txt", ios_base::app); + f << fixed; + f << mpLocalMapper->mInitTime << endl; + f.close(); +} + + +int System::GetTrackingState() +{ + unique_lock lock(mMutexState); + return mTrackingState; +} + +vector System::GetTrackedMapPoints() +{ + unique_lock lock(mMutexState); + return mTrackedMapPoints; +} + +vector System::GetTrackedKeyPointsUn() +{ + unique_lock lock(mMutexState); + return mTrackedKeyPointsUn; +} + +double System::GetTimeFromIMUInit() +{ + double aux = mpLocalMapper->GetCurrKFTime()-mpLocalMapper->mFirstTs; + if ((aux>0.) && mpAtlas->isImuInitialized()) + return mpLocalMapper->GetCurrKFTime()-mpLocalMapper->mFirstTs; + else + return 0.f; +} + +bool System::isLost() +{ + if (!mpAtlas->isImuInitialized()) + return false; + else + { + if ((mpTracker->mState==Tracking::LOST)) //||(mpTracker->mState==Tracking::RECENTLY_LOST)) + return true; + else + return false; + } +} + + +bool System::isFinished() +{ + return (GetTimeFromIMUInit()>0.1); +} + +void System::ChangeDataset() +{ + if(mpAtlas->GetCurrentMap()->KeyFramesInMap() < 12) + { + mpTracker->ResetActiveMap(); + } + else + { + mpTracker->CreateMapInAtlas(); + } + + mpTracker->NewDataset(); +} + +float System::GetImageScale() +{ + return mpTracker->GetImageScale(); +} + +#ifdef REGISTER_TIMES +void System::InsertRectTime(double& time) +{ + mpTracker->vdRectStereo_ms.push_back(time); +} + +void System::InsertResizeTime(double& time) +{ + mpTracker->vdResizeImage_ms.push_back(time); +} + +void System::InsertTrackTime(double& time) +{ + mpTracker->vdTrackTotal_ms.push_back(time); +} +#endif + +/** + * @brief 保存地图 + * @param type 保存类型 + */ +void System::SaveAtlas(int type) +{ + // mStrSaveAtlasToFile 如果配置文件里面没有指定，则不会保存地图 + if(!mStrSaveAtlasToFile.empty()) + { + //clock_t start = clock(); + + // Save the current session + // 1. 预保存想要保存的数据 + mpAtlas->PreSave(); + + // 2. 确定文件名字 + string pathSaveFileName = "./"; + pathSaveFileName = pathSaveFileName.append(mStrSaveAtlasToFile); + pathSaveFileName = pathSaveFileName.append(".osa"); + + // 3. 保存词典的校验结果及名字 + string strVocabularyChecksum = CalculateCheckSum(mStrVocabularyFilePath,TEXT_FILE); + std::size_t found = mStrVocabularyFilePath.find_last_of("/\\"); + string strVocabularyName = mStrVocabularyFilePath.substr(found+1); + + if(type == TEXT_FILE) // File text + { + cout << "Starting to write the save text file " << endl; + std::remove(pathSaveFileName.c_str()); + std::ofstream ofs(pathSaveFileName, std::ios::binary); + boost::archive::text_oarchive oa(ofs); + + oa << strVocabularyName; + oa << strVocabularyChecksum; + oa << mpAtlas; + cout << "End to write the save text file" << endl; + } + else if(type == BINARY_FILE) // File binary + { + cout << "Starting to write the save binary file" << endl; + std::remove(pathSaveFileName.c_str()); + std::ofstream ofs(pathSaveFileName, std::ios::binary); + boost::archive::binary_oarchive oa(ofs); + oa << strVocabularyName; + oa << strVocabularyChecksum; + oa << mpAtlas; + cout << "End to write save binary file" << endl; + } + } +} + +/** + * @brief 加载地图 + * @param type 保存类型 + */ +bool System::LoadAtlas(int type) +{ + // 1. 加载地图文件 + string strFileVoc, strVocChecksum; + bool isRead = false; + + string pathLoadFileName = "./"; + pathLoadFileName = pathLoadFileName.append(mStrLoadAtlasFromFile); + pathLoadFileName = pathLoadFileName.append(".osa"); + + if(type == TEXT_FILE) // File text + { + cout << "Starting to read the save text file " << endl; + std::ifstream ifs(pathLoadFileName, std::ios::binary); + if(!ifs.good()) + { + cout << "Load file not found" << endl; + return false; + } + boost::archive::text_iarchive ia(ifs); + ia >> strFileVoc; + ia >> strVocChecksum; + ia >> mpAtlas; + cout << "End to load the save text file " << endl; + isRead = true; + } + else if(type == BINARY_FILE) // File binary + { + cout << "Starting to read the save binary file" << endl; + std::ifstream ifs(pathLoadFileName, std::ios::binary); + if(!ifs.good()) + { + cout << "Load file not found" << endl; + return false; + } + boost::archive::binary_iarchive ia(ifs); + ia >> strFileVoc; + ia >> strVocChecksum; + ia >> mpAtlas; + cout << "End to load the save binary file" << endl; + isRead = true; + } + + // 2. 如果加载成功 + if(isRead) + { + //Check if the vocabulary is the same + // 校验词典是否一样 + string strInputVocabularyChecksum = CalculateCheckSum(mStrVocabularyFilePath,TEXT_FILE); + + if(strInputVocabularyChecksum.compare(strVocChecksum) != 0) + { + cout << "The vocabulary load isn't the same which the load session was created " << endl; + cout << "-Vocabulary name: " << strFileVoc << endl; + return false; // Both are differents + } + + // 加载对应数据 + mpAtlas->SetKeyFrameDababase(mpKeyFrameDatabase); + mpAtlas->SetORBVocabulary(mpVocabulary); + mpAtlas->PostLoad(); + + return true; + } + return false; +} + +// 校验词典文件，哈希出一个值，两个哈希值一样表示是同一文件 +string System::CalculateCheckSum(string filename, int type) +{ + string checksum = ""; + + unsigned char c[MD5_DIGEST_LENGTH]; + + std::ios_base::openmode flags = std::ios::in; + if(type == BINARY_FILE) // Binary file + flags = std::ios::in | std::ios::binary; + + ifstream f(filename.c_str(), flags); + if ( !f.is_open() ) + { + cout << "[E] Unable to open the in file " << filename << " for Md5 hash." << endl; + return checksum; + } + + MD5_CTX md5Context; + char buffer[1024]; + + MD5_Init (&md5Context); + while ( int count = f.readsome(buffer, sizeof(buffer))) + { + MD5_Update(&md5Context, buffer, count); + } + + f.close(); + + MD5_Final(c, &md5Context ); + + for(int i = 0; i < MD5_DIGEST_LENGTH; i++) + { + char aux[10]; + sprintf(aux,"%02x", c[i]); + checksum = checksum + aux; + } + + return checksum; +} + +} //namespace ORB_SLAM + diff --git a/ORB-SLAM3-UESTC/Workspace/src/Tracking.cc b/ORB-SLAM3-UESTC/Workspace/src/Tracking.cc new file mode 100644 index 0000000..eb6d270 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/Tracking.cc @@ -0,0 +1,4925 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#include "Tracking.h" + +#include "ORBmatcher.h" +#include "FrameDrawer.h" +#include "Converter.h" +#include "G2oTypes.h" +#include "Optimizer.h" +#include "Pinhole.h" +#include "KannalaBrandt8.h" +#include "MLPnPsolver.h" +#include "GeometricTools.h" + +#include + +#include +#include + + +using namespace std; + +// 程序中变量名的第一个字母如果为"m"则表示为类中的成员变量，member +// 第一个、第二个字母: +// "p"表示指针数据类型 +// "n"表示int类型 +// "b"表示bool类型 +// "s"表示set类型 +// "v"表示vector数据类型 +// 'l'表示list数据类型 +// "KF"表示KeyFrame数据类型 + +namespace ORB_SLAM3 +{ + +/** + * @brief 跟踪线程构造函数 + * @param pSys 系统类指针 + * @param pVoc 词典 + * @param pFrameDrawer 画图像的 + * @param pMapDrawer 画地图的 + * @param pAtlas atlas + * @param pKFDB 关键帧词典数据库 + * @param strSettingPath 参数文件路径 + * @param sensor 传感器类型 + * @param settings 参数类 + * @param _strSeqName 序列名字，没用到 + */ +Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, + Atlas *pAtlas, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor, Settings* settings, const string &_nameSeq) + : mState(NO_IMAGES_YET), mSensor(sensor), mTrackedFr(0), mbStep(false), + mbOnlyTracking(false), mbMapUpdated(false), mbVO(false), mpORBVocabulary(pVoc), mpKeyFrameDB(pKFDB), + mbReadyToInitializate(false), mpSystem(pSys), mpViewer(NULL), bStepByStep(false), + mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpAtlas(pAtlas), mnLastRelocFrameId(0), time_recently_lost(5.0), + mnInitialFrameId(0), mbCreatedMap(false), mnFirstFrameId(0), mpCamera2(nullptr), mpLastKeyFrame(static_cast(NULL)) +{ + // Load camera parameters from settings file + // Step 1 从配置文件中加载相机参数 + if(settings){ + newParameterLoader(settings); + } + else{ + cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); + + bool b_parse_cam = ParseCamParamFile(fSettings); + if(!b_parse_cam) + { + std::cout << "*Error with the camera parameters in the config file*" << std::endl; + } + + // Load ORB parameters + bool b_parse_orb = ParseORBParamFile(fSettings); + if(!b_parse_orb) + { + std::cout << "*Error with the ORB parameters in the config file*" << std::endl; + } + + bool b_parse_imu = true; + if(sensor==System::IMU_MONOCULAR || sensor==System::IMU_STEREO || sensor==System::IMU_RGBD) + { + b_parse_imu = ParseIMUParamFile(fSettings); + if(!b_parse_imu) + { + std::cout << "*Error with the IMU parameters in the config file*" << std::endl; + } + + mnFramesToResetIMU = mMaxFrames; + } + + if(!b_parse_cam || !b_parse_orb || !b_parse_imu) + { + std::cerr << "**ERROR in the config file, the format is not correct**" << std::endl; + try + { + throw -1; + } + catch(exception &e) + { + + } + } + } + + initID = 0; lastID = 0; + mbInitWith3KFs = false; + mnNumDataset = 0; + + // 遍历下地图中的相机，然后打印出来了 + vector vpCams = mpAtlas->GetAllCameras(); + std::cout << "There are " << vpCams.size() << " cameras in the atlas" << std::endl; + for(GeometricCamera* pCam : vpCams) + { + std::cout << "Camera " << pCam->GetId(); + if(pCam->GetType() == GeometricCamera::CAM_PINHOLE) + { + std::cout << " is pinhole" << std::endl; + } + else if(pCam->GetType() == GeometricCamera::CAM_FISHEYE) + { + std::cout << " is fisheye" << std::endl; + } + else + { + std::cout << " is unknown" << std::endl; + } + } + +#ifdef REGISTER_TIMES + vdRectStereo_ms.clear(); + vdResizeImage_ms.clear(); + vdORBExtract_ms.clear(); + vdStereoMatch_ms.clear(); + vdIMUInteg_ms.clear(); + vdPosePred_ms.clear(); + vdLMTrack_ms.clear(); + vdNewKF_ms.clear(); + vdTrackTotal_ms.clear(); +#endif +} + +#ifdef REGISTER_TIMES +double calcAverage(vector v_times) +{ + double accum = 0; + for(double value : v_times) + { + accum += value; + } + + return accum / v_times.size(); +} + +double calcDeviation(vector v_times, double average) +{ + double accum = 0; + for(double value : v_times) + { + accum += pow(value - average, 2); + } + return sqrt(accum / v_times.size()); +} + +double calcAverage(vector v_values) +{ + double accum = 0; + int total = 0; + for(double value : v_values) + { + if(value == 0) + continue; + accum += value; + total++; + } + + return accum / total; +} + +double calcDeviation(vector v_values, double average) +{ + double accum = 0; + int total = 0; + for(double value : v_values) + { + if(value == 0) + continue; + accum += pow(value - average, 2); + total++; + } + return sqrt(accum / total); +} + +void Tracking::LocalMapStats2File() +{ + ofstream f; + f.open("LocalMapTimeStats.txt"); + f << fixed << setprecision(6); + f << "#Stereo rect[ms], MP culling[ms], MP creation[ms], LBA[ms], KF culling[ms], Total[ms]" << endl; + for(int i=0; ivdLMTotal_ms.size(); ++i) + { + f << mpLocalMapper->vdKFInsert_ms[i] << "," << mpLocalMapper->vdMPCulling_ms[i] << "," + << mpLocalMapper->vdMPCreation_ms[i] << "," << mpLocalMapper->vdLBASync_ms[i] << "," + << mpLocalMapper->vdKFCullingSync_ms[i] << "," << mpLocalMapper->vdLMTotal_ms[i] << endl; + } + + f.close(); + + f.open("LBA_Stats.txt"); + f << fixed << setprecision(6); + f << "#LBA time[ms], KF opt[#], KF fixed[#], MP[#], Edges[#]" << endl; + for(int i=0; ivdLBASync_ms.size(); ++i) + { + f << mpLocalMapper->vdLBASync_ms[i] << "," << mpLocalMapper->vnLBA_KFopt[i] << "," + << mpLocalMapper->vnLBA_KFfixed[i] << "," << mpLocalMapper->vnLBA_MPs[i] << "," + << mpLocalMapper->vnLBA_edges[i] << endl; + } + + + f.close(); +} + +void Tracking::TrackStats2File() +{ + ofstream f; + f.open("SessionInfo.txt"); + f << fixed; + f << "Number of KFs: " << mpAtlas->GetAllKeyFrames().size() << endl; + f << "Number of MPs: " << mpAtlas->GetAllMapPoints().size() << endl; + + f << "OpenCV version: " << CV_VERSION << endl; + + f.close(); + + f.open("TrackingTimeStats.txt"); + f << fixed << setprecision(6); + + f << "#Image Rect[ms], Image Resize[ms], ORB ext[ms], Stereo match[ms], IMU preint[ms], Pose pred[ms], LM track[ms], KF dec[ms], Total[ms]" << endl; + + for(int i=0; ivdKFInsert_ms); + deviation = calcDeviation(mpLocalMapper->vdKFInsert_ms, average); + std::cout << "KF Insertion: " << average << "$\\pm$" << deviation << std::endl; + f << "KF Insertion: " << average << "$\\pm$" << deviation << std::endl; + + average = calcAverage(mpLocalMapper->vdMPCulling_ms); + deviation = calcDeviation(mpLocalMapper->vdMPCulling_ms, average); + std::cout << "MP Culling: " << average << "$\\pm$" << deviation << std::endl; + f << "MP Culling: " << average << "$\\pm$" << deviation << std::endl; + + average = calcAverage(mpLocalMapper->vdMPCreation_ms); + deviation = calcDeviation(mpLocalMapper->vdMPCreation_ms, average); + std::cout << "MP Creation: " << average << "$\\pm$" << deviation << std::endl; + f << "MP Creation: " << average << "$\\pm$" << deviation << std::endl; + + average = calcAverage(mpLocalMapper->vdLBA_ms); + deviation = calcDeviation(mpLocalMapper->vdLBA_ms, average); + std::cout << "LBA: " << average << "$\\pm$" << deviation << std::endl; + f << "LBA: " << average << "$\\pm$" << deviation << std::endl; + + average = calcAverage(mpLocalMapper->vdKFCulling_ms); + deviation = calcDeviation(mpLocalMapper->vdKFCulling_ms, average); + std::cout << "KF Culling: " << average << "$\\pm$" << deviation << std::endl; + f << "KF Culling: " << average << "$\\pm$" << deviation << std::endl; + + average = calcAverage(mpLocalMapper->vdLMTotal_ms); + deviation = calcDeviation(mpLocalMapper->vdLMTotal_ms, average); + std::cout << "Total Local Mapping: " << average << "$\\pm$" << deviation << std::endl; + f << "Total Local Mapping: " << average << "$\\pm$" << deviation << std::endl; + + // Local Mapping LBA complexity + std::cout << "---------------------------" << std::endl; + std::cout << std::endl << "LBA complexity (mean$\\pm$std)" << std::endl; + f << "---------------------------" << std::endl; + f << std::endl << "LBA complexity (mean$\\pm$std)" << std::endl; + + average = calcAverage(mpLocalMapper->vnLBA_edges); + deviation = calcDeviation(mpLocalMapper->vnLBA_edges, average); + std::cout << "LBA Edges: " << average << "$\\pm$" << deviation << std::endl; + f << "LBA Edges: " << average << "$\\pm$" << deviation << std::endl; + + average = calcAverage(mpLocalMapper->vnLBA_KFopt); + deviation = calcDeviation(mpLocalMapper->vnLBA_KFopt, average); + std::cout << "LBA KF optimized: " << average << "$\\pm$" << deviation << std::endl; + f << "LBA KF optimized: " << average << "$\\pm$" << deviation << std::endl; + + average = calcAverage(mpLocalMapper->vnLBA_KFfixed); + deviation = calcDeviation(mpLocalMapper->vnLBA_KFfixed, average); + std::cout << "LBA KF fixed: " << average << "$\\pm$" << deviation << std::endl; + f << "LBA KF fixed: " << average << "$\\pm$" << deviation << std::endl; + + average = calcAverage(mpLocalMapper->vnLBA_MPs); + deviation = calcDeviation(mpLocalMapper->vnLBA_MPs, average); + std::cout << "LBA MP: " << average << "$\\pm$" << deviation << std::endl << std::endl; + f << "LBA MP: " << average << "$\\pm$" << deviation << std::endl << std::endl; + + std::cout << "LBA executions: " << mpLocalMapper->nLBA_exec << std::endl; + std::cout << "LBA aborts: " << mpLocalMapper->nLBA_abort << std::endl; + f << "LBA executions: " << mpLocalMapper->nLBA_exec << std::endl; + f << "LBA aborts: " << mpLocalMapper->nLBA_abort << std::endl; + + // Map complexity + std::cout << "---------------------------" << std::endl; + std::cout << std::endl << "Map complexity" << std::endl; + std::cout << "KFs in map: " << mpAtlas->GetAllKeyFrames().size() << std::endl; + std::cout << "MPs in map: " << mpAtlas->GetAllMapPoints().size() << std::endl; + f << "---------------------------" << std::endl; + f << std::endl << "Map complexity" << std::endl; + vector vpMaps = mpAtlas->GetAllMaps(); + Map* pBestMap = vpMaps[0]; + for(int i=1; iGetAllKeyFrames().size() < vpMaps[i]->GetAllKeyFrames().size()) + { + pBestMap = vpMaps[i]; + } + } + + f << "KFs in map: " << pBestMap->GetAllKeyFrames().size() << std::endl; + f << "MPs in map: " << pBestMap->GetAllMapPoints().size() << std::endl; + + f << "---------------------------" << std::endl; + f << std::endl << "Place Recognition (mean$\\pm$std)" << std::endl; + std::cout << "---------------------------" << std::endl; + std::cout << std::endl << "Place Recognition (mean$\\pm$std)" << std::endl; + average = calcAverage(mpLoopClosing->vdDataQuery_ms); + deviation = calcDeviation(mpLoopClosing->vdDataQuery_ms, average); + f << "Database Query: " << average << "$\\pm$" << deviation << std::endl; + std::cout << "Database Query: " << average << "$\\pm$" << deviation << std::endl; + average = calcAverage(mpLoopClosing->vdEstSim3_ms); + deviation = calcDeviation(mpLoopClosing->vdEstSim3_ms, average); + f << "SE3 estimation: " << average << "$\\pm$" << deviation << std::endl; + std::cout << "SE3 estimation: " << average << "$\\pm$" << deviation << std::endl; + average = calcAverage(mpLoopClosing->vdPRTotal_ms); + deviation = calcDeviation(mpLoopClosing->vdPRTotal_ms, average); + f << "Total Place Recognition: " << average << "$\\pm$" << deviation << std::endl << std::endl; + std::cout << "Total Place Recognition: " << average << "$\\pm$" << deviation << std::endl << std::endl; + + f << std::endl << "Loop Closing (mean$\\pm$std)" << std::endl; + std::cout << std::endl << "Loop Closing (mean$\\pm$std)" << std::endl; + average = calcAverage(mpLoopClosing->vdLoopFusion_ms); + deviation = calcDeviation(mpLoopClosing->vdLoopFusion_ms, average); + f << "Loop Fusion: " << average << "$\\pm$" << deviation << std::endl; + std::cout << "Loop Fusion: " << average << "$\\pm$" << deviation << std::endl; + average = calcAverage(mpLoopClosing->vdLoopOptEss_ms); + deviation = calcDeviation(mpLoopClosing->vdLoopOptEss_ms, average); + f << "Essential Graph: " << average << "$\\pm$" << deviation << std::endl; + std::cout << "Essential Graph: " << average << "$\\pm$" << deviation << std::endl; + average = calcAverage(mpLoopClosing->vdLoopTotal_ms); + deviation = calcDeviation(mpLoopClosing->vdLoopTotal_ms, average); + f << "Total Loop Closing: " << average << "$\\pm$" << deviation << std::endl << std::endl; + std::cout << "Total Loop Closing: " << average << "$\\pm$" << deviation << std::endl << std::endl; + + f << "Numb exec: " << mpLoopClosing->nLoop << std::endl; + std::cout << "Num exec: " << mpLoopClosing->nLoop << std::endl; + average = calcAverage(mpLoopClosing->vnLoopKFs); + deviation = calcDeviation(mpLoopClosing->vnLoopKFs, average); + f << "Number of KFs: " << average << "$\\pm$" << deviation << std::endl; + std::cout << "Number of KFs: " << average << "$\\pm$" << deviation << std::endl; + + f << std::endl << "Map Merging (mean$\\pm$std)" << std::endl; + std::cout << std::endl << "Map Merging (mean$\\pm$std)" << std::endl; + average = calcAverage(mpLoopClosing->vdMergeMaps_ms); + deviation = calcDeviation(mpLoopClosing->vdMergeMaps_ms, average); + f << "Merge Maps: " << average << "$\\pm$" << deviation << std::endl; + std::cout << "Merge Maps: " << average << "$\\pm$" << deviation << std::endl; + average = calcAverage(mpLoopClosing->vdWeldingBA_ms); + deviation = calcDeviation(mpLoopClosing->vdWeldingBA_ms, average); + f << "Welding BA: " << average << "$\\pm$" << deviation << std::endl; + std::cout << "Welding BA: " << average << "$\\pm$" << deviation << std::endl; + average = calcAverage(mpLoopClosing->vdMergeOptEss_ms); + deviation = calcDeviation(mpLoopClosing->vdMergeOptEss_ms, average); + f << "Optimization Ess.: " << average << "$\\pm$" << deviation << std::endl; + std::cout << "Optimization Ess.: " << average << "$\\pm$" << deviation << std::endl; + average = calcAverage(mpLoopClosing->vdMergeTotal_ms); + deviation = calcDeviation(mpLoopClosing->vdMergeTotal_ms, average); + f << "Total Map Merging: " << average << "$\\pm$" << deviation << std::endl << std::endl; + std::cout << "Total Map Merging: " << average << "$\\pm$" << deviation << std::endl << std::endl; + + f << "Numb exec: " << mpLoopClosing->nMerges << std::endl; + std::cout << "Num exec: " << mpLoopClosing->nMerges << std::endl; + average = calcAverage(mpLoopClosing->vnMergeKFs); + deviation = calcDeviation(mpLoopClosing->vnMergeKFs, average); + f << "Number of KFs: " << average << "$\\pm$" << deviation << std::endl; + std::cout << "Number of KFs: " << average << "$\\pm$" << deviation << std::endl; + average = calcAverage(mpLoopClosing->vnMergeMPs); + deviation = calcDeviation(mpLoopClosing->vnMergeMPs, average); + f << "Number of MPs: " << average << "$\\pm$" << deviation << std::endl; + std::cout << "Number of MPs: " << average << "$\\pm$" << deviation << std::endl; + + f << std::endl << "Full GBA (mean$\\pm$std)" << std::endl; + std::cout << std::endl << "Full GBA (mean$\\pm$std)" << std::endl; + average = calcAverage(mpLoopClosing->vdGBA_ms); + deviation = calcDeviation(mpLoopClosing->vdGBA_ms, average); + f << "GBA: " << average << "$\\pm$" << deviation << std::endl; + std::cout << "GBA: " << average << "$\\pm$" << deviation << std::endl; + average = calcAverage(mpLoopClosing->vdUpdateMap_ms); + deviation = calcDeviation(mpLoopClosing->vdUpdateMap_ms, average); + f << "Map Update: " << average << "$\\pm$" << deviation << std::endl; + std::cout << "Map Update: " << average << "$\\pm$" << deviation << std::endl; + average = calcAverage(mpLoopClosing->vdFGBATotal_ms); + deviation = calcDeviation(mpLoopClosing->vdFGBATotal_ms, average); + f << "Total Full GBA: " << average << "$\\pm$" << deviation << std::endl << std::endl; + std::cout << "Total Full GBA: " << average << "$\\pm$" << deviation << std::endl << std::endl; + + f << "Numb exec: " << mpLoopClosing->nFGBA_exec << std::endl; + std::cout << "Num exec: " << mpLoopClosing->nFGBA_exec << std::endl; + f << "Numb abort: " << mpLoopClosing->nFGBA_abort << std::endl; + std::cout << "Num abort: " << mpLoopClosing->nFGBA_abort << std::endl; + average = calcAverage(mpLoopClosing->vnGBAKFs); + deviation = calcDeviation(mpLoopClosing->vnGBAKFs, average); + f << "Number of KFs: " << average << "$\\pm$" << deviation << std::endl; + std::cout << "Number of KFs: " << average << "$\\pm$" << deviation << std::endl; + average = calcAverage(mpLoopClosing->vnGBAMPs); + deviation = calcDeviation(mpLoopClosing->vnGBAMPs, average); + f << "Number of MPs: " << average << "$\\pm$" << deviation << std::endl; + std::cout << "Number of MPs: " << average << "$\\pm$" << deviation << std::endl; + + f.close(); + +} + +#endif + +Tracking::~Tracking() +{ + //f_track_stats.close(); + +} + +/** + * @brief 根据参数类读取参数，可快速略过不看 + * @param settings 参数类 + */ +void Tracking::newParameterLoader(Settings *settings) { + // 1. 读取相机1 + mpCamera = settings->camera1(); + mpCamera = mpAtlas->AddCamera(mpCamera); + + if(settings->needToUndistort()){ + mDistCoef = settings->camera1DistortionCoef(); + } + else{ + mDistCoef = cv::Mat::zeros(4,1,CV_32F); + } + + //TODO: missing image scaling and rectification + mImageScale = 1.0f; + + mK = cv::Mat::eye(3,3,CV_32F); + mK.at(0,0) = mpCamera->getParameter(0); + mK.at(1,1) = mpCamera->getParameter(1); + mK.at(0,2) = mpCamera->getParameter(2); + mK.at(1,2) = mpCamera->getParameter(3); + + mK_.setIdentity(); + mK_(0,0) = mpCamera->getParameter(0); + mK_(1,1) = mpCamera->getParameter(1); + mK_(0,2) = mpCamera->getParameter(2); + mK_(1,2) = mpCamera->getParameter(3); + + // 读取相机2 + if((mSensor==System::STEREO || mSensor==System::IMU_STEREO || mSensor==System::IMU_RGBD) && + settings->cameraType() == Settings::KannalaBrandt){ + mpCamera2 = settings->camera2(); + mpCamera2 = mpAtlas->AddCamera(mpCamera2); + + mTlr = settings->Tlr(); + + mpFrameDrawer->both = true; + } + + // 读取双目 + if(mSensor==System::STEREO || mSensor==System::RGBD || mSensor==System::IMU_STEREO || mSensor==System::IMU_RGBD ){ + mbf = settings->bf(); + mThDepth = settings->b() * settings->thDepth(); + } + + // 读取rgbd + if(mSensor==System::RGBD || mSensor==System::IMU_RGBD){ + mDepthMapFactor = settings->depthMapFactor(); + if(fabs(mDepthMapFactor)<1e-5) + mDepthMapFactor=1; + else + mDepthMapFactor = 1.0f/mDepthMapFactor; + } + + mMinFrames = 0; + mMaxFrames = settings->fps(); + mbRGB = settings->rgb(); + + //ORB parameters + // 2. 读取特征点参数 + int nFeatures = settings->nFeatures(); + int nLevels = settings->nLevels(); + int fIniThFAST = settings->initThFAST(); + int fMinThFAST = settings->minThFAST(); + float fScaleFactor = settings->scaleFactor(); + + mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); + + if(mSensor==System::STEREO || mSensor==System::IMU_STEREO) + mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); + + if(mSensor==System::MONOCULAR || mSensor==System::IMU_MONOCULAR) + mpIniORBextractor = new ORBextractor(5*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); + + //IMU parameters + // 3. 读取imu参数 + Sophus::SE3f Tbc = settings->Tbc(); + mInsertKFsLost = settings->insertKFsWhenLost(); + mImuFreq = settings->imuFrequency(); + mImuPer = 0.001; //1.0 / (double) mImuFreq; //TODO: ESTO ESTA BIEN? + float Ng = settings->noiseGyro(); + float Na = settings->noiseAcc(); + float Ngw = settings->gyroWalk(); + float Naw = settings->accWalk(); + + const float sf = sqrt(mImuFreq); + mpImuCalib = new IMU::Calib(Tbc,Ng*sf,Na*sf,Ngw/sf,Naw/sf); + + mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib); +} + +/** + * @brief 根据文件读取相机参数，可快速略过不看 + * @param fSettings 配置文件 + */ +bool Tracking::ParseCamParamFile(cv::FileStorage &fSettings) +{ + mDistCoef = cv::Mat::zeros(4,1,CV_32F); + cout << endl << "Camera Parameters: " << endl; + bool b_miss_params = false; + + string sCameraName = fSettings["Camera.type"]; + if(sCameraName == "PinHole") + { + float fx, fy, cx, cy; + mImageScale = 1.f; + + // Camera calibration parameters + cv::FileNode node = fSettings["Camera.fx"]; + if(!node.empty() && node.isReal()) + { + fx = node.real(); + } + else + { + std::cerr << "*Camera.fx parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Camera.fy"]; + if(!node.empty() && node.isReal()) + { + fy = node.real(); + } + else + { + std::cerr << "*Camera.fy parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Camera.cx"]; + if(!node.empty() && node.isReal()) + { + cx = node.real(); + } + else + { + std::cerr << "*Camera.cx parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Camera.cy"]; + if(!node.empty() && node.isReal()) + { + cy = node.real(); + } + else + { + std::cerr << "*Camera.cy parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + // Distortion parameters + node = fSettings["Camera.k1"]; + if(!node.empty() && node.isReal()) + { + mDistCoef.at(0) = node.real(); + } + else + { + std::cerr << "*Camera.k1 parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Camera.k2"]; + if(!node.empty() && node.isReal()) + { + mDistCoef.at(1) = node.real(); + } + else + { + std::cerr << "*Camera.k2 parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Camera.p1"]; + if(!node.empty() && node.isReal()) + { + mDistCoef.at(2) = node.real(); + } + else + { + std::cerr << "*Camera.p1 parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Camera.p2"]; + if(!node.empty() && node.isReal()) + { + mDistCoef.at(3) = node.real(); + } + else + { + std::cerr << "*Camera.p2 parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Camera.k3"]; + if(!node.empty() && node.isReal()) + { + mDistCoef.resize(5); + mDistCoef.at(4) = node.real(); + } + + node = fSettings["Camera.imageScale"]; + if(!node.empty() && node.isReal()) + { + mImageScale = node.real(); + } + + if(b_miss_params) + { + return false; + } + + if(mImageScale != 1.f) + { + // K matrix parameters must be scaled. + fx = fx * mImageScale; + fy = fy * mImageScale; + cx = cx * mImageScale; + cy = cy * mImageScale; + } + + vector vCamCalib{fx,fy,cx,cy}; + + mpCamera = new Pinhole(vCamCalib); + + mpCamera = mpAtlas->AddCamera(mpCamera); + + std::cout << "- Camera: Pinhole" << std::endl; + std::cout << "- Image scale: " << mImageScale << std::endl; + std::cout << "- fx: " << fx << std::endl; + std::cout << "- fy: " << fy << std::endl; + std::cout << "- cx: " << cx << std::endl; + std::cout << "- cy: " << cy << std::endl; + std::cout << "- k1: " << mDistCoef.at(0) << std::endl; + std::cout << "- k2: " << mDistCoef.at(1) << std::endl; + + + std::cout << "- p1: " << mDistCoef.at(2) << std::endl; + std::cout << "- p2: " << mDistCoef.at(3) << std::endl; + + if(mDistCoef.rows==5) + std::cout << "- k3: " << mDistCoef.at(4) << std::endl; + + mK = cv::Mat::eye(3,3,CV_32F); + mK.at(0,0) = fx; + mK.at(1,1) = fy; + mK.at(0,2) = cx; + mK.at(1,2) = cy; + + mK_.setIdentity(); + mK_(0,0) = fx; + mK_(1,1) = fy; + mK_(0,2) = cx; + mK_(1,2) = cy; + } + else if(sCameraName == "KannalaBrandt8") + { + float fx, fy, cx, cy; + float k1, k2, k3, k4; + mImageScale = 1.f; + + // Camera calibration parameters + cv::FileNode node = fSettings["Camera.fx"]; + if(!node.empty() && node.isReal()) + { + fx = node.real(); + } + else + { + std::cerr << "*Camera.fx parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + node = fSettings["Camera.fy"]; + if(!node.empty() && node.isReal()) + { + fy = node.real(); + } + else + { + std::cerr << "*Camera.fy parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Camera.cx"]; + if(!node.empty() && node.isReal()) + { + cx = node.real(); + } + else + { + std::cerr << "*Camera.cx parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Camera.cy"]; + if(!node.empty() && node.isReal()) + { + cy = node.real(); + } + else + { + std::cerr << "*Camera.cy parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + // Distortion parameters + node = fSettings["Camera.k1"]; + if(!node.empty() && node.isReal()) + { + k1 = node.real(); + } + else + { + std::cerr << "*Camera.k1 parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + node = fSettings["Camera.k2"]; + if(!node.empty() && node.isReal()) + { + k2 = node.real(); + } + else + { + std::cerr << "*Camera.k2 parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Camera.k3"]; + if(!node.empty() && node.isReal()) + { + k3 = node.real(); + } + else + { + std::cerr << "*Camera.k3 parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Camera.k4"]; + if(!node.empty() && node.isReal()) + { + k4 = node.real(); + } + else + { + std::cerr << "*Camera.k4 parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Camera.imageScale"]; + if(!node.empty() && node.isReal()) + { + mImageScale = node.real(); + } + + if(!b_miss_params) + { + if(mImageScale != 1.f) + { + // K matrix parameters must be scaled. + fx = fx * mImageScale; + fy = fy * mImageScale; + cx = cx * mImageScale; + cy = cy * mImageScale; + } + + vector vCamCalib{fx,fy,cx,cy,k1,k2,k3,k4}; + mpCamera = new KannalaBrandt8(vCamCalib); + mpCamera = mpAtlas->AddCamera(mpCamera); + std::cout << "- Camera: Fisheye" << std::endl; + std::cout << "- Image scale: " << mImageScale << std::endl; + std::cout << "- fx: " << fx << std::endl; + std::cout << "- fy: " << fy << std::endl; + std::cout << "- cx: " << cx << std::endl; + std::cout << "- cy: " << cy << std::endl; + std::cout << "- k1: " << k1 << std::endl; + std::cout << "- k2: " << k2 << std::endl; + std::cout << "- k3: " << k3 << std::endl; + std::cout << "- k4: " << k4 << std::endl; + + mK = cv::Mat::eye(3,3,CV_32F); + mK.at(0,0) = fx; + mK.at(1,1) = fy; + mK.at(0,2) = cx; + mK.at(1,2) = cy; + + mK_.setIdentity(); + mK_(0,0) = fx; + mK_(1,1) = fy; + mK_(0,2) = cx; + mK_(1,2) = cy; + } + + if(mSensor==System::STEREO || mSensor==System::IMU_STEREO || mSensor==System::IMU_RGBD){ + // Right camera + // Camera calibration parameters + cv::FileNode node = fSettings["Camera2.fx"]; + if(!node.empty() && node.isReal()) + { + fx = node.real(); + } + else + { + std::cerr << "*Camera2.fx parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + node = fSettings["Camera2.fy"]; + if(!node.empty() && node.isReal()) + { + fy = node.real(); + } + else + { + std::cerr << "*Camera2.fy parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Camera2.cx"]; + if(!node.empty() && node.isReal()) + { + cx = node.real(); + } + else + { + std::cerr << "*Camera2.cx parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Camera2.cy"]; + if(!node.empty() && node.isReal()) + { + cy = node.real(); + } + else + { + std::cerr << "*Camera2.cy parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + // Distortion parameters + node = fSettings["Camera2.k1"]; + if(!node.empty() && node.isReal()) + { + k1 = node.real(); + } + else + { + std::cerr << "*Camera2.k1 parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + node = fSettings["Camera2.k2"]; + if(!node.empty() && node.isReal()) + { + k2 = node.real(); + } + else + { + std::cerr << "*Camera2.k2 parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Camera2.k3"]; + if(!node.empty() && node.isReal()) + { + k3 = node.real(); + } + else + { + std::cerr << "*Camera2.k3 parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Camera2.k4"]; + if(!node.empty() && node.isReal()) + { + k4 = node.real(); + } + else + { + std::cerr << "*Camera2.k4 parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + + int leftLappingBegin = -1; + int leftLappingEnd = -1; + + int rightLappingBegin = -1; + int rightLappingEnd = -1; + + node = fSettings["Camera.lappingBegin"]; + if(!node.empty() && node.isInt()) + { + leftLappingBegin = node.operator int(); + } + else + { + std::cout << "WARNING: Camera.lappingBegin not correctly defined" << std::endl; + } + node = fSettings["Camera.lappingEnd"]; + if(!node.empty() && node.isInt()) + { + leftLappingEnd = node.operator int(); + } + else + { + std::cout << "WARNING: Camera.lappingEnd not correctly defined" << std::endl; + } + node = fSettings["Camera2.lappingBegin"]; + if(!node.empty() && node.isInt()) + { + rightLappingBegin = node.operator int(); + } + else + { + std::cout << "WARNING: Camera2.lappingBegin not correctly defined" << std::endl; + } + node = fSettings["Camera2.lappingEnd"]; + if(!node.empty() && node.isInt()) + { + rightLappingEnd = node.operator int(); + } + else + { + std::cout << "WARNING: Camera2.lappingEnd not correctly defined" << std::endl; + } + + node = fSettings["Tlr"]; + cv::Mat cvTlr; + if(!node.empty()) + { + cvTlr = node.mat(); + if(cvTlr.rows != 3 || cvTlr.cols != 4) + { + std::cerr << "*Tlr matrix have to be a 3x4 transformation matrix*" << std::endl; + b_miss_params = true; + } + } + else + { + std::cerr << "*Tlr matrix doesn't exist*" << std::endl; + b_miss_params = true; + } + + if(!b_miss_params) + { + if(mImageScale != 1.f) + { + // K matrix parameters must be scaled. + fx = fx * mImageScale; + fy = fy * mImageScale; + cx = cx * mImageScale; + cy = cy * mImageScale; + + leftLappingBegin = leftLappingBegin * mImageScale; + leftLappingEnd = leftLappingEnd * mImageScale; + rightLappingBegin = rightLappingBegin * mImageScale; + rightLappingEnd = rightLappingEnd * mImageScale; + } + + static_cast(mpCamera)->mvLappingArea[0] = leftLappingBegin; + static_cast(mpCamera)->mvLappingArea[1] = leftLappingEnd; + + mpFrameDrawer->both = true; + + vector vCamCalib2{fx,fy,cx,cy,k1,k2,k3,k4}; + mpCamera2 = new KannalaBrandt8(vCamCalib2); + mpCamera2 = mpAtlas->AddCamera(mpCamera2); + + mTlr = Converter::toSophus(cvTlr); + + static_cast(mpCamera2)->mvLappingArea[0] = rightLappingBegin; + static_cast(mpCamera2)->mvLappingArea[1] = rightLappingEnd; + + std::cout << "- Camera1 Lapping: " << leftLappingBegin << ", " << leftLappingEnd << std::endl; + + std::cout << std::endl << "Camera2 Parameters:" << std::endl; + std::cout << "- Camera: Fisheye" << std::endl; + std::cout << "- Image scale: " << mImageScale << std::endl; + std::cout << "- fx: " << fx << std::endl; + std::cout << "- fy: " << fy << std::endl; + std::cout << "- cx: " << cx << std::endl; + std::cout << "- cy: " << cy << std::endl; + std::cout << "- k1: " << k1 << std::endl; + std::cout << "- k2: " << k2 << std::endl; + std::cout << "- k3: " << k3 << std::endl; + std::cout << "- k4: " << k4 << std::endl; + + std::cout << "- mTlr: \n" << cvTlr << std::endl; + + std::cout << "- Camera2 Lapping: " << rightLappingBegin << ", " << rightLappingEnd << std::endl; + } + } + + if(b_miss_params) + { + return false; + } + + } + else + { + std::cerr << "*Not Supported Camera Sensor*" << std::endl; + std::cerr << "Check an example configuration file with the desired sensor" << std::endl; + } + + if(mSensor==System::STEREO || mSensor==System::RGBD || mSensor==System::IMU_STEREO || mSensor==System::IMU_RGBD ) + { + cv::FileNode node = fSettings["Camera.bf"]; + if(!node.empty() && node.isReal()) + { + mbf = node.real(); + if(mImageScale != 1.f) + { + mbf *= mImageScale; + } + } + else + { + std::cerr << "*Camera.bf parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + } + + float fps = fSettings["Camera.fps"]; + if(fps==0) + fps=30; + + // Max/Min Frames to insert keyframes and to check relocalisation + mMinFrames = 0; + mMaxFrames = fps; + + cout << "- fps: " << fps << endl; + + + int nRGB = fSettings["Camera.RGB"]; + mbRGB = nRGB; + + if(mbRGB) + cout << "- color order: RGB (ignored if grayscale)" << endl; + else + cout << "- color order: BGR (ignored if grayscale)" << endl; + + if(mSensor==System::STEREO || mSensor==System::RGBD || mSensor==System::IMU_STEREO || mSensor==System::IMU_RGBD) + { + float fx = mpCamera->getParameter(0); + cv::FileNode node = fSettings["ThDepth"]; + if(!node.empty() && node.isReal()) + { + mThDepth = node.real(); + mThDepth = mbf*mThDepth/fx; + cout << endl << "Depth Threshold (Close/Far Points): " << mThDepth << endl; + } + else + { + std::cerr << "*ThDepth parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + + } + + if(mSensor==System::RGBD || mSensor==System::IMU_RGBD) + { + cv::FileNode node = fSettings["DepthMapFactor"]; + if(!node.empty() && node.isReal()) + { + mDepthMapFactor = node.real(); + if(fabs(mDepthMapFactor)<1e-5) + mDepthMapFactor=1; + else + mDepthMapFactor = 1.0f/mDepthMapFactor; + } + else + { + std::cerr << "*DepthMapFactor parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + } + + if(b_miss_params) + { + return false; + } + + return true; +} + +/** + * @brief 根据文件读取特征点参数，可快速略过不看 + * @param fSettings 配置文件 + */ +bool Tracking::ParseORBParamFile(cv::FileStorage &fSettings) +{ + bool b_miss_params = false; + int nFeatures, nLevels, fIniThFAST, fMinThFAST; + float fScaleFactor; + + cv::FileNode node = fSettings["ORBextractor.nFeatures"]; + if(!node.empty() && node.isInt()) + { + nFeatures = node.operator int(); + } + else + { + std::cerr << "*ORBextractor.nFeatures parameter doesn't exist or is not an integer*" << std::endl; + b_miss_params = true; + } + + node = fSettings["ORBextractor.scaleFactor"]; + if(!node.empty() && node.isReal()) + { + fScaleFactor = node.real(); + } + else + { + std::cerr << "*ORBextractor.scaleFactor parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["ORBextractor.nLevels"]; + if(!node.empty() && node.isInt()) + { + nLevels = node.operator int(); + } + else + { + std::cerr << "*ORBextractor.nLevels parameter doesn't exist or is not an integer*" << std::endl; + b_miss_params = true; + } + + node = fSettings["ORBextractor.iniThFAST"]; + if(!node.empty() && node.isInt()) + { + fIniThFAST = node.operator int(); + } + else + { + std::cerr << "*ORBextractor.iniThFAST parameter doesn't exist or is not an integer*" << std::endl; + b_miss_params = true; + } + + node = fSettings["ORBextractor.minThFAST"]; + if(!node.empty() && node.isInt()) + { + fMinThFAST = node.operator int(); + } + else + { + std::cerr << "*ORBextractor.minThFAST parameter doesn't exist or is not an integer*" << std::endl; + b_miss_params = true; + } + + if(b_miss_params) + { + return false; + } + + mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); + + if(mSensor==System::STEREO || mSensor==System::IMU_STEREO) + mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); + + if(mSensor==System::MONOCULAR || mSensor==System::IMU_MONOCULAR) + mpIniORBextractor = new ORBextractor(5*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); + + cout << endl << "ORB Extractor Parameters: " << endl; + cout << "- Number of Features: " << nFeatures << endl; + cout << "- Scale Levels: " << nLevels << endl; + cout << "- Scale Factor: " << fScaleFactor << endl; + cout << "- Initial Fast Threshold: " << fIniThFAST << endl; + cout << "- Minimum Fast Threshold: " << fMinThFAST << endl; + + return true; +} + +/** + * @brief 根据文件读取IMU参数，可快速略过不看 + * @param fSettings 配置文件 + */ +bool Tracking::ParseIMUParamFile(cv::FileStorage &fSettings) +{ + bool b_miss_params = false; + + cv::Mat cvTbc; + cv::FileNode node = fSettings["Tbc"]; + if(!node.empty()) + { + cvTbc = node.mat(); + if(cvTbc.rows != 4 || cvTbc.cols != 4) + { + std::cerr << "*Tbc matrix have to be a 4x4 transformation matrix*" << std::endl; + b_miss_params = true; + } + } + else + { + std::cerr << "*Tbc matrix doesn't exist*" << std::endl; + b_miss_params = true; + } + cout << endl; + cout << "Left camera to Imu Transform (Tbc): " << endl << cvTbc << endl; + Eigen::Matrix eigTbc(cvTbc.ptr(0)); + Sophus::SE3f Tbc(eigTbc); + + node = fSettings["InsertKFsWhenLost"]; + mInsertKFsLost = true; + if(!node.empty() && node.isInt()) + { + mInsertKFsLost = (bool) node.operator int(); + } + + if(!mInsertKFsLost) + cout << "Do not insert keyframes when lost visual tracking " << endl; + + + + float Ng, Na, Ngw, Naw; + + node = fSettings["IMU.Frequency"]; + if(!node.empty() && node.isInt()) + { + mImuFreq = node.operator int(); + mImuPer = 0.001; //1.0 / (double) mImuFreq; + } + else + { + std::cerr << "*IMU.Frequency parameter doesn't exist or is not an integer*" << std::endl; + b_miss_params = true; + } + + node = fSettings["IMU.NoiseGyro"]; + if(!node.empty() && node.isReal()) + { + Ng = node.real(); + } + else + { + std::cerr << "*IMU.NoiseGyro parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["IMU.NoiseAcc"]; + if(!node.empty() && node.isReal()) + { + Na = node.real(); + } + else + { + std::cerr << "*IMU.NoiseAcc parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["IMU.GyroWalk"]; + if(!node.empty() && node.isReal()) + { + Ngw = node.real(); + } + else + { + std::cerr << "*IMU.GyroWalk parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["IMU.AccWalk"]; + if(!node.empty() && node.isReal()) + { + Naw = node.real(); + } + else + { + std::cerr << "*IMU.AccWalk parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["IMU.fastInit"]; + mFastInit = false; + if(!node.empty()) + { + mFastInit = static_cast(fSettings["IMU.fastInit"]) != 0; + } + + if(mFastInit) + cout << "Fast IMU initialization. Acceleration is not checked \n"; + + if(b_miss_params) + { + return false; + } + + const float sf = sqrt(mImuFreq); + cout << endl; + cout << "IMU frequency: " << mImuFreq << " Hz" << endl; + cout << "IMU gyro noise: " << Ng << " rad/s/sqrt(Hz)" << endl; + cout << "IMU gyro walk: " << Ngw << " rad/s^2/sqrt(Hz)" << endl; + cout << "IMU accelerometer noise: " << Na << " m/s^2/sqrt(Hz)" << endl; + cout << "IMU accelerometer walk: " << Naw << " m/s^3/sqrt(Hz)" << endl; + + mpImuCalib = new IMU::Calib(Tbc,Ng*sf,Na*sf,Ngw/sf,Naw/sf); + + mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib); + + + return true; +} + +/** + * @brief 设置局部建图器 + * @param pLocalMapper 局部地图类 + */ +void Tracking::SetLocalMapper(LocalMapping *pLocalMapper) +{ + mpLocalMapper=pLocalMapper; +} + +/** + * @brief 设置回环器 + * @param pLoopClosing 回环类 + */ +void Tracking::SetLoopClosing(LoopClosing *pLoopClosing) +{ + mpLoopClosing=pLoopClosing; +} + +/** + * @brief 设置显示器 + * @param pViewer 显示类 + */ +void Tracking::SetViewer(Viewer *pViewer) +{ + mpViewer=pViewer; +} + +// 一步一步进行 +void Tracking::SetStepByStep(bool bSet) +{ + bStepByStep = bSet; +} + +bool Tracking::GetStepByStep() +{ + return bStepByStep; +} + + +/** + * @brief 输入左右目图像，可以为RGB、BGR、RGBA、GRAY + * 1、将图像转为mImGray和imGrayRight并初始化mCurrentFrame + * 2、进行tracking过程 + * 输出世界坐标系到该帧相机坐标系的变换矩阵 + * @param imRectLeft 左图 + * @param imRectRight 右图 + * @param timestamp 时间戳 + * @param filename 文件名字，貌似调试用的 + */ +Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double ×tamp, string filename) +{ + //cout << "GrabImageStereo" << endl; + + mImGray = imRectLeft; + cv::Mat imGrayRight = imRectRight; + mImRight = imRectRight; + + // step 1 ：将RGB或RGBA图像转为灰度图像 + if(mImGray.channels()==3) + { + //cout << "Image with 3 channels" << endl; + if(mbRGB) + { + cvtColor(mImGray,mImGray,cv::COLOR_RGB2GRAY); + cvtColor(imGrayRight,imGrayRight,cv::COLOR_RGB2GRAY); + } + else + { + cvtColor(mImGray,mImGray,cv::COLOR_BGR2GRAY); + cvtColor(imGrayRight,imGrayRight,cv::COLOR_BGR2GRAY); + } + } + // 这里考虑得十分周全,甚至连四通道的图像都考虑到了 + else if(mImGray.channels()==4) + { + //cout << "Image with 4 channels" << endl; + if(mbRGB) + { + cvtColor(mImGray,mImGray,cv::COLOR_RGBA2GRAY); + cvtColor(imGrayRight,imGrayRight,cv::COLOR_RGBA2GRAY); + } + else + { + cvtColor(mImGray,mImGray,cv::COLOR_BGRA2GRAY); + cvtColor(imGrayRight,imGrayRight,cv::COLOR_BGRA2GRAY); + } + } + + //cout << "Incoming frame creation" << endl; + // 双目模式，注意跟两个相机模式区分开 + if (mSensor == System::STEREO && !mpCamera2) + mCurrentFrame = Frame( + mImGray, // 左目图像 + imGrayRight, // 右目图像 + timestamp, // 时间戳 + mpORBextractorLeft, // 左目特征提取器 + mpORBextractorRight, // 右目特征提取器 + mpORBVocabulary, // 字典 + mK, // 内参矩阵 + mDistCoef, // 去畸变参数 + mbf, // 基线长度 + mThDepth, // 远点,近点的区分阈值 + mpCamera); // 相机模型 + else if(mSensor == System::STEREO && mpCamera2) + mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth,mpCamera,mpCamera2,mTlr); + else if(mSensor == System::IMU_STEREO && !mpCamera2) + mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth,mpCamera,&mLastFrame,*mpImuCalib); + else if(mSensor == System::IMU_STEREO && mpCamera2) + mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth,mpCamera,mpCamera2,mTlr,&mLastFrame,*mpImuCalib); + + //cout << "Incoming frame ended" << endl; + + mCurrentFrame.mNameFile = filename; + mCurrentFrame.mnDataset = mnNumDataset; + +#ifdef REGISTER_TIMES + vdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext); + vdStereoMatch_ms.push_back(mCurrentFrame.mTimeStereoMatch); +#endif + + //cout << "Tracking start" << endl; + // Step 2 ：跟踪 + Track(); + //cout << "Tracking end" << endl; + + // 返回位姿 + return mCurrentFrame.GetPose(); +} + +/** + * @brief 输入左目RGB或RGBA图像和深度图 + * 1、将图像转为mImGray和imDepth并初始化mCurrentFrame + * 2、进行tracking过程 + * 输出世界坐标系到该帧相机坐标系的变换矩阵 + * @param imRGB 彩色图 + * @param imD 深度图 + * @param timestamp 时间戳 + * @param filename 文件名字，貌似调试用的 + */ +Sophus::SE3f Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp, string filename) +{ + mImGray = imRGB; + cv::Mat imDepth = imD; + + // step 1：将RGB或RGBA图像转为灰度图像 + if(mImGray.channels()==3) + { + if(mbRGB) + cvtColor(mImGray,mImGray,cv::COLOR_RGB2GRAY); + else + cvtColor(mImGray,mImGray,cv::COLOR_BGR2GRAY); + } + else if(mImGray.channels()==4) + { + if(mbRGB) + cvtColor(mImGray,mImGray,cv::COLOR_RGBA2GRAY); + else + cvtColor(mImGray,mImGray,cv::COLOR_BGRA2GRAY); + } + + // Step 2 ：将深度相机的disparity转为Depth , 也就是转换成为真正尺度下的深度 + if((fabs(mDepthMapFactor-1.0f)>1e-5) && imDepth.type()!=CV_32F) + imDepth.convertTo(imDepth,CV_32F,mDepthMapFactor); + + // Step 3：构造Frame + if (mSensor == System::RGBD) + mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth,mpCamera); + else if(mSensor == System::IMU_RGBD) + mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth,mpCamera,&mLastFrame,*mpImuCalib); + + mCurrentFrame.mNameFile = filename; + mCurrentFrame.mnDataset = mnNumDataset; + +#ifdef REGISTER_TIMES + vdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext); +#endif + // Step 4：跟踪 + Track(); + + // 返回当前帧的位姿 + return mCurrentFrame.GetPose(); +} + +/** + * @brief 输入左目RGB或RGBA图像，输出世界坐标系到该帧相机坐标系的变换矩阵 + * + * @param im 图像 + * @param timestamp 时间戳 + * @param filename 文件名字，貌似调试用的 + * + * Step 1 ：将彩色图像转为灰度图像 + * Step 2 ：构造Frame + * Step 3 ：跟踪 + */ +Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp, string filename) +{ + mImGray = im; + // Step 1 ：将彩色图像转为灰度图像 + // 若图片是3、4通道的彩色图，还需要转化成单通道灰度图 + if(mImGray.channels()==3) + { + if(mbRGB) + cvtColor(mImGray,mImGray,cv::COLOR_RGB2GRAY); + else + cvtColor(mImGray,mImGray,cv::COLOR_BGR2GRAY); + } + else if(mImGray.channels()==4) + { + if(mbRGB) + cvtColor(mImGray,mImGray,cv::COLOR_RGBA2GRAY); + else + cvtColor(mImGray,mImGray,cv::COLOR_BGRA2GRAY); + } + + // Step 2 ：构造Frame类 + if (mSensor == System::MONOCULAR) + { + if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET ||(lastID - initID) < mMaxFrames) + mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth); + else + mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth); + } + else if(mSensor == System::IMU_MONOCULAR) + { + // 判断该帧是不是初始化 + if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET) //没有成功初始化的前一个状态就是NO_IMAGES_YET + { + mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib); + } + else + mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib); + } + + // t0存储未初始化时的第1帧图像时间戳 + if (mState==NO_IMAGES_YET) + t0=timestamp; + + mCurrentFrame.mNameFile = filename; + mCurrentFrame.mnDataset = mnNumDataset; + +#ifdef REGISTER_TIMES + vdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext); +#endif + + lastID = mCurrentFrame.mnId; + // Step 3 ：跟踪 + Track(); + + // 返回当前帧的位姿 + return mCurrentFrame.GetPose(); +} + +/** + * @brief 将imu数据存放在mlQueueImuData的list链表里 + * @param[in] imuMeasurement + */ +void Tracking::GrabImuData(const IMU::Point &imuMeasurement) +{ + unique_lock lock(mMutexImuQueue); + mlQueueImuData.push_back(imuMeasurement); +} + +/** + * @brief 预积分，对于一个帧有两种预积分，一种是相对于上一帧，一种是相对于上一个关键帧 + */ +void Tracking::PreintegrateIMU() +{ + // Step 1.拿到两两帧之间待处理的预积分数据，组成一个集合 + // 上一帧不存在,说明两帧之间没有imu数据，不进行预积分 + if(!mCurrentFrame.mpPrevFrame) + { + Verbose::PrintMess("non prev frame ", Verbose::VERBOSITY_NORMAL); + mCurrentFrame.setIntegrated(); + return; + } + + mvImuFromLastFrame.clear(); + mvImuFromLastFrame.reserve(mlQueueImuData.size()); + // 没有imu数据,不进行预积分 + if(mlQueueImuData.size() == 0) + { + Verbose::PrintMess("Not IMU data in mlQueueImuData!!", Verbose::VERBOSITY_NORMAL); + mCurrentFrame.setIntegrated(); + return; + } + + while(true) + { + // 数据还没有时,会等待一段时间,直到mlQueueImuData中有imu数据.一开始不需要等待 + bool bSleep = false; + { + unique_lock lock(mMutexImuQueue); + if(!mlQueueImuData.empty()) + { + // 拿到第一个imu数据作为起始数据 + IMU::Point* m = &mlQueueImuData.front(); + cout.precision(17); + // imu起始数据会比当前帧的前一帧时间戳早,如果相差0.001则舍弃这个imu数据 + if(m->tmTimeStamp-mImuPer) + { + mlQueueImuData.pop_front(); + } + // 同样最后一个的imu数据时间戳也不能理当前帧时间间隔多余0.001 + else if(m->tmTimeStamp; + // 设当前时刻imu的加速度a0，下一时刻加速度a1，时间间隔tab 为t10，tini t0p + // 正常情况下时为了求上一帧到当前时刻imu的一个平均加速度，但是imu时间不会正好落在上一帧的时刻，需要做补偿，要求得a0时刻到上一帧这段时间加速度的改变量 + // 有了这个改变量将其加到a0上之后就可以表示上一帧时的加速度了。其中a0 - (a1-a0)*(tini/tab) 为上一帧时刻的加速度再加上a1 之后除以2就为这段时间的加速度平均值 + // 其中tstep表示a1到上一帧的时间间隔，a0 - (a1-a0)*(tini/tab)这个式子中tini可以是正也可以是负表示时间上的先后，(a1-a0)也是一样，多种情况下这个式子依然成立 + acc = (mvImuFromLastFrame[i].a+mvImuFromLastFrame[i+1].a- + (mvImuFromLastFrame[i+1].a-mvImuFromLastFrame[i].a)*(tini/tab))*0.5f; + // 计算过程类似加速度 + angVel = (mvImuFromLastFrame[i].w+mvImuFromLastFrame[i+1].w- + (mvImuFromLastFrame[i+1].w-mvImuFromLastFrame[i].w)*(tini/tab))*0.5f; + tstep = mvImuFromLastFrame[i+1].t-mCurrentFrame.mpPrevFrame->mTimeStamp; + } + else if(i<(n-1)) + { + // 中间的数据不存在帧的干扰，正常计算 + acc = (mvImuFromLastFrame[i].a+mvImuFromLastFrame[i+1].a)*0.5f; + angVel = (mvImuFromLastFrame[i].w+mvImuFromLastFrame[i+1].w)*0.5f; + tstep = mvImuFromLastFrame[i+1].t-mvImuFromLastFrame[i].t; + } + // 直到倒数第二个imu时刻时，计算过程跟第一时刻类似，都需要考虑帧与imu时刻的关系 + else if((i>0) && (i==(n-1))) + { + float tab = mvImuFromLastFrame[i+1].t-mvImuFromLastFrame[i].t; + float tend = mvImuFromLastFrame[i+1].t-mCurrentFrame.mTimeStamp; + acc = (mvImuFromLastFrame[i].a+mvImuFromLastFrame[i+1].a- + (mvImuFromLastFrame[i+1].a-mvImuFromLastFrame[i].a)*(tend/tab))*0.5f; + angVel = (mvImuFromLastFrame[i].w+mvImuFromLastFrame[i+1].w- + (mvImuFromLastFrame[i+1].w-mvImuFromLastFrame[i].w)*(tend/tab))*0.5f; + tstep = mCurrentFrame.mTimeStamp-mvImuFromLastFrame[i].t; + } + // 就两个数据时使用第一个时刻的，这种情况应该没有吧，，回头应该试试看 + else if((i==0) && (i==(n-1))) + { + acc = mvImuFromLastFrame[i].a; + angVel = mvImuFromLastFrame[i].w; + tstep = mCurrentFrame.mTimeStamp-mCurrentFrame.mpPrevFrame->mTimeStamp; + } + // Step 3.依次进行预积分计算 + // 应该是必存在的吧，一个是相对上一关键帧，一个是相对上一帧 + if (!mpImuPreintegratedFromLastKF) + cout << "mpImuPreintegratedFromLastKF does not exist" << endl; + mpImuPreintegratedFromLastKF->IntegrateNewMeasurement(acc,angVel,tstep); + pImuPreintegratedFromLastFrame->IntegrateNewMeasurement(acc,angVel,tstep); + } + + // 记录当前预积分的图像帧 + mCurrentFrame.mpImuPreintegratedFrame = pImuPreintegratedFromLastFrame; + mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF; + mCurrentFrame.mpLastKeyFrame = mpLastKeyFrame; + + mCurrentFrame.setIntegrated(); + + //Verbose::PrintMess("Preintegration is finished!! ", Verbose::VERBOSITY_DEBUG); +} + +/** + * @brief 跟踪不成功的时候，用初始化好的imu数据做跟踪处理，通过IMU预测状态 + * 两个地方用到： + * 1. 匀速模型计算速度,但并没有给当前帧位姿赋值； + * 2. 跟踪丢失时不直接判定丢失，通过这个函数预测当前帧位姿看看能不能拽回来，代替纯视觉中的重定位 + * + * @return true + * @return false + */ +bool Tracking::PredictStateIMU() +{ + if(!mCurrentFrame.mpPrevFrame) + { + Verbose::PrintMess("No last frame", Verbose::VERBOSITY_NORMAL); + return false; + } + + // 总结下都在什么时候地图更新，也就是mbMapUpdated为true + // 1. 回环或融合 + // 2. 局部地图LocalBundleAdjustment + // 3. IMU三阶段的初始化 + // 下面的代码流程一模一样，只不过计算时相对的帧不同，地图有更新后相对于上一关键帧做的，反之相对于上一帧 + // 地图更新后会更新关键帧与MP，所以相对于关键帧更准 + // 而没更新的话，距离上一帧更近，计算起来误差更小 + // 地图更新时，并且上一个图像关键帧存在 + if(mbMapUpdated && mpLastKeyFrame) + { + const Eigen::Vector3f twb1 = mpLastKeyFrame->GetImuPosition(); + const Eigen::Matrix3f Rwb1 = mpLastKeyFrame->GetImuRotation(); + const Eigen::Vector3f Vwb1 = mpLastKeyFrame->GetVelocity(); + + const Eigen::Vector3f Gz(0, 0, -IMU::GRAVITY_VALUE); + const float t12 = mpImuPreintegratedFromLastKF->dT; + + // 计算当前帧在世界坐标系的位姿,原理都是用预积分的位姿（预积分的值不会变化）与上一帧的位姿（会迭代变化）进行更新 + // 旋转 R_wb2 = R_wb1 * R_b1b2 + Eigen::Matrix3f Rwb2 = IMU::NormalizeRotation(Rwb1 * mpImuPreintegratedFromLastKF->GetDeltaRotation(mpLastKeyFrame->GetImuBias())); + // 位移 + Eigen::Vector3f twb2 = twb1 + Vwb1*t12 + 0.5f*t12*t12*Gz+ Rwb1*mpImuPreintegratedFromLastKF->GetDeltaPosition(mpLastKeyFrame->GetImuBias()); + // 速度 + Eigen::Vector3f Vwb2 = Vwb1 + t12*Gz + Rwb1 * mpImuPreintegratedFromLastKF->GetDeltaVelocity(mpLastKeyFrame->GetImuBias()); + // 设置当前帧的世界坐标系的相机位姿 + mCurrentFrame.SetImuPoseVelocity(Rwb2,twb2,Vwb2); + + // 记录bias + mCurrentFrame.mImuBias = mpLastKeyFrame->GetImuBias(); + mCurrentFrame.mPredBias = mCurrentFrame.mImuBias; + return true; + } + // 地图未更新时 + else if(!mbMapUpdated) + { + const Eigen::Vector3f twb1 = mLastFrame.GetImuPosition(); + const Eigen::Matrix3f Rwb1 = mLastFrame.GetImuRotation(); + const Eigen::Vector3f Vwb1 = mLastFrame.GetVelocity(); + const Eigen::Vector3f Gz(0, 0, -IMU::GRAVITY_VALUE); + // mpImuPreintegratedFrame是当前帧上一帧，不一定是关键帧 + const float t12 = mCurrentFrame.mpImuPreintegratedFrame->dT; + + Eigen::Matrix3f Rwb2 = IMU::NormalizeRotation(Rwb1 * mCurrentFrame.mpImuPreintegratedFrame->GetDeltaRotation(mLastFrame.mImuBias)); + Eigen::Vector3f twb2 = twb1 + Vwb1*t12 + 0.5f*t12*t12*Gz+ Rwb1 * mCurrentFrame.mpImuPreintegratedFrame->GetDeltaPosition(mLastFrame.mImuBias); + Eigen::Vector3f Vwb2 = Vwb1 + t12*Gz + Rwb1 * mCurrentFrame.mpImuPreintegratedFrame->GetDeltaVelocity(mLastFrame.mImuBias); + + mCurrentFrame.SetImuPoseVelocity(Rwb2,twb2,Vwb2); + + mCurrentFrame.mImuBias = mLastFrame.mImuBias; + mCurrentFrame.mPredBias = mCurrentFrame.mImuBias; + return true; + } + else + cout << "not IMU prediction!!" << endl; + + return false; +} + +void Tracking::ResetFrameIMU() +{ + // TODO To implement... +} + +/** + * @brief 跟踪过程，包括恒速模型跟踪、参考关键帧跟踪、局部地图跟踪 + * track包含两部分：估计运动、跟踪局部地图 + * + * Step 1：初始化 + * Step 2：跟踪 + * Step 3：记录位姿信息，用于轨迹复现 + */ +void Tracking::Track() +{ + + if (bStepByStep) + { + std::cout << "Tracking: Waiting to the next step" << std::endl; + while(!mbStep && bStepByStep) + usleep(500); + mbStep = false; + } + + // Step 1 如局部建图里认为IMU有问题，重置当前活跃地图 + if(mpLocalMapper->mbBadImu) + { + cout << "TRACK: Reset map because local mapper set the bad imu flag " << endl; + mpSystem->ResetActiveMap(); + return; + } + + // 从Atlas中取出当前active的地图 + Map* pCurrentMap = mpAtlas->GetCurrentMap(); + if(!pCurrentMap) + { + cout << "ERROR: There is not an active map in the atlas" << endl; + } + + // Step 2 处理时间戳异常的情况 + if(mState!=NO_IMAGES_YET) + { + if(mLastFrame.mTimeStamp>mCurrentFrame.mTimeStamp) + { + // 如果当前图像时间戳比前一帧图像时间戳小，说明出错了，清除imu数据，创建新的子地图 + cerr << "ERROR: Frame with a timestamp older than previous frame detected!" << endl; + unique_lock lock(mMutexImuQueue); + // mlQueueImuData.clear(); + // 创建新地图 + CreateMapInAtlas(); + return; + } + else if(mCurrentFrame.mTimeStamp>mLastFrame.mTimeStamp+1.0) + { + // cout << mCurrentFrame.mTimeStamp << ", " << mLastFrame.mTimeStamp << endl; + // cout << "id last: " << mLastFrame.mnId << " id curr: " << mCurrentFrame.mnId << endl; + // 如果当前图像时间戳和前一帧图像时间戳大于1s，说明时间戳明显跳变了，重置地图后直接返回 + //根据是否是imu模式,进行imu的补偿 + if(mpAtlas->isInertial()) + { + // 如果当前地图imu成功初始化 + if(mpAtlas->isImuInitialized()) + { + cout << "Timestamp jump detected. State set to LOST. Reseting IMU integration..." << endl; + // IMU完成第3次初始化（在localmapping线程里） + if(!pCurrentMap->GetIniertialBA2()) + { + // 如果当前子图中imu没有经过BA2，重置active地图，也就是之前的数据不要了 + mpSystem->ResetActiveMap(); + } + else + { + // 如果当前子图中imu进行了BA2，重新创建新的子图，保存当前地图 + CreateMapInAtlas(); + } + } + else + { + // 如果当前子图中imu还没有初始化，重置active地图 + cout << "Timestamp jump detected, before IMU initialization. Reseting..." << endl; + mpSystem->ResetActiveMap(); + } + return; + } + + } + } + + // Step 3 IMU模式下设置IMU的Bias参数,还要保证上一帧存在 + if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && mpLastKeyFrame) + mCurrentFrame.SetNewBias(mpLastKeyFrame->GetImuBias()); // 使用上一帧的bias作为当前帧的初值 + + if(mState==NO_IMAGES_YET) + { + mState = NOT_INITIALIZED; + } + + mLastProcessedState=mState; + // Step 4 IMU模式且没有创建地图的情况下对IMU数据进行预积分 + if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && !mbCreatedMap) + { +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartPreIMU = std::chrono::steady_clock::now(); +#endif + // IMU数据进行预积分 + PreintegrateIMU(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndPreIMU = std::chrono::steady_clock::now(); + + double timePreImu = std::chrono::duration_cast >(time_EndPreIMU - time_StartPreIMU).count(); + vdIMUInteg_ms.push_back(timePreImu); +#endif + + } + mbCreatedMap = false; + + // Get Map Mutex -> Map cannot be changed + // 地图更新时加锁。保证地图不会发生变化 + // 疑问:这样子会不会影响地图的实时更新? + // 回答：主要耗时在构造帧中特征点的提取和匹配部分,在那个时候地图是没有被上锁的,有足够的时间更新地图 + unique_lock lock(pCurrentMap->mMutexMapUpdate); + + mbMapUpdated = false; + + // 判断地图id是否更新了 + int nCurMapChangeIndex = pCurrentMap->GetMapChangeIndex(); + int nMapChangeIndex = pCurrentMap->GetLastMapChange(); + if(nCurMapChangeIndex>nMapChangeIndex) + { + // 检测到地图更新了 + pCurrentMap->SetLastMapChange(nCurMapChangeIndex); + mbMapUpdated = true; + } + + // Step 5 初始化 + if(mState==NOT_INITIALIZED) + { + if(mSensor==System::STEREO || mSensor==System::RGBD || mSensor==System::IMU_STEREO || mSensor==System::IMU_RGBD) + { + // 双目RGBD相机的初始化共用一个函数 + StereoInitialization(); + } + else + { + // 单目初始化 + MonocularInitialization(); + } + + //mpFrameDrawer->Update(this); + + if(mState!=OK) // If rightly initialized, mState=OK + { + // 如果没有成功初始化，直接返回 + mLastFrame = Frame(mCurrentFrame); + return; + } + + if(mpAtlas->GetAllMaps().size() == 1) + { + // 如果当前地图是第一个地图，记录当前帧id为第一帧 + mnFirstFrameId = mCurrentFrame.mnId; + } + } + else + { + // System is initialized. Track Frame. + // Step 6 系统成功初始化，下面是具体跟踪过程 + bool bOK; + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartPosePred = std::chrono::steady_clock::now(); +#endif + + // Initial camera pose estimation using motion model or relocalization (if tracking is lost) + // mbOnlyTracking等于false表示正常SLAM模式（定位+地图更新），mbOnlyTracking等于true表示仅定位模式 + // tracking 类构造时默认为false。在viewer中有个开关ActivateLocalizationMode，可以控制是否开启mbOnlyTracking + if(!mbOnlyTracking) + { + + // State OK + // Local Mapping is activated. This is the normal behaviour, unless + // you explicitly activate the "only tracking" mode. + // 跟踪进入正常SLAM模式，有地图更新 + + // 如果正常跟踪 + if(mState==OK) + { + + // Local Mapping might have changed some MapPoints tracked in last frame + // Step 6.1 检查并更新上一帧被替换的MapPoints + // 局部建图线程则可能会对原有的地图点进行替换.在这里进行检查 + CheckReplacedInLastFrame(); + + // Step 6.2 运动模型是空的并且imu未初始化或刚完成重定位，跟踪参考关键帧；否则恒速模型跟踪 + // 第一个条件,如果运动模型为空并且imu未初始化,说明是刚开始第一帧跟踪，或者已经跟丢了。 + // 第二个条件,如果当前帧紧紧地跟着在重定位的帧的后面，我们用重定位帧来恢复位姿 + // mnLastRelocFrameId 上一次重定位的那一帧 + if((!mbVelocity && !pCurrentMap->isImuInitialized()) || mCurrentFrame.mnIdKeyFramesInMap()>10) + { + // cout << "KF in map: " << pCurrentMap->KeyFramesInMap() << endl; + // 条件1：当前地图中关键帧数目较多（大于10） + // 条件2（隐藏条件）：当前帧距离上次重定位帧超过1s（说明还比较争气，值的救）或者非IMU模式 + // 同时满足条件1，2，则将状态标记为RECENTLY_LOST，后面会结合IMU预测的位姿看看能不能拽回来 + mState = RECENTLY_LOST; + // 记录丢失时间 + mTimeStampLost = mCurrentFrame.mTimeStamp; + } + else + { + mState = LOST; + } + } + } + else // 跟踪不正常按照下面处理 + { + // 如果是RECENTLY_LOST状态 + if (mState == RECENTLY_LOST) + { + Verbose::PrintMess("Lost for a short time", Verbose::VERBOSITY_NORMAL); + // bOK先置为true + bOK = true; + if((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)) + { + // IMU模式下可以用IMU来预测位姿，看能否拽回来 + // Step 6.4 如果当前地图中IMU已经成功初始化，就用IMU数据预测位姿 + if(pCurrentMap->isImuInitialized()) + PredictStateIMU(); + else + bOK = false; + + // 如果IMU模式下当前帧距离跟丢帧超过5s还没有找回（time_recently_lost默认为5s） + // 放弃了，将RECENTLY_LOST状态改为LOST + if (mCurrentFrame.mTimeStamp-mTimeStampLost>time_recently_lost) + { + mState = LOST; + Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL); + bOK=false; + } + } + else + { + // Step 6.5 纯视觉模式则进行重定位。主要是BOW搜索，EPnP求解位姿 + // Relocalization + bOK = Relocalization(); + //std::cout << "mCurrentFrame.mTimeStamp:" << to_string(mCurrentFrame.mTimeStamp) << std::endl; + //std::cout << "mTimeStampLost:" << to_string(mTimeStampLost) << std::endl; + if(mCurrentFrame.mTimeStamp-mTimeStampLost>3.0f && !bOK) + { + // 纯视觉模式下重定位失败，状态为LOST + mState = LOST; + Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL); + bOK=false; + } + } + } + else if (mState == LOST) // 上一帧为最近丢失且重定位失败时 + { + // Step 6.6 如果是LOST状态 + // 开启一个新地图 + Verbose::PrintMess("A new map is started...", Verbose::VERBOSITY_NORMAL); + + if (pCurrentMap->KeyFramesInMap()<10) + { + // 当前地图中关键帧数目小于10，重置当前地图 + mpSystem->ResetActiveMap(); + Verbose::PrintMess("Reseting current map...", Verbose::VERBOSITY_NORMAL); + }else + CreateMapInAtlas(); // 当前地图中关键帧数目超过10，创建新地图 + // 干掉上一个关键帧 + if(mpLastKeyFrame) + mpLastKeyFrame = static_cast(NULL); + + Verbose::PrintMess("done", Verbose::VERBOSITY_NORMAL); + + return; + } + } + + } + else // 纯定位模式 + { + // Localization Mode: Local Mapping is deactivated (TODO Not available in inertial mode) + // 只进行跟踪tracking，局部地图不工作 + if(mState==LOST) + { + if(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) + Verbose::PrintMess("IMU. State LOST", Verbose::VERBOSITY_NORMAL); + // Step 6.1 LOST状态进行重定位 + bOK = Relocalization(); + } + else + { + // mbVO是mbOnlyTracking为true时的才有的一个变量 + // mbVO为false表示此帧匹配了很多的MapPoints，跟踪很正常 (注意有点反直觉) + // mbVO为true表明此帧匹配了很少的MapPoints，少于10个，要跟丢 + if(!mbVO) + { + // In last frame we tracked enough MapPoints in the map + // Step 6.2 如果跟踪状态正常，使用恒速模型或参考关键帧跟踪 + if(mbVelocity) + { + // 优先使用恒速模型跟踪 + bOK = TrackWithMotionModel(); + } + else + { + // 如果恒速模型不被满足,那么就只能够通过参考关键帧来跟踪 + bOK = TrackReferenceKeyFrame(); + } + } + else + { + // In last frame we tracked mainly "visual odometry" points. + + // We compute two camera poses, one from motion model and one doing relocalization. + // If relocalization is sucessfull we choose that solution, otherwise we retain + // the "visual odometry" solution. + + // mbVO为true，表明此帧匹配了很少（小于10）的地图点，要跟丢的节奏，既做跟踪又做重定位 + + // MM=Motion Model,通过运动模型进行跟踪的结果 + bool bOKMM = false; + // 通过重定位方法来跟踪的结果 + bool bOKReloc = false; + // 运动模型中构造的地图点 + vector vpMPsMM; + // 在追踪运动模型后发现的外点 + vector vbOutMM; + // 运动模型得到的位姿 + Sophus::SE3f TcwMM; + // Step 6.3 当运动模型有效的时候,根据运动模型计算位姿 + if(mbVelocity) + { + bOKMM = TrackWithMotionModel(); + // 将恒速模型跟踪结果暂存到这几个变量中，因为后面重定位会改变这些变量 + vpMPsMM = mCurrentFrame.mvpMapPoints; + vbOutMM = mCurrentFrame.mvbOutlier; + TcwMM = mCurrentFrame.GetPose(); + } + // Step 6.4 使用重定位的方法来得到当前帧的位姿 + bOKReloc = Relocalization(); + + // Step 6.5 根据前面的恒速模型、重定位结果来更新状态 + if(bOKMM && !bOKReloc) + { + // 恒速模型成功、重定位失败，重新使用之前暂存的恒速模型结果 + mCurrentFrame.SetPose(TcwMM); + mCurrentFrame.mvpMapPoints = vpMPsMM; + mCurrentFrame.mvbOutlier = vbOutMM; + + // 如果当前帧匹配的3D点很少，增加当前可视地图点的被观测次数 + if(mbVO) + { + // 更新当前帧的地图点被找到次数，注意跟观测次数区分，这是两个概念 + for(int i =0; iIncreaseFound(); + } + } + } + } + else if(bOKReloc) + { + // 只要重定位成功整个跟踪过程正常进行（重定位与跟踪，更相信重定位） + mbVO = false; + } + // 有一个成功我们就认为执行成功了 + bOK = bOKReloc || bOKMM; + } + } + } + // 将最新的关键帧作为当前帧的参考关键帧 + // mpReferenceKF先是上一时刻的参考关键帧，如果当前为新关键帧则变成当前关键帧，如果不是新的关键帧则先为上一帧的参考关键帧，而后经过更新局部关键帧重新确定 + if(!mCurrentFrame.mpReferenceKF) + mCurrentFrame.mpReferenceKF = mpReferenceKF; + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndPosePred = std::chrono::steady_clock::now(); + + double timePosePred = std::chrono::duration_cast >(time_EndPosePred - time_StartPosePred).count(); + vdPosePred_ms.push_back(timePosePred); +#endif + + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartLMTrack = std::chrono::steady_clock::now(); +#endif + + // Step 7 在跟踪得到当前帧初始姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿 + // 前面只是跟踪一帧得到初始位姿，这里搜索局部关键帧、局部地图点，和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化 + // 在帧间匹配得到初始的姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿 + // local map:当前帧、当前帧的MapPoints、当前关键帧与其它关键帧共视关系 + // 前面主要是两两跟踪（恒速模型跟踪上一帧、跟踪参考帧），这里搜索局部关键帧后搜集所有局部MapPoints， + // 然后将局部MapPoints和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化 + // If we have an initial estimation of the camera pose and matching. Track the local map. + if(!mbOnlyTracking) + { + if(bOK) + { + // 局部地图跟踪 + bOK = TrackLocalMap(); + } + if(!bOK) + cout << "Fail to track local map!" << endl; + } + else + { + // mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve + // a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes + // the camera we will use the local map again. + if(bOK && !mbVO) + bOK = TrackLocalMap(); + } + // 到此为止跟踪确定位姿阶段结束，下面开始做收尾工作和为下一帧做准备 + + // 查看到此为止时的两个状态变化 + // bOK的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---true -->OK 1 跟踪局部地图成功 + // \ \ \---局部地图跟踪失败---false + // \ \---当前帧跟踪失败---false + // \---上一帧跟踪失败---重定位成功---局部地图跟踪成功---true -->OK 2 重定位 + // \ \---局部地图跟踪失败---false + // \---重定位失败---false + + // + // mState的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---OK -->OK 1 跟踪局部地图成功 + // \ \ \---局部地图跟踪失败---OK -->OK 3 正常跟踪 + // \ \---当前帧跟踪失败---非OK + // \---上一帧跟踪失败---重定位成功---局部地图跟踪成功---非OK + // \ \---局部地图跟踪失败---非OK + // \---重定位失败---非OK（传不到这里，因为直接return了） + // 由上图可知当前帧的状态OK的条件是跟踪局部地图成功，重定位或正常跟踪都可 + // Step 8 根据上面的操作来判断是否追踪成功 + if(bOK) + // 此时还OK才说明跟踪成功了 + mState = OK; + else if (mState == OK) // 由上图可知只有当第一阶段跟踪成功，但第二阶段局部地图跟踪失败时执行 + { + // 状态变为最近丢失 + if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) + { + Verbose::PrintMess("Track lost for less than one second...", Verbose::VERBOSITY_NORMAL); + if(!pCurrentMap->isImuInitialized() || !pCurrentMap->GetIniertialBA2()) + { + // IMU模式下IMU没有成功初始化或者没有完成IMU BA，则重置当前地图 + cout << "IMU is not or recently initialized. Reseting active map..." << endl; + mpSystem->ResetActiveMap(); + } + + mState=RECENTLY_LOST; + } + else + mState=RECENTLY_LOST; // 上一个版本这里直接判定丢失 LOST + + // 被注释掉了，记录丢失时间 + /*if(mCurrentFrame.mnId>mnLastRelocFrameId+mMaxFrames) + {*/ + mTimeStampLost = mCurrentFrame.mTimeStamp; + //} + } + + // Save frame if recent relocalization, since they are used for IMU reset (as we are making copy, it shluld be once mCurrFrame is completely modified) + // 这段貌似没啥作用 + if((mCurrentFrame.mnId<(mnLastRelocFrameId+mnFramesToResetIMU)) && (mCurrentFrame.mnId > mnFramesToResetIMU) && + (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && pCurrentMap->isImuInitialized()) + { + // TODO check this situation + Verbose::PrintMess("Saving pointer to frame. imu needs reset...", Verbose::VERBOSITY_NORMAL); + Frame* pF = new Frame(mCurrentFrame); + pF->mpPrevFrame = new Frame(mLastFrame); + + // Load preintegration + pF->mpImuPreintegratedFrame = new IMU::Preintegrated(mCurrentFrame.mpImuPreintegratedFrame); + } + // 下面代码没有用 + if(pCurrentMap->isImuInitialized()) + { + if(bOK) + { + // 当前帧距离上次重定位帧刚好等于1s，重置（还未实现 TODO） + if(mCurrentFrame.mnId==(mnLastRelocFrameId+mnFramesToResetIMU)) + { + cout << "RESETING FRAME!!!" << endl; + ResetFrameIMU(); + } + else if(mCurrentFrame.mnId>(mnLastRelocFrameId+30)) + mLastBias = mCurrentFrame.mImuBias; // 没啥用，后面会重新赋值后传给普通帧 + } + } + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndLMTrack = std::chrono::steady_clock::now(); + + double timeLMTrack = std::chrono::duration_cast >(time_EndLMTrack - time_StartLMTrack).count(); + vdLMTrack_ms.push_back(timeLMTrack); +#endif + + // Update drawer + // 更新显示线程中的图像、特征点、地图点等信息 + mpFrameDrawer->Update(this); + if(mCurrentFrame.isSet()) + mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.GetPose()); + + // 查看到此为止时的两个状态变化 + // bOK的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---true + // \ \ \---局部地图跟踪失败---false + // \ \---当前帧跟踪失败---false + // \---上一帧跟踪失败---重定位成功---局部地图跟踪成功---true + // \ \---局部地图跟踪失败---false + // \---重定位失败---false + + // mState的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---OK + // \ \ \---局部地图跟踪失败---非OK（IMU时为RECENTLY_LOST） + // \ \---当前帧跟踪失败---非OK(地图超过10个关键帧时 RECENTLY_LOST) + // \---上一帧跟踪失败(RECENTLY_LOST)---重定位成功---局部地图跟踪成功---OK + // \ \ \---局部地图跟踪失败---LOST + // \ \---重定位失败---LOST（传不到这里，因为直接return了） + // \--上一帧跟踪失败(LOST)--LOST（传不到这里，因为直接return了） + + // Step 9 如果跟踪成功 或 最近刚刚跟丢，更新速度，清除无效地图点，按需创建关键帧 + if(bOK || mState==RECENTLY_LOST) + { + // Update motion model + // Step 9.1 更新恒速运动模型 TrackWithMotionModel 中的mVelocity + if(mLastFrame.isSet() && mCurrentFrame.isSet()) + { + Sophus::SE3f LastTwc = mLastFrame.GetPose().inverse(); + // mVelocity = Tcl = Tcw * Twl,表示上一帧到当前帧的变换， 其中 Twl = LastTwc + mVelocity = mCurrentFrame.GetPose() * LastTwc; + mbVelocity = true; + } + else { + // 否则没有速度 + mbVelocity = false; + } + + // 使用IMU积分的位姿显示 + if(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) + mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.GetPose()); + + // Clean VO matches + // Step 9.2 清除观测不到的地图点 + for(int i=0; iObservations()<1) + { + mCurrentFrame.mvbOutlier[i] = false; + mCurrentFrame.mvpMapPoints[i]=static_cast(NULL); + } + } + + // Delete temporal MapPoints + // Step 9.3 清除恒速模型跟踪中 UpdateLastFrame中为当前帧临时添加的MapPoints（仅双目和rgbd） + // 上个步骤中只是在当前帧中将这些MapPoints剔除，这里从MapPoints数据库中删除 + // 临时地图点仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了，没有添加到地图中 + for(list::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++) + { + MapPoint* pMP = *lit; + delete pMP; + } + // 这里不仅仅是清除mlpTemporalPoints，通过delete pMP还删除了指针指向的MapPoint + // 不能够直接执行这个是因为其中存储的都是指针,之前的操作都是为了避免内存泄露 + mlpTemporalPoints.clear(); + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartNewKF = std::chrono::steady_clock::now(); +#endif + // 判断是否需要插入关键帧 + bool bNeedKF = NeedNewKeyFrame(); + + // Check if we need to insert a new keyframe + // if(bNeedKF && bOK) + + // Step 9.4 根据条件来判断是否插入关键帧 + // 需要同时满足下面条件1和2 + // 条件1：bNeedKF=true，需要插入关键帧 + // 条件2：bOK=true跟踪成功 或 IMU模式下的RECENTLY_LOST模式且mInsertKFsLost为true + if(bNeedKF && (bOK || (mInsertKFsLost && mState==RECENTLY_LOST && + (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)))) + CreateNewKeyFrame(); // 创建关键帧，对于双目或RGB-D会产生新的地图点 + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndNewKF = std::chrono::steady_clock::now(); + + double timeNewKF = std::chrono::duration_cast >(time_EndNewKF - time_StartNewKF).count(); + vdNewKF_ms.push_back(timeNewKF); +#endif + + // We allow points with high innovation (considererd outliers by the Huber Function) + // pass to the new keyframe, so that bundle adjustment will finally decide + // if they are outliers or not. We don't want next frame to estimate its position + // with those points so we discard them in the frame. Only has effect if lastframe is tracked + // 作者这里说允许在BA中被Huber核函数判断为外点的传入新的关键帧中，让后续的BA来审判他们是不是真正的外点 + // 但是估计下一帧位姿的时候我们不想用这些外点，所以删掉 + + // Step 9.5 删除那些在BA中检测为外点的地图点 + for(int i=0; i(NULL); + } + } + + // Reset if the camera get lost soon after initialization + // Step 10 如果第二阶段跟踪失败，跟踪状态为LOST + if(mState==LOST) + { + // 如果地图中关键帧小于10，重置当前地图，退出当前跟踪 + if(pCurrentMap->KeyFramesInMap()<=10) // 上一个版本这里是5 + { + mpSystem->ResetActiveMap(); + return; + } + if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) + if (!pCurrentMap->isImuInitialized()) + { + // 如果是IMU模式并且还未进行IMU初始化，重置当前地图，退出当前跟踪 + Verbose::PrintMess("Track lost before IMU initialisation, reseting...", Verbose::VERBOSITY_QUIET); + mpSystem->ResetActiveMap(); + return; + } + // 如果地图中关键帧超过10 并且 纯视觉模式 或 虽然是IMU模式但是已经完成IMU初始化了，保存当前地图，创建新的地图 + CreateMapInAtlas(); + + // 新增加了个return + return; + } + // 确保已经设置了参考关键帧 + if(!mCurrentFrame.mpReferenceKF) + mCurrentFrame.mpReferenceKF = mpReferenceKF; + // 保存上一帧的数据,当前帧变上一帧 + mLastFrame = Frame(mCurrentFrame); + } + + // 查看到此为止 + // mState的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---OK + // \ \ \---局部地图跟踪失败---非OK（IMU时为RECENTLY_LOST） + // \ \---当前帧跟踪失败---非OK(地图超过10个关键帧时 RECENTLY_LOST) + // \---上一帧跟踪失败(RECENTLY_LOST)---重定位成功---局部地图跟踪成功---OK + // \ \ \---局部地图跟踪失败---LOST + // \ \---重定位失败---LOST（传不到这里，因为直接return了） + // \--上一帧跟踪失败(LOST)--LOST（传不到这里，因为直接return了） + // last.记录位姿信息，用于轨迹复现 + // Step 11 记录位姿信息，用于最后保存所有的轨迹 + if(mState==OK || mState==RECENTLY_LOST) + { + // Store frame pose information to retrieve the complete camera trajectory afterwards. + // Step 11：记录位姿信息，用于最后保存所有的轨迹 + if(mCurrentFrame.isSet()) + { + // 计算相对姿态Tcr = Tcw * Twr, Twr = Trw^-1 + Sophus::SE3f Tcr_ = mCurrentFrame.GetPose() * mCurrentFrame.mpReferenceKF->GetPoseInverse(); + mlRelativeFramePoses.push_back(Tcr_); + mlpReferences.push_back(mCurrentFrame.mpReferenceKF); + mlFrameTimes.push_back(mCurrentFrame.mTimeStamp); + mlbLost.push_back(mState==LOST); + } + else + { + // This can happen if tracking is lost + // 如果跟踪失败，则相对位姿使用上一次值 + mlRelativeFramePoses.push_back(mlRelativeFramePoses.back()); + mlpReferences.push_back(mlpReferences.back()); + mlFrameTimes.push_back(mlFrameTimes.back()); + mlbLost.push_back(mState==LOST); + } + + } + +#ifdef REGISTER_LOOP + if (Stop()) { + + // Safe area to stop + while(isStopped()) + { + usleep(3000); + } + } +#endif +} + +/* + * @brief 双目和rgbd的地图初始化，比单目简单很多 + * + * 由于具有深度信息，直接生成MapPoints + */ +void Tracking::StereoInitialization() +{ + // 初始化要求当前帧的特征点超过500 + if(mCurrentFrame.N>500) + { + if (mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) + { + if (!mCurrentFrame.mpImuPreintegrated || !mLastFrame.mpImuPreintegrated) + { + cout << "not IMU meas" << endl; + return; + } + + if (!mFastInit && (mCurrentFrame.mpImuPreintegratedFrame->avgA-mLastFrame.mpImuPreintegratedFrame->avgA).norm()<0.5) + { + cout << "not enough acceleration" << endl; + return; + } + + if(mpImuPreintegratedFromLastKF) + delete mpImuPreintegratedFromLastKF; + + mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib); + mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF; + } + + // Set Frame pose to the origin (In case of inertial SLAM to imu) + // 设定初始位姿为单位旋转，0平移，imu模式下设置的是相机位姿 + if (mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) + { + Eigen::Matrix3f Rwb0 = mCurrentFrame.mImuCalib.mTcb.rotationMatrix(); + Eigen::Vector3f twb0 = mCurrentFrame.mImuCalib.mTcb.translation(); + Eigen::Vector3f Vwb0; + Vwb0.setZero(); + mCurrentFrame.SetImuPoseVelocity(Rwb0, twb0, Vwb0); + } + else + mCurrentFrame.SetPose(Sophus::SE3f()); + + // Create KeyFrame + // 将当前帧构造为初始关键帧 + KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpAtlas->GetCurrentMap(),mpKeyFrameDB); + + // Insert KeyFrame in the map + // 在地图中添加该初始关键帧 + mpAtlas->AddKeyFrame(pKFini); + + // Create MapPoints and asscoiate to KeyFrame + if(!mpCamera2){ + // 为每个特征点构造MapPoint + for(int i=0; i0) + { + // 通过反投影得到该特征点的世界坐标系下3D坐标 + Eigen::Vector3f x3D; + mCurrentFrame.UnprojectStereo(i, x3D); + MapPoint* pNewMP = new MapPoint(x3D, pKFini, mpAtlas->GetCurrentMap()); + // 为该MapPoint添加属性： + // a.观测到该MapPoint的关键帧 + // b.该MapPoint的描述子 + // c.该MapPoint的平均观测方向和深度范围 + pNewMP->AddObservation(pKFini,i); + pKFini->AddMapPoint(pNewMP,i); + pNewMP->ComputeDistinctiveDescriptors(); + pNewMP->UpdateNormalAndDepth(); + mpAtlas->AddMapPoint(pNewMP); + + mCurrentFrame.mvpMapPoints[i]=pNewMP; + } + } + } else{ + for(int i = 0; i < mCurrentFrame.Nleft; i++){ + int rightIndex = mCurrentFrame.mvLeftToRightMatch[i]; + if(rightIndex != -1){ + Eigen::Vector3f x3D = mCurrentFrame.mvStereo3Dpoints[i]; + + MapPoint* pNewMP = new MapPoint(x3D, pKFini, mpAtlas->GetCurrentMap()); + + pNewMP->AddObservation(pKFini,i); + pNewMP->AddObservation(pKFini,rightIndex + mCurrentFrame.Nleft); + + pKFini->AddMapPoint(pNewMP,i); + pKFini->AddMapPoint(pNewMP,rightIndex + mCurrentFrame.Nleft); + + pNewMP->ComputeDistinctiveDescriptors(); + pNewMP->UpdateNormalAndDepth(); + mpAtlas->AddMapPoint(pNewMP); + + mCurrentFrame.mvpMapPoints[i]=pNewMP; + mCurrentFrame.mvpMapPoints[rightIndex + mCurrentFrame.Nleft]=pNewMP; + } + } + } + + Verbose::PrintMess("New Map created with " + to_string(mpAtlas->MapPointsInMap()) + " points", Verbose::VERBOSITY_QUIET); + + //cout << "Active map: " << mpAtlas->GetCurrentMap()->GetId() << endl; + // 在局部地图中添加该初始关键帧 + mpLocalMapper->InsertKeyFrame(pKFini); + + // 更新当前帧为上一帧 + mLastFrame = Frame(mCurrentFrame); + mnLastKeyFrameId = mCurrentFrame.mnId; + mpLastKeyFrame = pKFini; + //mnLastRelocFrameId = mCurrentFrame.mnId; + + mvpLocalKeyFrames.push_back(pKFini); + mvpLocalMapPoints=mpAtlas->GetAllMapPoints(); + mpReferenceKF = pKFini; + mCurrentFrame.mpReferenceKF = pKFini; + + // 把当前（最新的）局部MapPoints作为ReferenceMapPoints + mpAtlas->SetReferenceMapPoints(mvpLocalMapPoints); + + mpAtlas->GetCurrentMap()->mvpKeyFrameOrigins.push_back(pKFini); + + mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.GetPose()); + + // 追踪成功 + mState=OK; + } +} + +/* + * @brief 单目的地图初始化 + * + * 并行地计算基础矩阵和单应性矩阵，选取其中一个模型，恢复出最开始两帧之间的相对姿态以及点云 + * 得到初始两帧的匹配、相对运动、初始MapPoints + * + * Step 1：（未创建）得到用于初始化的第一帧，初始化需要两帧 + * Step 2：（已创建）如果当前帧特征点数大于100，则得到用于单目初始化的第二帧 + * Step 3：在mInitialFrame与mCurrentFrame中找匹配的特征点对 + * Step 4：如果初始化的两帧之间的匹配点太少，重新初始化 + * Step 5：通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints + * Step 6：删除那些无法进行三角化的匹配点 + * Step 7：将三角化得到的3D点包装成MapPoints + */ +void Tracking::MonocularInitialization() +{ + std::cout<<"enter mono initialization"<100) + { + // 初始化需要两帧，分别是mInitialFrame，mCurrentFrame + mInitialFrame = Frame(mCurrentFrame); + // 用当前帧更新上一帧 + mLastFrame = Frame(mCurrentFrame); + // mvbPrevMatched 记录"上一帧"所有特征点 + mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size()); + for(size_t i=0; i1.0))) + { + mbReadyToInitializate = false; + + return; + } + + // Find correspondences + // Step 3 在mInitialFrame与mCurrentFrame中找匹配的特征点对 + // 0.9 表示最佳的和次佳特征点评分的比值阈值，这里是比较宽松的，跟踪时一般是0.7 + // true 表示检查特征点的方向 + ORBmatcher matcher(0.9,true); + + // 对 mInitialFrame,mCurrentFrame 进行特征点匹配 + // mvbPrevMatched为参考帧的特征点坐标，初始化存储的是mInitialFrame中特征点坐标，匹配后存储的是匹配好的当前帧的特征点坐标 + // mvIniMatches 保存参考帧F1中特征点是否匹配上，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引 + int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100); + std::cout<<"nmatches:"< vbTriangulated; // Triangulated Correspondences (mvIniMatches) + + if(mpCamera->ReconstructWithTwoViews(mInitialFrame.mvKeysUn,mCurrentFrame.mvKeysUn,mvIniMatches,Tcw,mvIniP3D,vbTriangulated)) + { + // Step 6 初始化成功后，删除那些无法进行三角化的匹配点 + for(size_t i=0, iend=mvIniMatches.size(); i=0 && !vbTriangulated[i]) + { + mvIniMatches[i]=-1; + nmatches--; + } + } + + // Set Frame Poses + // Step 7 将初始化的第一帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵 + mInitialFrame.SetPose(Sophus::SE3f()); + // 由Rcw和tcw构造Tcw,并赋值给mTcw，mTcw为世界坐标系到相机坐标系的变换矩阵 + mCurrentFrame.SetPose(Tcw); + + // Step 8 创建初始化地图点MapPoints + // Initialize函数会得到mvIniP3D， + // mvIniP3D是cv::Point3f类型的一个容器，是个存放3D点的临时变量， + // CreateInitialMapMonocular将3D点包装成MapPoint类型存入KeyFrame和Map中 + CreateInitialMapMonocular(); + } + } +} + +/** + * @brief 单目相机成功初始化后用三角化得到的点生成MapPoints + * + */ +void Tracking::CreateInitialMapMonocular() +{ + // Create KeyFrames 认为单目初始化时候的参考帧和当前帧都是关键帧 + KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpAtlas->GetCurrentMap(),mpKeyFrameDB); + KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpAtlas->GetCurrentMap(),mpKeyFrameDB); + + if(mSensor == System::IMU_MONOCULAR) + pKFini->mpImuPreintegrated = (IMU::Preintegrated*)(NULL); + + // Step 1 将初始关键帧,当前关键帧的描述子转为BoW + pKFini->ComputeBoW(); + pKFcur->ComputeBoW(); + + // Insert KFs in the map + // Step 2 将关键帧插入到地图 + mpAtlas->AddKeyFrame(pKFini); + mpAtlas->AddKeyFrame(pKFcur); + + // Step 3 用初始化得到的3D点来生成地图点MapPoints + // mvIniMatches[i] 表示初始化两帧特征点匹配关系。 + // 具体解释：i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值,没有匹配关系的话，vMatches12[i]值为 -1 + for(size_t i=0; iGetCurrentMap()); + + // Step 3.2 为该MapPoint添加属性： + // a.观测到该MapPoint的关键帧 + // b.该MapPoint的描述子 + // c.该MapPoint的平均观测方向和深度范围 + + // 表示该KeyFrame的2D特征点和对应的3D地图点 + pKFini->AddMapPoint(pMP,i); + pKFcur->AddMapPoint(pMP,mvIniMatches[i]); + + // a.表示该MapPoint可以被哪个KeyFrame的哪个特征点观测到 + pMP->AddObservation(pKFini,i); + pMP->AddObservation(pKFcur,mvIniMatches[i]); + + // b.从众多观测到该MapPoint的特征点中挑选最有代表性的描述子 + pMP->ComputeDistinctiveDescriptors(); + + // c.更新该MapPoint平均观测方向以及观测距离的范围 + pMP->UpdateNormalAndDepth(); + + //Fill Current Frame structure + // mvIniMatches下标i表示在初始化参考帧中的特征点的序号 + // mvIniMatches[i]是初始化当前帧中的特征点的序号 + mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP; + mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false; + + //Add to Map + mpAtlas->AddMapPoint(pMP); + } + + + // Update Connections + // Step 3.3 更新关键帧间的连接关系 + // 在3D点和关键帧之间建立边，每个边有一个权重，边的权重是该关键帧与当前帧公共3D点的个数 + pKFini->UpdateConnections(); + pKFcur->UpdateConnections(); + + std::set sMPs; + sMPs = pKFini->GetMapPoints(); + + // Bundle Adjustment + // Step 4 全局BA优化，同时优化所有位姿和三维点 + Verbose::PrintMess("New Map created with " + to_string(mpAtlas->MapPointsInMap()) + " points", Verbose::VERBOSITY_QUIET); + Optimizer::GlobalBundleAdjustemnt(mpAtlas->GetCurrentMap(),20); + + // Step 5 取场景的中值深度，用于尺度归一化 + // 为什么是 pKFini 而不是 pKCur ? 答：都可以的，内部做了位姿变换了 + float medianDepth = pKFini->ComputeSceneMedianDepth(2); + float invMedianDepth; + if(mSensor == System::IMU_MONOCULAR) + invMedianDepth = 4.0f/medianDepth; // 4.0f + else + invMedianDepth = 1.0f/medianDepth; + + // 两个条件,一个是平均深度要大于0,另外一个是在当前帧中被观测到的地图点的数目应该大于50 + if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<50) // TODO Check, originally 100 tracks + { + Verbose::PrintMess("Wrong initialization, reseting...", Verbose::VERBOSITY_QUIET); + mpSystem->ResetActiveMap(); + return; + } + + // Step 6 将两帧之间的变换归一化到平均深度1的尺度下 + // Scale initial baseline + Sophus::SE3f Tc2w = pKFcur->GetPose(); + // x/z y/z 将z归一化到1 + Tc2w.translation() *= invMedianDepth; + pKFcur->SetPose(Tc2w); + + // Scale points + // Step 7 把3D点的尺度也归一化到1 + // 为什么是pKFini? 是不是就算是使用 pKFcur 得到的结果也是相同的? 答：是的，因为是同样的三维点 + vector vpAllMapPoints = pKFini->GetMapPointMatches(); + for(size_t iMP=0; iMPSetWorldPos(pMP->GetWorldPos()*invMedianDepth); + pMP->UpdateNormalAndDepth(); + } + } + + if (mSensor == System::IMU_MONOCULAR) + { + pKFcur->mPrevKF = pKFini; + pKFini->mNextKF = pKFcur; + pKFcur->mpImuPreintegrated = mpImuPreintegratedFromLastKF; + + mpImuPreintegratedFromLastKF = new IMU::Preintegrated(pKFcur->mpImuPreintegrated->GetUpdatedBias(),pKFcur->mImuCalib); + } + + // Step 8 将关键帧插入局部地图，更新归一化后的位姿、局部地图点 + mpLocalMapper->InsertKeyFrame(pKFini); + mpLocalMapper->InsertKeyFrame(pKFcur); + mpLocalMapper->mFirstTs=pKFcur->mTimeStamp; + + mCurrentFrame.SetPose(pKFcur->GetPose()); + mnLastKeyFrameId=mCurrentFrame.mnId; + mpLastKeyFrame = pKFcur; + //mnLastRelocFrameId = mInitialFrame.mnId; + + mvpLocalKeyFrames.push_back(pKFcur); + mvpLocalKeyFrames.push_back(pKFini); + // 单目初始化之后，得到的初始地图中的所有点都是局部地图点 + mvpLocalMapPoints=mpAtlas->GetAllMapPoints(); + mpReferenceKF = pKFcur; + mCurrentFrame.mpReferenceKF = pKFcur; + + // Compute here initial velocity + vector vKFs = mpAtlas->GetAllKeyFrames(); + + Sophus::SE3f deltaT = vKFs.back()->GetPose() * vKFs.front()->GetPoseInverse(); + mbVelocity = false; + Eigen::Vector3f phi = deltaT.so3().log(); + + double aux = (mCurrentFrame.mTimeStamp-mLastFrame.mTimeStamp)/(mCurrentFrame.mTimeStamp-mInitialFrame.mTimeStamp); + phi *= aux; + + mLastFrame = Frame(mCurrentFrame); + + mpAtlas->SetReferenceMapPoints(mvpLocalMapPoints); + + mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose()); + + mpAtlas->GetCurrentMap()->mvpKeyFrameOrigins.push_back(pKFini); + + // 初始化成功，至此，初始化过程完成 + mState=OK; + + initID = pKFcur->mnId; +} + +/** + * @brief 在Atlas中保存当前地图，创建新地图，所有跟状态相关的变量全部重置 + * 1. 前后两帧对应的时间戳反了 + * 2. imu模式下前后帧超过1s + * 3. 上一帧为最近丢失且重定位失败时 + * 4. 重定位成功，局部地图跟踪失败 + */ +void Tracking::CreateMapInAtlas() +{ + mnLastInitFrameId = mCurrentFrame.mnId; + mpAtlas->CreateNewMap(); + if (mSensor==System::IMU_STEREO || mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_RGBD) + mpAtlas->SetInertialSensor(); // mpAtlas中map的mbIsInertial=true + mbSetInit=false; // 好像没什么用 + + mnInitialFrameId = mCurrentFrame.mnId+1; + mState = NO_IMAGES_YET; + + // Restart the variable with information about the last KF + mbVelocity = false; + //mnLastRelocFrameId = mnLastInitFrameId; // The last relocation KF_id is the current id, because it is the new starting point for new map + Verbose::PrintMess("First frame id in map: " + to_string(mnLastInitFrameId+1), Verbose::VERBOSITY_NORMAL); + mbVO = false; // Init value for know if there are enough MapPoints in the last KF + if(mSensor == System::MONOCULAR || mSensor == System::IMU_MONOCULAR) + { + mbReadyToInitializate = false; + } + + if((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && mpImuPreintegratedFromLastKF) + { + delete mpImuPreintegratedFromLastKF; + mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib); + } + + if(mpLastKeyFrame) + mpLastKeyFrame = static_cast(NULL); + + if(mpReferenceKF) + mpReferenceKF = static_cast(NULL); + + mLastFrame = Frame(); + mCurrentFrame = Frame(); + mvIniMatches.clear(); + mlQueueImuData.clear(); + + mbCreatedMap = true; +} + +/* + * @brief 检查上一帧中的地图点是否需要被替换 + * + * Local Mapping线程可能会将关键帧中某些地图点进行替换，由于tracking中需要用到上一帧地图点，所以这里检查并更新上一帧中被替换的地图点 + * @see LocalMapping::SearchInNeighbors() + */ +void Tracking::CheckReplacedInLastFrame() +{ + for(int i =0; iGetReplaced(); + if(pRep) + { + // 然后替换一下 + mLastFrame.mvpMapPoints[i] = pRep; + } + } + } +} + +/* + * @brief 用参考关键帧的地图点来对当前普通帧进行跟踪 + * + * Step 1：将当前普通帧的描述子转化为BoW向量 + * Step 2：通过词袋BoW加速当前帧与参考帧之间的特征点匹配 + * Step 3: 将上一帧的位姿态作为当前帧位姿的初始值 + * Step 4: 通过优化3D-2D的重投影误差来获得位姿 + * Step 5：剔除优化后的匹配点中的外点 + * @return 如果匹配数超10，返回true + * + */ +bool Tracking::TrackReferenceKeyFrame() +{ + // Compute Bag of Words vector + // Step 1：将当前帧的描述子转化为BoW向量 + mCurrentFrame.ComputeBoW(); + + // We perform first an ORB matching with the reference keyframe + // If enough matches are found we setup a PnP solver + ORBmatcher matcher(0.7,true); + vector vpMapPointMatches; + + // Step 2：通过词袋BoW加速当前帧与参考帧之间的特征点匹配 + int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches); + + // 匹配数目小于15，认为跟踪失败 + if(nmatches<15) + { + cout << "TRACK_REF_KF: Less than 15 matches!!\n"; + return false; + } + + // Step 3:将上一帧的位姿态作为当前帧位姿的初始值 + mCurrentFrame.mvpMapPoints = vpMapPointMatches; + mCurrentFrame.SetPose(mLastFrame.GetPose()); // 用上一次的Tcw设置初值，在PoseOptimization可以收敛快一些 + + //mCurrentFrame.PrintPointDistribution(); + + + // cout << " TrackReferenceKeyFrame mLastFrame.mTcw: " << mLastFrame.mTcw << endl; + // Step 4:通过优化3D-2D的重投影误差来获得位姿 + Optimizer::PoseOptimization(&mCurrentFrame); + + // Discard outliers + // Step 5：剔除优化后的匹配点中的外点 + //之所以在优化之后才剔除外点，是因为在优化的过程中就有了对这些外点的标记 + int nmatchesMap = 0; + for(int i =0; i= mCurrentFrame.Nleft) break; + if(mCurrentFrame.mvpMapPoints[i]) + { + // 如果对应到的某个特征点是外点 + if(mCurrentFrame.mvbOutlier[i]) + { + // 清除它在当前帧中存在过的痕迹 + MapPoint* pMP = mCurrentFrame.mvpMapPoints[i]; + + mCurrentFrame.mvpMapPoints[i]=static_cast(NULL); + mCurrentFrame.mvbOutlier[i]=false; + if(i < mCurrentFrame.Nleft){ + pMP->mbTrackInView = false; + } + else{ + pMP->mbTrackInViewR = false; + } + pMP->mbTrackInView = false; + pMP->mnLastFrameSeen = mCurrentFrame.mnId; + nmatches--; + } + else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0) + // 匹配的内点计数++ + nmatchesMap++; + } + } + + if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) + return true; + else + return nmatchesMap>=10; // 跟踪成功的数目超过10才认为跟踪成功，否则跟踪失败 +} + +/** + * @brief 更新上一帧位姿，在上一帧中生成临时地图点 + * 单目情况：只计算了上一帧的世界坐标系位姿 + * 双目和rgbd情况：选取有有深度值的并且没有被选为地图点的点生成新的临时地图点，提高跟踪鲁棒性 + */ +void Tracking::UpdateLastFrame() +{ + // Update pose according to reference keyframe + // Step 1：利用参考关键帧更新上一帧在世界坐标系下的位姿 + // 上一普通帧的参考关键帧，注意这里用的是参考关键帧（位姿准）而不是上上一帧的普通帧 + KeyFrame* pRef = mLastFrame.mpReferenceKF; + // ref_keyframe 到 lastframe的位姿变换 + Sophus::SE3f Tlr = mlRelativeFramePoses.back(); + // 将上一帧的世界坐标系下的位姿计算出来 + // l:last, r:reference, w:world + // Tlw = Tlr*Trw + mLastFrame.SetPose(Tlr * pRef->GetPose()); + + // 如果上一帧为关键帧，或者单目/单目惯性，SLAM模式的情况，则退出 + if(mnLastKeyFrameId==mLastFrame.mnId || mSensor==System::MONOCULAR || mSensor==System::IMU_MONOCULAR || !mbOnlyTracking) + return; + + // Step 2：对于双目或rgbd相机，为上一帧生成新的临时地图点 + // 注意这些地图点只是用来跟踪，不加入到地图中，跟踪完后会删除 + // Create "visual odometry" MapPoints + // We sort points according to their measured depth by the stereo/RGB-D sensor + // Step 2.1：得到上一帧中具有有效深度值的特征点（不一定是地图点） + vector > vDepthIdx; + const int Nfeat = mLastFrame.Nleft == -1? mLastFrame.N : mLastFrame.Nleft; + vDepthIdx.reserve(Nfeat); + for(int i=0; i0) + { + // vDepthIdx第一个元素是某个点的深度,第二个元素是对应的特征点id + vDepthIdx.push_back(make_pair(z,i)); + } + } + // 如果上一帧中没有有效深度的点,那么就直接退出 + if(vDepthIdx.empty()) + return; + + // 按照深度从小到大排序 + sort(vDepthIdx.begin(),vDepthIdx.end()); + + // We insert all close points (depthObservations()<1) + // 地图点被创建后就没有被观测，认为不靠谱，也需要重新创建 + bCreateNew = true; + + if(bCreateNew) + { + // Step 2.3：需要创建的点，包装为地图点。只是为了提高双目和RGBD的跟踪成功率，并没有添加复杂属性，因为后面会扔掉 + // 反投影到世界坐标系中 + Eigen::Vector3f x3D; + + if(mLastFrame.Nleft == -1){ + mLastFrame.UnprojectStereo(i, x3D); + } + else{ + x3D = mLastFrame.UnprojectStereoFishEye(i); + } + + // 加入上一帧的地图点中 + MapPoint* pNewMP = new MapPoint(x3D,mpAtlas->GetCurrentMap(),&mLastFrame,i); + mLastFrame.mvpMapPoints[i]=pNewMP; + + // 标记为临时添加的MapPoint，之后在CreateNewKeyFrame之前会全部删除 + mlpTemporalPoints.push_back(pNewMP); + nPoints++; + } + else + { + // 因为从近到远排序，记录其中不需要创建地图点的个数 + nPoints++; + } + + // Step 2.4：如果地图点质量不好，停止创建地图点 + // 停止新增临时地图点必须同时满足以下条件： + // 1、当前的点的深度已经超过了设定的深度阈值（35倍基线） + // 2、nPoints已经超过100个点，说明距离比较远了，可能不准确，停掉退出 + if(vDepthIdx[j].first>mThDepth && nPoints>100) + break; + + } +} + +/** + * @brief 根据恒定速度模型用上一帧地图点来对当前帧进行跟踪 + * Step 1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点 + * Step 2：根据上一帧特征点对应地图点进行投影匹配 + * Step 3：优化当前帧位姿 + * Step 4：剔除地图点中外点 + * @return 如果匹配数大于10，认为跟踪成功，返回true + */ +bool Tracking::TrackWithMotionModel() +{ + // 最小距离 < 0.9*次小距离 匹配成功，检查旋转 + ORBmatcher matcher(0.9,true); + + // Update last frame pose according to its reference keyframe + // Create "visual odometry" points if in Localization Mode + // Step 1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点 + UpdateLastFrame(); + + // Step 2：根据IMU或者恒速模型得到当前帧的初始位姿。 + if (mpAtlas->isImuInitialized() && (mCurrentFrame.mnId>mnLastRelocFrameId+mnFramesToResetIMU)) + { + // Predict state with IMU if it is initialized and it doesnt need reset + // IMU完成初始化 并且 距离重定位挺久不需要重置IMU，用IMU来估计位姿，没有后面的这那那这的 + PredictStateIMU(); + return true; + } + else + { + // 根据之前估计的速度，用恒速模型得到当前帧的初始位姿。 + mCurrentFrame.SetPose(mVelocity * mLastFrame.GetPose()); + } + + // 清空当前帧的地图点 + fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast(NULL)); + + // Project points seen in previous frame + // 设置特征匹配过程中的搜索半径 + int th; + + if(mSensor==System::STEREO) + th=7; + else + th=15; + + // Step 3：用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次 + int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR || mSensor==System::IMU_MONOCULAR); + + // If few matches, uses a wider window search + // 如果匹配点太少，则扩大搜索半径再来一次 + if(nmatches<20) + { + Verbose::PrintMess("Not enough matches, wider window search!!", Verbose::VERBOSITY_NORMAL); + fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast(NULL)); + + nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR || mSensor==System::IMU_MONOCULAR); + Verbose::PrintMess("Matches with wider search: " + to_string(nmatches), Verbose::VERBOSITY_NORMAL); + + } + + // 这里不同于ORB-SLAM2的方式 + if(nmatches<20) + { + Verbose::PrintMess("Not enough matches!!", Verbose::VERBOSITY_NORMAL); + if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) + return true; + else + return false; + } + + // Optimize frame pose with all matches + // Step 4：利用3D-2D投影关系，优化当前帧位姿 + Optimizer::PoseOptimization(&mCurrentFrame); + + // Discard outliers + // Step 5：剔除地图点中外点 + int nmatchesMap = 0; + for(int i =0; i(NULL); + mCurrentFrame.mvbOutlier[i]=false; + if(i < mCurrentFrame.Nleft){ + pMP->mbTrackInView = false; + } + else{ + pMP->mbTrackInViewR = false; + } + pMP->mnLastFrameSeen = mCurrentFrame.mnId; + nmatches--; + } + else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0) + // 累加成功匹配到的地图点数目 + nmatchesMap++; + } + } + + // 纯定位模式下：如果成功追踪的地图点非常少,那么这里的mbVO标志就会置位 + if(mbOnlyTracking) + { + mbVO = nmatchesMap<10; + return nmatches>20; + } + + if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) + return true; + else + return nmatchesMap>=10; // 匹配超过10个点就认为跟踪成功 +} + +/** + * @brief 用局部地图进行跟踪，进一步优化位姿 + * + * 1. 更新局部地图，包括局部关键帧和关键点 + * 2. 对局部MapPoints进行投影匹配 + * 3. 根据匹配对估计当前帧的姿态 + * 4. 根据姿态剔除误匹配 + * @return true if success + * + * Step 1：更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints + * Step 2：在局部地图中查找与当前帧匹配的MapPoints, 其实也就是对局部地图点进行跟踪 + * Step 3：更新局部所有MapPoints后对位姿再次优化 + * Step 4：更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果 + * Step 5：决定是否跟踪成功 + */ +bool Tracking::TrackLocalMap() +{ + + // We have an estimation of the camera pose and some map points tracked in the frame. + // We retrieve the local map and try to find matches to points in the local map. + mTrackedFr++; + + // Step 1：更新局部关键帧 mvpLocalKeyFrames 和局部地图点 mvpLocalMapPoints + UpdateLocalMap(); + // Step 2：筛选局部地图中新增的在视野范围内的地图点，投影到当前帧搜索匹配，得到更多的匹配关系 + SearchLocalPoints(); + + // TOO check outliers before PO + // 查看内外点数目，调试用 + int aux1 = 0, aux2=0; + for(int i=0; iisImuInitialized()) + Optimizer::PoseOptimization(&mCurrentFrame); + else + { + // 初始化，重定位，重新开启一个地图都会使mnLastRelocFrameId变化 + if(mCurrentFrame.mnId<=mnLastRelocFrameId+mnFramesToResetIMU) + { + Verbose::PrintMess("TLM: PoseOptimization ", Verbose::VERBOSITY_DEBUG); + Optimizer::PoseOptimization(&mCurrentFrame); + } + else // 如果积累的IMU数据量比较多，考虑使用IMU数据优化 + { + // if(!mbMapUpdated && mState == OK) // && (mnMatchesInliers>30)) + // mbMapUpdated变化见Tracking::PredictStateIMU() + // 未更新地图 + if(!mbMapUpdated) // && (mnMatchesInliers>30)) + { + Verbose::PrintMess("TLM: PoseInertialOptimizationLastFrame ", Verbose::VERBOSITY_DEBUG); + // 使用上一普通帧以及当前帧的视觉信息和IMU信息联合优化当前帧位姿、速度和IMU零偏 + inliers = Optimizer::PoseInertialOptimizationLastFrame(&mCurrentFrame); // , !mpLastKeyFrame->GetMap()->GetIniertialBA1()); + } + else + { + Verbose::PrintMess("TLM: PoseInertialOptimizationLastKeyFrame ", Verbose::VERBOSITY_DEBUG); + // 使用上一关键帧以及当前帧的视觉信息和IMU信息联合优化当前帧位姿、速度和IMU零偏 + inliers = Optimizer::PoseInertialOptimizationLastKeyFrame(&mCurrentFrame); // , !mpLastKeyFrame->GetMap()->GetIniertialBA1()); + } + } + } + // 查看内外点数目，调试用 + aux1 = 0, aux2 = 0; + for(int i=0; iIncreaseFound(); + // 查看当前是否是在纯定位过程 + if(!mbOnlyTracking) + { + // 如果该地图点被相机观测数目nObs大于0，匹配内点计数+1 + // nObs： 被观测到的相机数目，单目+1，双目或RGB-D则+2 + if(mCurrentFrame.mvpMapPoints[i]->Observations()>0) + mnMatchesInliers++; + } + else + // 记录当前帧跟踪到的地图点数目，用于统计跟踪效果 + mnMatchesInliers++; + } + // 如果这个地图点是外点,并且当前相机输入还是双目的时候,就删除这个点 + // 原因分析：因为双目本身可以左右互匹配，删掉无所谓 + else if(mSensor==System::STEREO) + mCurrentFrame.mvpMapPoints[i] = static_cast(NULL); + } + } + + // Decide if the tracking was succesful + // More restrictive if there was a relocalization recently + mpLocalMapper->mnMatchesInliers=mnMatchesInliers; + // Step 5：根据跟踪匹配数目及重定位情况决定是否跟踪成功 + // 如果最近刚刚发生了重定位,那么至少成功匹配50个点才认为是成功跟踪 + if(mCurrentFrame.mnId10)&&(mState==RECENTLY_LOST)) + return true; + + // 单目IMU模式下做完初始化至少成功跟踪15个才算成功，没做初始化需要50个 + if (mSensor == System::IMU_MONOCULAR) + { + if((mnMatchesInliers<15 && mpAtlas->isImuInitialized())||(mnMatchesInliers<50 && !mpAtlas->isImuInitialized())) + { + return false; + } + else + return true; + } + else if (mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) + { + if(mnMatchesInliers<15) + { + return false; + } + else + return true; + } + else + { + //以上情况都不满足，只要跟踪的地图点大于30个就认为成功了 + if(mnMatchesInliers<30) + return false; + else + return true; + } +} + +/** + * @brief 判断当前帧是否需要插入关键帧 + * + * Step 1：纯VO模式下不插入关键帧，如果局部地图被闭环检测使用，则不插入关键帧 + * Step 2：如果距离上一次重定位比较近，或者关键帧数目超出最大限制，不插入关键帧 + * Step 3：得到参考关键帧跟踪到的地图点数量 + * Step 4：查询局部地图管理器是否繁忙,也就是当前能否接受新的关键帧 + * Step 5：对于双目或RGBD摄像头，统计可以添加的有效地图点总数 和 跟踪到的地图点数量 + * Step 6：决策是否需要插入关键帧 + * @return true 需要 + * @return false 不需要 + */ +bool Tracking::NeedNewKeyFrame() +{ + // 如果是IMU模式并且当前地图中未完成IMU初始化 + if((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && !mpAtlas->GetCurrentMap()->isImuInitialized()) + { + // 如果是IMU模式，当前帧距离上一关键帧时间戳超过0.25s，则说明需要插入关键帧，不再进行后续判断 + if (mSensor == System::IMU_MONOCULAR && (mCurrentFrame.mTimeStamp-mpLastKeyFrame->mTimeStamp)>=0.25) + return true; + else if ((mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && (mCurrentFrame.mTimeStamp-mpLastKeyFrame->mTimeStamp)>=0.25) + return true; + else + return false; + } + + // Step 1：纯VO模式下不插入关键帧 + if(mbOnlyTracking) + return false; + + // If Local Mapping is freezed by a Loop Closure do not insert keyframes + // Step 2：如果局部地图线程被闭环检测使用，则不插入关键帧 + if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested()) { + /*if(mSensor == System::MONOCULAR) + { + std::cout << "NeedNewKeyFrame: localmap stopped" << std::endl; + }*/ + return false; + } + + // 获取当前地图中的关键帧数目 + const int nKFs = mpAtlas->KeyFramesInMap(); + + // Do not insert keyframes if not enough frames have passed from last relocalisation + // mCurrentFrame.mnId是当前帧的ID + // mnLastRelocFrameId是最近一次重定位帧的ID + // mMaxFrames等于图像输入的帧率 + // Step 3：如果距离上一次重定位比较近，并且关键帧数目超出最大限制，不插入关键帧 + if(mCurrentFrame.mnIdmMaxFrames) + { + return false; + } + + // Tracked MapPoints in the reference keyframe + // Step 4：得到参考关键帧跟踪到的地图点数量 + // UpdateLocalKeyFrames 函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧 + + // 地图点的最小观测次数 + int nMinObs = 3; + if(nKFs<=2) + nMinObs=2; + // 参考关键帧地图点中观测的数目>= nMinObs的地图点数目 + int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs); + + // Local Mapping accept keyframes? + // Step 5：查询局部地图线程是否繁忙，当前能否接受新的关键帧 + bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames(); + + // Check how many "close" points are being tracked and how many could be potentially created. + // Step 6：对于双目或RGBD摄像头，统计成功跟踪的近点的数量，如果跟踪到的近点太少，没有跟踪到的近点较多，可以插入关键帧 + int nNonTrackedClose = 0; // 双目或RGB-D中没有跟踪到的近点 + int nTrackedClose= 0; // 双目或RGB-D中成功跟踪的近点（三维点） + + if(mSensor!=System::MONOCULAR && mSensor!=System::IMU_MONOCULAR) + { + int N = (mCurrentFrame.Nleft == -1) ? mCurrentFrame.N : mCurrentFrame.Nleft; + for(int i =0; i0 && mCurrentFrame.mvDepth[i] closed points: " + to_string(nTrackedClose) + "; non tracked closed points: " + to_string(nNonTrackedClose), Verbose::VERBOSITY_NORMAL);// Verbose::VERBOSITY_DEBUG); + } + // 双目或RGBD情况下：跟踪到的地图点中近点太少 同时 没有跟踪到的三维点太多，可以插入关键帧了 + // 单目时，为false + bool bNeedToInsertClose; + bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70); + + // Step 7：决策是否需要插入关键帧 + // Thresholds + // Step 7.1：设定比例阈值，当前帧和参考关键帧跟踪到点的比例，比例越大，越倾向于增加关键帧 + float thRefRatio = 0.75f; + // 关键帧只有一帧，那么插入关键帧的阈值设置的低一点，插入频率较低 + if(nKFs<2) + thRefRatio = 0.4f; + + /*int nClosedPoints = nTrackedClose + nNonTrackedClose; + const int thStereoClosedPoints = 15; + if(nClosedPoints < thStereoClosedPoints && (mSensor==System::STEREO || mSensor==System::IMU_STEREO)) + { + //Pseudo-monocular, there are not enough close points to be confident about the stereo observations. + thRefRatio = 0.9f; + }*/ + + // 单目情况下插入关键帧的频率很高 + if(mSensor==System::MONOCULAR) + thRefRatio = 0.9f; + + if(mpCamera2) thRefRatio = 0.75f; + + // 单目+IMU情况下如果，匹配内点数目超过350，插入关键帧的频率可以适当降低 + if(mSensor==System::IMU_MONOCULAR) + { + if(mnMatchesInliers>350) // Points tracked from the local map + thRefRatio = 0.75f; + else + thRefRatio = 0.90f; + } + + // Condition 1a: More than "MaxFrames" have passed from last keyframe insertion + // Step 7.2：很长时间没有插入关键帧，可以插入 + const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames; + // Condition 1b: More than "MinFrames" have passed and Local Mapping is idle + // Step 7.3：满足插入关键帧的最小间隔并且localMapper处于空闲状态，可以插入 + const bool c1b = ((mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames) && bLocalMappingIdle); //mpLocalMapper->KeyframesInQueue() < 2); + //Condition 1c: tracking is weak + // Step 7.4：在双目，RGB-D的情况下当前帧跟踪到的点比参考关键帧的0.25倍还少，或者满足bNeedToInsertClose + const bool c1c = mSensor!=System::MONOCULAR && mSensor!=System::IMU_MONOCULAR && mSensor!=System::IMU_STEREO && mSensor!=System::IMU_RGBD && (mnMatchesInliers15); + + //std::cout << "NeedNewKF: c1a=" << c1a << "; c1b=" << c1b << "; c1c=" << c1c << "; c2=" << c2 << std::endl; + // Temporal condition for Inertial cases + // 新增的条件c3：单目/双目+IMU模式下，并且IMU完成了初始化（隐藏条件），当前帧和上一关键帧之间时间超过0.5秒，则c3=true + bool c3 = false; + if(mpLastKeyFrame) + { + if (mSensor==System::IMU_MONOCULAR) + { + if ((mCurrentFrame.mTimeStamp-mpLastKeyFrame->mTimeStamp)>=0.5) + c3 = true; + } + else if (mSensor==System::IMU_STEREO || mSensor == System::IMU_RGBD) + { + if ((mCurrentFrame.mTimeStamp-mpLastKeyFrame->mTimeStamp)>=0.5) + c3 = true; + } + } + + // 新增的条件c4：单目+IMU模式下，当前帧匹配内点数在15~75之间或者是RECENTLY_LOST状态，c4=true + bool c4 = false; + if ((((mnMatchesInliers<75) && (mnMatchesInliers>15)) || mState==RECENTLY_LOST) && (mSensor == System::IMU_MONOCULAR)) // MODIFICATION_2, originally ((((mnMatchesInliers<75) && (mnMatchesInliers>15)) || mState==RECENTLY_LOST) && ((mSensor == System::IMU_MONOCULAR))) + c4=true; + else + c4=false; + + // 相比ORB-SLAM2多了c3,c4 + if(((c1a||c1b||c1c) && c2)||c3 ||c4) + { + // If the mapping accepts keyframes, insert keyframe. + // Otherwise send a signal to interrupt BA + // Step 7.6：local mapping空闲时或者正在做imu初始化时可以直接插入，不空闲的时候要根据情况插入 + if(bLocalMappingIdle || mpLocalMapper->IsInitializing()) + { + // 可以插入关键帧 + return true; + } + else + { + mpLocalMapper->InterruptBA(); + if(mSensor!=System::MONOCULAR && mSensor!=System::IMU_MONOCULAR) + { + // 双目或双目+IMU或RGB-D模式下，如队列里没有阻塞太多关键帧，可以插入 + // tracking插入关键帧不是直接插入，而且先插入到mlNewKeyFrames中， + // 然后localmapper再逐个pop出来插入到mspKeyFrames + if(mpLocalMapper->KeyframesInQueue()<3) + // 队列中的关键帧数目不是很多,可以插入 + return true; + else + // 队列中缓冲的关键帧数目太多,暂时不能插入 + return false; + } + else + { + //std::cout << "NeedNewKeyFrame: localmap is busy" << std::endl; + //对于单目情况,就直接无法插入关键帧了 + //? 为什么这里对单目情况的处理不一样? + //回答：可能是单目关键帧相对比较密集 + return false; + } + } + } + else + // 不满足上面的条件,自然不能插入关键帧 + return false; +} + +/** + * @brief 创建新的关键帧 + * 对于非单目的情况，同时创建新的MapPoints + * + * Step 1：将当前帧构造成关键帧 + * Step 2：将当前关键帧设置为当前帧的参考关键帧 + * Step 3：对于双目或rgbd摄像头，为当前帧生成新的MapPoints + */ +void Tracking::CreateNewKeyFrame() +{ + // 如果局部建图线程正在初始化且没做完或关闭了,就无法插入关键帧 + if(mpLocalMapper->IsInitializing() && !mpAtlas->isImuInitialized()) + return; + + if(!mpLocalMapper->SetNotStop(true)) + return; + + // Step 1：将当前帧构造成关键帧 + KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpAtlas->GetCurrentMap(),mpKeyFrameDB); + + if(mpAtlas->isImuInitialized()) // || mpLocalMapper->IsInitializing()) + pKF->bImu = true; + + pKF->SetNewBias(mCurrentFrame.mImuBias); + // Step 2：将当前关键帧设置为当前帧的参考关键帧 + // 在UpdateLocalKeyFrames函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧 + mpReferenceKF = pKF; + mCurrentFrame.mpReferenceKF = pKF; + + if(mpLastKeyFrame) + { + pKF->mPrevKF = mpLastKeyFrame; + mpLastKeyFrame->mNextKF = pKF; + } + else + Verbose::PrintMess("No last KF in KF creation!!", Verbose::VERBOSITY_NORMAL); + + // Reset preintegration from last KF (Create new object) + if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) + { + mpImuPreintegratedFromLastKF = new IMU::Preintegrated(pKF->GetImuBias(),pKF->mImuCalib); + } + + // 这段代码和 Tracking::UpdateLastFrame 中的那一部分代码功能相同 + // Step 3：对于双目或rgbd摄像头，为当前帧生成新的地图点；单目无操作 + if(mSensor!=System::MONOCULAR && mSensor != System::IMU_MONOCULAR) // TODO check if incluide imu_stereo + { + // 根据Tcw计算mRcw、mtcw和mRwc、mOw + mCurrentFrame.UpdatePoseMatrices(); + // cout << "create new MPs" << endl; + // We sort points by the measured depth by the stereo/RGBD sensor. + // We create all those MapPoints whose depth < mThDepth. + // If there are less than 100 close points we create the 100 closest. + int maxPoint = 100; + if(mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) + maxPoint = 100; + + // Step 3.1：得到当前帧有深度值的特征点（不一定是地图点） + vector > vDepthIdx; + int N = (mCurrentFrame.Nleft != -1) ? mCurrentFrame.Nleft : mCurrentFrame.N; + vDepthIdx.reserve(mCurrentFrame.N); + for(int i=0; i0) + { + // 第一个元素是深度,第二个元素是对应的特征点的id + vDepthIdx.push_back(make_pair(z,i)); + } + } + + if(!vDepthIdx.empty()) + { + // Step 3.2：按照深度从小到大排序 + sort(vDepthIdx.begin(),vDepthIdx.end()); + + // Step 3.3：从中找出不是地图点的生成临时地图点 + // 处理的近点的个数 + int nPoints = 0; + for(size_t j=0; jObservations()<1) + { + bCreateNew = true; + mCurrentFrame.mvpMapPoints[i] = static_cast(NULL); + } + + // 如果需要就新建地图点，这里的地图点不是临时的，是全局地图中新建地图点，用于跟踪 + if(bCreateNew) + { + Eigen::Vector3f x3D; + + if(mCurrentFrame.Nleft == -1){ + mCurrentFrame.UnprojectStereo(i, x3D); + } + else{ + x3D = mCurrentFrame.UnprojectStereoFishEye(i); + } + + MapPoint* pNewMP = new MapPoint(x3D,pKF,mpAtlas->GetCurrentMap()); + // 这些添加属性的操作是每次创建MapPoint后都要做的 + pNewMP->AddObservation(pKF,i); + + //Check if it is a stereo observation in order to not + //duplicate mappoints + if(mCurrentFrame.Nleft != -1 && mCurrentFrame.mvLeftToRightMatch[i] >= 0){ + mCurrentFrame.mvpMapPoints[mCurrentFrame.Nleft + mCurrentFrame.mvLeftToRightMatch[i]]=pNewMP; + pNewMP->AddObservation(pKF,mCurrentFrame.Nleft + mCurrentFrame.mvLeftToRightMatch[i]); + pKF->AddMapPoint(pNewMP,mCurrentFrame.Nleft + mCurrentFrame.mvLeftToRightMatch[i]); + } + + pKF->AddMapPoint(pNewMP,i); + pNewMP->ComputeDistinctiveDescriptors(); + pNewMP->UpdateNormalAndDepth(); + mpAtlas->AddMapPoint(pNewMP); + + mCurrentFrame.mvpMapPoints[i]=pNewMP; + nPoints++; + } + else + { + // 因为从近到远排序，记录其中不需要创建地图点的个数 + nPoints++; + } + + // Step 3.4：停止新建地图点必须同时满足以下条件： + // 1、当前的点的深度已经超过了设定的深度阈值（35倍基线） + // 2、nPoints已经超过100个点，说明距离比较远了，可能不准确，停掉退出 + if(vDepthIdx[j].first>mThDepth && nPoints>maxPoint) + { + break; + } + } + //Verbose::PrintMess("new mps for stereo KF: " + to_string(nPoints), Verbose::VERBOSITY_NORMAL); + } + } + + // Step 4：插入关键帧 + // 关键帧插入到列表 mlNewKeyFrames中，等待local mapping线程临幸 + mpLocalMapper->InsertKeyFrame(pKF); + + // 插入好了，允许局部建图停止 + mpLocalMapper->SetNotStop(false); + + // 当前帧成为新的关键帧，更新 + mnLastKeyFrameId = mCurrentFrame.mnId; + mpLastKeyFrame = pKF; +} + +/** + * @brief 用局部地图点进行投影匹配，得到更多的匹配关系 + * 注意：局部地图点中已经是当前帧地图点的不需要再投影，只需要将此外的并且在视野范围内的点和当前帧进行投影匹配 + */ +void Tracking::SearchLocalPoints() +{ + // Do not search map points already matched + // Step 1：遍历当前帧的地图点，标记这些地图点不参与之后的投影搜索匹配 + for(vector::iterator vit=mCurrentFrame.mvpMapPoints.begin(), vend=mCurrentFrame.mvpMapPoints.end(); vit!=vend; vit++) + { + MapPoint* pMP = *vit; + if(pMP) + { + if(pMP->isBad()) + { + *vit = static_cast(NULL); + } + else + { + // 更新能观测到该点的帧数加1(被当前帧观测了) + pMP->IncreaseVisible(); + // 标记该点被当前帧观测到 + pMP->mnLastFrameSeen = mCurrentFrame.mnId; + // 标记该点在后面搜索匹配时不被投影，因为已经有匹配了 + pMP->mbTrackInView = false; + pMP->mbTrackInViewR = false; + } + } + } + + // 准备进行投影匹配的点的数目 + int nToMatch=0; + + // Project points in frame and check its visibility + // Step 2：判断所有局部地图点中除当前帧地图点外的点，是否在当前帧视野范围内 + for(vector::iterator vit=mvpLocalMapPoints.begin(), vend=mvpLocalMapPoints.end(); vit!=vend; vit++) + { + MapPoint* pMP = *vit; + + // 已经被当前帧观测到的地图点肯定在视野范围内，跳过 + if(pMP->mnLastFrameSeen == mCurrentFrame.mnId) + continue; + // 跳过坏点 + if(pMP->isBad()) + continue; + // Project (this fills MapPoint variables for matching) + // 判断地图点是否在在当前帧视野内 + if(mCurrentFrame.isInFrustum(pMP,0.5)) + { + // 观测到该点的帧数加1 + pMP->IncreaseVisible(); + // 只有在视野范围内的地图点才参与之后的投影匹配 + nToMatch++; + } + if(pMP->mbTrackInView) + { + mCurrentFrame.mmProjectPoints[pMP->mnId] = cv::Point2f(pMP->mTrackProjX, pMP->mTrackProjY); + } + } + + // Step 3：如果需要进行投影匹配的点的数目大于0，就进行投影匹配，增加更多的匹配关系 + if(nToMatch>0) + { + ORBmatcher matcher(0.8); + int th = 1; + if(mSensor==System::RGBD || mSensor==System::IMU_RGBD) // RGBD相机输入的时候,搜索的阈值会变得稍微大一些 + th=3; + if(mpAtlas->isImuInitialized()) + { + if(mpAtlas->GetCurrentMap()->GetIniertialBA2()) + th=2; + else + th=6; // 0.4版本这里是3 + } + else if(!mpAtlas->isImuInitialized() && (mSensor==System::IMU_MONOCULAR || mSensor==System::IMU_STEREO || mSensor == System::IMU_RGBD)) + { + th=10; + } + + // If the camera has been relocalised recently, perform a coarser search + // 如果不久前进行过重定位，那么进行一个更加宽泛的搜索，阈值需要增大 + if(mCurrentFrame.mnIdmbFarPoints, mpLocalMapper->mThFarPoints); + } +} + +/** + * @brief 更新LocalMap + * + * 局部地图包括： + * 1、K1个关键帧、K2个临近关键帧和参考关键帧 + * 2、由这些关键帧观测到的MapPoints + */ +void Tracking::UpdateLocalMap() +{ + // This is for visualization + // 设置参考地图点用于绘图显示局部地图点（红色） + mpAtlas->SetReferenceMapPoints(mvpLocalMapPoints); + + // Update + // 用共视图来更新局部关键帧和局部地图点 + UpdateLocalKeyFrames(); + UpdateLocalPoints(); +} + +/* + * @brief 更新局部关键点。先把局部地图清空，然后将局部关键帧的有效地图点添加到局部地图中 + */ +void Tracking::UpdateLocalPoints() +{ + // Step 1：清空局部地图点 + mvpLocalMapPoints.clear(); + + int count_pts = 0; + + // Step 2：遍历局部关键帧 mvpLocalKeyFrames + for(vector::const_reverse_iterator itKF=mvpLocalKeyFrames.rbegin(), itEndKF=mvpLocalKeyFrames.rend(); itKF!=itEndKF; ++itKF) + { + KeyFrame* pKF = *itKF; + const vector vpMPs = pKF->GetMapPointMatches(); + + // step 2：将局部关键帧的地图点添加到mvpLocalMapPoints + for(vector::const_iterator itMP=vpMPs.begin(), itEndMP=vpMPs.end(); itMP!=itEndMP; itMP++) + { + + MapPoint* pMP = *itMP; + if(!pMP) + continue; + // 用该地图点的成员变量mnTrackReferenceForFrame 记录当前帧的id + // 表示它已经是当前帧的局部地图点了，可以防止重复添加局部地图点 + if(pMP->mnTrackReferenceForFrame==mCurrentFrame.mnId) + continue; + if(!pMP->isBad()) + { + count_pts++; + mvpLocalMapPoints.push_back(pMP); + pMP->mnTrackReferenceForFrame=mCurrentFrame.mnId; + } + } + } +} + +/** + * @brief 跟踪局部地图函数里，更新局部关键帧 + * 方法是遍历当前帧的地图点，将观测到这些地图点的关键帧和相邻的关键帧及其父子关键帧，作为mvpLocalKeyFrames + * Step 1：遍历当前帧的地图点，记录所有能观测到当前帧地图点的关键帧 + * Step 2：更新局部关键帧（mvpLocalKeyFrames），添加局部关键帧包括以下3种类型 + * 类型1：能观测到当前帧地图点的关键帧，也称一级共视关键帧 + * 类型2：一级共视关键帧的共视关键帧，称为二级共视关键帧 + * 类型3：一级共视关键帧的子关键帧、父关键帧 + * Step 3：更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧 + */ +void Tracking::UpdateLocalKeyFrames() +{ + // Each map point vote for the keyframes in which it has been observed + // Step 1：遍历当前帧的地图点，记录所有能观测到当前帧地图点的关键帧 + map keyframeCounter; + // 如果IMU未初始化 或者 刚刚完成重定位 + if(!mpAtlas->isImuInitialized() || (mCurrentFrame.mnIdisBad()) + { + // 得到观测到该地图点的关键帧和该地图点在关键帧中的索引 + const map> observations = pMP->GetObservations(); + // 由于一个地图点可以被多个关键帧观测到,因此对于每一次观测,都对观测到这个地图点的关键帧进行累计投票 + for(map>::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++) + // 这里的操作非常精彩！ + // map[key] = value，当要插入的键存在时，会覆盖键对应的原来的值。如果键不存在，则添加一组键值对 + // it->first 是地图点看到的关键帧，同一个关键帧看到的地图点会累加到该关键帧计数 + // 所以最后keyframeCounter 第一个参数表示某个关键帧，第2个参数表示该关键帧看到了多少当前帧(mCurrentFrame)的地图点，也就是共视程度 + keyframeCounter[it->first]++; + } + else + { + mCurrentFrame.mvpMapPoints[i]=NULL; + } + } + } + } + else + { + // ?为什么IMU初始化后用mLastFrame？mLastFrame存储的是上一帧跟踪成功后帧数据。 + for(int i=0; iisBad()) + { + const map> observations = pMP->GetObservations(); + for(map>::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++) + keyframeCounter[it->first]++; + } + else + { + // MODIFICATION + mLastFrame.mvpMapPoints[i]=NULL; + } + } + } + } + + // 存储具有最多观测次数（max）的关键帧 + int max=0; + KeyFrame* pKFmax= static_cast(NULL); + + // Step 2：更新局部关键帧（mvpLocalKeyFrames），添加局部关键帧有3种类型 + // 先清空局部关键帧 + mvpLocalKeyFrames.clear(); + // 先申请3倍内存，不够后面再加 + mvpLocalKeyFrames.reserve(3*keyframeCounter.size()); + + // All keyframes that observe a map point are included in the local map. Also check which keyframe shares most points + // Step 2.1 类型1：能观测到当前帧地图点的关键帧作为局部关键帧 （将邻居拉拢入伙）（一级共视关键帧） + for(map::const_iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++) + { + KeyFrame* pKF = it->first; + + // 如果设定为要删除的，跳过 + if(pKF->isBad()) + continue; + + // 寻找具有最大观测数目的关键帧 + if(it->second>max) + { + max=it->second; + pKFmax=pKF; + } + + // 添加到局部关键帧的列表里 + mvpLocalKeyFrames.push_back(pKF); + // 用该关键帧的成员变量mnTrackReferenceForFrame 记录当前帧的id + // 表示它已经是当前帧的局部关键帧了，可以防止重复添加局部关键帧 + pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId; + } + + // Include also some not-already-included keyframes that are neighbors to already-included keyframes + // Step 2.2 遍历一级共视关键帧，寻找更多的局部关键帧 + for(vector::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++) + { + // Limit the number of keyframes + // 处理的局部关键帧不超过80帧 + if(mvpLocalKeyFrames.size()>80) // 80 + break; + + KeyFrame* pKF = *itKF; + + // 类型2:一级共视关键帧的共视（前10个）关键帧，称为二级共视关键帧（将邻居的邻居拉拢入伙） + // 如果共视帧不足10帧,那么就返回所有具有共视关系的关键帧 + const vector vNeighs = pKF->GetBestCovisibilityKeyFrames(10); + + // vNeighs 是按照共视程度从大到小排列 + for(vector::const_iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++) + { + KeyFrame* pNeighKF = *itNeighKF; + if(!pNeighKF->isBad()) + { + // mnTrackReferenceForFrame防止重复添加局部关键帧 + if(pNeighKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId) + { + mvpLocalKeyFrames.push_back(pNeighKF); + pNeighKF->mnTrackReferenceForFrame=mCurrentFrame.mnId; + break; + } + } + } + + // 类型3:将一级共视关键帧的子关键帧作为局部关键帧（将邻居的孩子们拉拢入伙） + const set spChilds = pKF->GetChilds(); + for(set::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++) + { + KeyFrame* pChildKF = *sit; + if(!pChildKF->isBad()) + { + if(pChildKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId) + { + mvpLocalKeyFrames.push_back(pChildKF); + pChildKF->mnTrackReferenceForFrame=mCurrentFrame.mnId; + break; + } + } + } + + // 类型3:将一级共视关键帧的父关键帧（将邻居的父母们拉拢入伙） + KeyFrame* pParent = pKF->GetParent(); + if(pParent) + { + // mnTrackReferenceForFrame防止重复添加局部关键帧 + if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId) + { + mvpLocalKeyFrames.push_back(pParent); + pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId; + break; + } + } + } + + // Add 10 last temporal KFs (mainly for IMU) + // IMU模式下增加了临时的关键帧 + if((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) &&mvpLocalKeyFrames.size()<80) + { + KeyFrame* tempKeyFrame = mCurrentFrame.mpLastKeyFrame; + + const int Nd = 20; + for(int i=0; imnTrackReferenceForFrame!=mCurrentFrame.mnId) + { + mvpLocalKeyFrames.push_back(tempKeyFrame); + tempKeyFrame->mnTrackReferenceForFrame=mCurrentFrame.mnId; + tempKeyFrame=tempKeyFrame->mPrevKF; + } + } + } + + // Step 3：更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧 + if(pKFmax) + { + mpReferenceKF = pKFmax; + mCurrentFrame.mpReferenceKF = mpReferenceKF; + } +} + +/** + * @details 重定位过程 + * @return true + * @return false + * + * Step 1：计算当前帧特征点的词袋向量 + * Step 2：找到与当前帧相似的候选关键帧 + * Step 3：通过BoW进行匹配 + * Step 4：通过EPnP算法估计姿态 + * Step 5：通过PoseOptimization对姿态进行优化求解 + * Step 6：如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解 + */ +bool Tracking::Relocalization() +{ + Verbose::PrintMess("Starting relocalization", Verbose::VERBOSITY_NORMAL); + // Compute Bag of Words Vector + // Step 1: 计算当前帧特征点的Bow映射 + mCurrentFrame.ComputeBoW(); + + // Relocalization is performed when tracking is lost + // Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation + // Step 2：找到与当前帧相似的候选关键帧组 + vector vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame, mpAtlas->GetCurrentMap()); + + if(vpCandidateKFs.empty()) { + Verbose::PrintMess("There are not candidates", Verbose::VERBOSITY_NORMAL); + return false; + } + + const int nKFs = vpCandidateKFs.size(); + + // We perform first an ORB matching with each candidate + // If enough matches are found we setup a PnP solver + ORBmatcher matcher(0.75,true); + + // 每个关键帧的解算器 + vector vpMLPnPsolvers; + vpMLPnPsolvers.resize(nKFs); + + // 每个关键帧和当前帧中特征点的匹配关系 + vector > vvpMapPointMatches; + vvpMapPointMatches.resize(nKFs); + + // 放弃某个关键帧的标记 + vector vbDiscarded; + vbDiscarded.resize(nKFs); + + // 有效的候选关键帧数目 + int nCandidates=0; + + // Step 3：遍历所有的候选关键帧，通过BoW进行快速匹配，用匹配结果初始化PnP Solver + for(int i=0; iisBad()) + vbDiscarded[i] = true; + else + { + // 当前帧和候选关键帧用BoW进行快速匹配，匹配结果记录在vvpMapPointMatches，nmatches表示匹配的数目 + int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]); + // 如果和当前帧的匹配数小于15,那么只能放弃这个关键帧 + if(nmatches<15) + { + vbDiscarded[i] = true; + continue; + } + else + { + // 如果匹配数目够用，用匹配结果初始化MLPnPsolver + // ? 为什么用MLPnP? 因为考虑了鱼眼相机模型，解耦某些关系？ + // 参考论文《MLPNP-A REAL-TIME MAXIMUM LIKELIHOOD SOLUTION TO THE PERSPECTIVE-N-POINT PROBLEM》 + MLPnPsolver* pSolver = new MLPnPsolver(mCurrentFrame,vvpMapPointMatches[i]); + // 构造函数调用了一遍，这里重新设置参数 + pSolver->SetRansacParameters( + 0.99, // 模型最大概率值，默认0.9 + 10, // 内点的最小阈值，默认8 + 300, // 最大迭代次数，默认300 + 6, // 最小集，每次采样六个点，即最小集应该设置为6，论文里面写着I > 5 + 0.5, // 理论最少内点个数，这里是按照总数的比例计算，所以epsilon是比例，默认是0.4 + 5.991); // 卡方检验阈值 //This solver needs at least 6 points + vpMLPnPsolvers[i] = pSolver; + nCandidates++; // 1.0版本新加的 + } + } + } + + // Alternatively perform some iterations of P4P RANSAC + // Until we found a camera pose supported by enough inliers + // 足够的内点才能匹配使用PNP算法，MLPnP需要至少6个点 + // 是否已经找到相匹配的关键帧的标志 + bool bMatch = false; + ORBmatcher matcher2(0.9,true); + + // Step 4: 通过一系列操作,直到找到能够匹配上的关键帧 + // 为什么搞这么复杂？答：是担心误闭环 + while(nCandidates>0 && !bMatch) + { + // 遍历当前所有的候选关键帧 + for(int i=0; i vbInliers; + // 内点数 + int nInliers; + // 表示RANSAC已经没有更多的迭代次数可用 -- 也就是说数据不够好，RANSAC也已经尽力了。。。 + bool bNoMore; + + // Step 4.1：通过MLPnP算法估计姿态，迭代5次 + MLPnPsolver* pSolver = vpMLPnPsolvers[i]; + Eigen::Matrix4f eigTcw; + // PnP算法的入口函数 + bool bTcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers, eigTcw); + + // If Ransac reachs max. iterations discard keyframe + // bNoMore 为true 表示已经超过了RANSAC最大迭代次数，就放弃当前关键帧 + if(bNoMore) + { + vbDiscarded[i]=true; + nCandidates--; + } + + // If a Camera Pose is computed, optimize + if(bTcw) + { + // Step 4.2：如果MLPnP 计算出了位姿，对内点进行BA优化 + Sophus::SE3f Tcw(eigTcw); + mCurrentFrame.SetPose(Tcw); + // Tcw.copyTo(mCurrentFrame.mTcw); + // MLPnP 里RANSAC后的内点的集合 + set sFound; + + const int np = vbInliers.size(); + + // 遍历所有内点 + for(int j=0; j(NULL); + + // If few inliers, search by projection in a coarse window and optimize again + // Step 4.3：如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解 + // 前面的匹配关系是用词袋匹配过程得到的 + if(nGood<50) + { + // 通过投影的方式将关键帧中未匹配的地图点投影到当前帧中, 生成新的匹配 + int nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100); + + // 如果通过投影过程新增了比较多的匹配特征点对 + if(nadditional+nGood>=50) + { + // 根据投影匹配的结果，再次采用3D-2D pnp BA优化位姿 + nGood = Optimizer::PoseOptimization(&mCurrentFrame); + + // If many inliers but still not enough, search by projection again in a narrower window + // the camera has been already optimized with many points + // Step 4.4：如果BA后内点数还是比较少(<50)但是还不至于太少(>30)，可以挽救一下, 最后垂死挣扎 + // 重新执行上一步 4.3的过程，只不过使用更小的搜索窗口 + // 这里的位姿已经使用了更多的点进行了优化,应该更准，所以使用更小的窗口搜索 + if(nGood>30 && nGood<50) + { + // 用更小窗口、更严格的描述子阈值，重新进行投影搜索匹配 + sFound.clear(); + for(int ip =0; ip=50) + { + nGood = Optimizer::PoseOptimization(&mCurrentFrame); + // 更新地图点 + for(int io =0; io=50) + { + bMatch = true; + // 只要有一个候选关键帧重定位成功，就退出循环，不考虑其他候选关键帧了 + break; + } + } + }// 一直运行,知道已经没有足够的关键帧,或者是已经有成功匹配上的关键帧 + } + + // 折腾了这么久还是没有匹配上，重定位失败 + if(!bMatch) + { + return false; + } + else + { + // 如果匹配上了,说明当前帧重定位成功了(当前帧已经有了自己的位姿) + // 记录成功重定位帧的id，防止短时间多次重定位 + mnLastRelocFrameId = mCurrentFrame.mnId; + cout << "Relocalized!!" << endl; + return true; + } + +} + +/** + * @brief 整个追踪线程执行复位操作 + */ +void Tracking::Reset(bool bLocMap) +{ + Verbose::PrintMess("System Reseting", Verbose::VERBOSITY_NORMAL); + // 基本上是挨个请求各个线程终止 + if(mpViewer) + { + mpViewer->RequestStop(); + while(!mpViewer->isStopped()) + usleep(3000); + } + + // Reset Local Mapping + if (!bLocMap) + { + Verbose::PrintMess("Reseting Local Mapper...", Verbose::VERBOSITY_NORMAL); + mpLocalMapper->RequestReset(); + Verbose::PrintMess("done", Verbose::VERBOSITY_NORMAL); + } + + + // Reset Loop Closing + Verbose::PrintMess("Reseting Loop Closing...", Verbose::VERBOSITY_NORMAL); + mpLoopClosing->RequestReset(); + Verbose::PrintMess("done", Verbose::VERBOSITY_NORMAL); + + // Clear BoW Database + Verbose::PrintMess("Reseting Database...", Verbose::VERBOSITY_NORMAL); + mpKeyFrameDB->clear(); + Verbose::PrintMess("done", Verbose::VERBOSITY_NORMAL); + + // Clear Map (this erase MapPoints and KeyFrames) + mpAtlas->clearAtlas(); + mpAtlas->CreateNewMap(); + if (mSensor==System::IMU_STEREO || mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_RGBD) + mpAtlas->SetInertialSensor(); + mnInitialFrameId = 0; + + // 然后复位各种变量 + KeyFrame::nNextId = 0; + Frame::nNextId = 0; + mState = NO_IMAGES_YET; + + mbReadyToInitializate = false; + mbSetInit=false; + + mlRelativeFramePoses.clear(); + mlpReferences.clear(); + mlFrameTimes.clear(); + mlbLost.clear(); + mCurrentFrame = Frame(); + mnLastRelocFrameId = 0; + mLastFrame = Frame(); + mpReferenceKF = static_cast(NULL); + mpLastKeyFrame = static_cast(NULL); + mvIniMatches.clear(); + + if(mpViewer) + mpViewer->Release(); + + Verbose::PrintMess(" End reseting! ", Verbose::VERBOSITY_NORMAL); +} + +/** + * @brief 重置当前活动地图 + */ +void Tracking::ResetActiveMap(bool bLocMap) +{ + Verbose::PrintMess("Active map Reseting", Verbose::VERBOSITY_NORMAL); + if(mpViewer) + { + mpViewer->RequestStop(); + while(!mpViewer->isStopped()) + usleep(3000); + } + + Map* pMap = mpAtlas->GetCurrentMap(); + + if (!bLocMap) + { + Verbose::PrintMess("Reseting Local Mapper...", Verbose::VERBOSITY_VERY_VERBOSE); + mpLocalMapper->RequestResetActiveMap(pMap); + Verbose::PrintMess("done", Verbose::VERBOSITY_VERY_VERBOSE); + } + + // Reset Loop Closing + Verbose::PrintMess("Reseting Loop Closing...", Verbose::VERBOSITY_NORMAL); + mpLoopClosing->RequestResetActiveMap(pMap); + Verbose::PrintMess("done", Verbose::VERBOSITY_NORMAL); + + // Clear BoW Database + Verbose::PrintMess("Reseting Database", Verbose::VERBOSITY_NORMAL); + mpKeyFrameDB->clearMap(pMap); // Only clear the active map references + Verbose::PrintMess("done", Verbose::VERBOSITY_NORMAL); + + // Clear Map (this erase MapPoints and KeyFrames) + mpAtlas->clearMap(); + + + //KeyFrame::nNextId = mpAtlas->GetLastInitKFid(); + //Frame::nNextId = mnLastInitFrameId; + mnLastInitFrameId = Frame::nNextId; + //mnLastRelocFrameId = mnLastInitFrameId; + mState = NO_IMAGES_YET; //NOT_INITIALIZED; + + mbReadyToInitializate = false; + + list lbLost; + // lbLost.reserve(mlbLost.size()); + unsigned int index = mnFirstFrameId; + cout << "mnFirstFrameId = " << mnFirstFrameId << endl; + for(Map* pMap : mpAtlas->GetAllMaps()) + { + if(pMap->GetAllKeyFrames().size() > 0) + { + if(index > pMap->GetLowerKFID()) + index = pMap->GetLowerKFID(); + } + } + + //cout << "First Frame id: " << index << endl; + int num_lost = 0; + cout << "mnInitialFrameId = " << mnInitialFrameId << endl; + + for(list::iterator ilbL = mlbLost.begin(); ilbL != mlbLost.end(); ilbL++) + { + if(index < mnInitialFrameId) + lbLost.push_back(*ilbL); + else + { + lbLost.push_back(true); + num_lost += 1; + } + + index++; + } + cout << num_lost << " Frames set to lost" << endl; + + mlbLost = lbLost; + + mnInitialFrameId = mCurrentFrame.mnId; + mnLastRelocFrameId = mCurrentFrame.mnId; + + mCurrentFrame = Frame(); + mLastFrame = Frame(); + mpReferenceKF = static_cast(NULL); + mpLastKeyFrame = static_cast(NULL); + mvIniMatches.clear(); + + mbVelocity = false; + + if(mpViewer) + mpViewer->Release(); + + Verbose::PrintMess(" End reseting! ", Verbose::VERBOSITY_NORMAL); +} + +/** + * @brief 显示用的 + */ +vector Tracking::GetLocalMapMPS() +{ + return mvpLocalMapPoints; +} + +/** + * @brief 没用 + */ +void Tracking::ChangeCalibration(const string &strSettingPath) +{ + cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); + float fx = fSettings["Camera.fx"]; + float fy = fSettings["Camera.fy"]; + float cx = fSettings["Camera.cx"]; + float cy = fSettings["Camera.cy"]; + + mK_.setIdentity(); + mK_(0,0) = fx; + mK_(1,1) = fy; + mK_(0,2) = cx; + mK_(1,2) = cy; + + cv::Mat K = cv::Mat::eye(3,3,CV_32F); + K.at(0,0) = fx; + K.at(1,1) = fy; + K.at(0,2) = cx; + K.at(1,2) = cy; + K.copyTo(mK); + + cv::Mat DistCoef(4,1,CV_32F); + DistCoef.at(0) = fSettings["Camera.k1"]; + DistCoef.at(1) = fSettings["Camera.k2"]; + DistCoef.at(2) = fSettings["Camera.p1"]; + DistCoef.at(3) = fSettings["Camera.p2"]; + const float k3 = fSettings["Camera.k3"]; + if(k3!=0) + { + DistCoef.resize(5); + DistCoef.at(4) = k3; + } + DistCoef.copyTo(mDistCoef); + + mbf = fSettings["Camera.bf"]; + + Frame::mbInitialComputations = true; +} + +void Tracking::InformOnlyTracking(const bool &flag) +{ + mbOnlyTracking = flag; +} + +/** + * @brief 更新了关键帧的位姿，但需要修改普通帧的位姿，因为正常跟踪需要普通帧 + * localmapping中初始化imu中使用，速度的走向（仅在imu模式使用），最开始速度定义于imu初始化时，每个关键帧都根据位移除以时间得到，经过非线性优化保存于KF中. + * 之后使用本函数，让上一帧与当前帧分别与他们对应的上一关键帧做速度叠加得到，后面新的frame速度由上一个帧速度决定，如果使用匀速模型（大多数情况下），通过imu积分更新速度。 + * 新的关键帧继承于对应帧 + * @param s 尺度 + * @param b 初始化后第一帧的偏置 + * @param pCurrentKeyFrame 当前关键帧 + */ +void Tracking::UpdateFrameIMU(const float s, const IMU::Bias &b, KeyFrame* pCurrentKeyFrame) +{ + Map * pMap = pCurrentKeyFrame->GetMap(); + unsigned int index = mnFirstFrameId; + + // 每一帧的参考关键帧 + list::iterator lRit = mlpReferences.begin(); + list::iterator lbL = mlbLost.begin(); // 对应帧是否跟踪丢失 + + // mlRelativeFramePoses 存放的是Tcr + // 三个变量一一对应 + // mlRelativeFramePoses用于输出位姿，因此初始化之前里面数据没有尺度，所以要更新下尺度 + for(auto lit=mlRelativeFramePoses.begin(),lend=mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lbL++) + { + if(*lbL) + continue; + + KeyFrame* pKF = *lRit; + + while(pKF->isBad()) + { + pKF = pKF->GetParent(); + } + + if(pKF->GetMap() == pMap) + { + (*lit).translation() *= s; + } + } + // 设置偏置 + mLastBias = b; + // 设置上一关键帧，如果说mpLastKeyFrame已经是经过添加的新的kf，而pCurrentKeyFrame还是上一个kf，mpLastKeyFrame直接指向之前的kf + mpLastKeyFrame = pCurrentKeyFrame; + // 更新偏置 + mLastFrame.SetNewBias(mLastBias); + mCurrentFrame.SetNewBias(mLastBias); + + while(!mCurrentFrame.imuIsPreintegrated()) + { + // 当前帧需要预积分完毕，这段函数实在localmapping里调用的 + usleep(500); + } + + // TODO 如果上一帧正好是上一帧的上一关键帧（mLastFrame.mpLastKeyFrame与mLastFrame不可能是一个，可以验证一下） + if(mLastFrame.mnId == mLastFrame.mpLastKeyFrame->mnFrameId) + { + mLastFrame.SetImuPoseVelocity(mLastFrame.mpLastKeyFrame->GetImuRotation(), + mLastFrame.mpLastKeyFrame->GetImuPosition(), + mLastFrame.mpLastKeyFrame->GetVelocity()); + } + else + { + const Eigen::Vector3f Gz(0, 0, -IMU::GRAVITY_VALUE); + const Eigen::Vector3f twb1 = mLastFrame.mpLastKeyFrame->GetImuPosition(); + const Eigen::Matrix3f Rwb1 = mLastFrame.mpLastKeyFrame->GetImuRotation(); + const Eigen::Vector3f Vwb1 = mLastFrame.mpLastKeyFrame->GetVelocity(); + float t12 = mLastFrame.mpImuPreintegrated->dT; + // 根据mLastFrame的上一个关键帧的信息（此时已经经过imu初始化了，所以关键帧的信息都是校正后的）以及imu的预积分重新计算上一帧的位姿 + mLastFrame.SetImuPoseVelocity(IMU::NormalizeRotation(Rwb1*mLastFrame.mpImuPreintegrated->GetUpdatedDeltaRotation()), + twb1 + Vwb1*t12 + 0.5f*t12*t12*Gz+ Rwb1*mLastFrame.mpImuPreintegrated->GetUpdatedDeltaPosition(), + Vwb1 + Gz*t12 + Rwb1*mLastFrame.mpImuPreintegrated->GetUpdatedDeltaVelocity()); + } + + // 当前帧是否做了预积分 + if (mCurrentFrame.mpImuPreintegrated) + { + const Eigen::Vector3f Gz(0, 0, -IMU::GRAVITY_VALUE); + + const Eigen::Vector3f twb1 = mCurrentFrame.mpLastKeyFrame->GetImuPosition(); + const Eigen::Matrix3f Rwb1 = mCurrentFrame.mpLastKeyFrame->GetImuRotation(); + const Eigen::Vector3f Vwb1 = mCurrentFrame.mpLastKeyFrame->GetVelocity(); + float t12 = mCurrentFrame.mpImuPreintegrated->dT; + + mCurrentFrame.SetImuPoseVelocity(IMU::NormalizeRotation(Rwb1*mCurrentFrame.mpImuPreintegrated->GetUpdatedDeltaRotation()), + twb1 + Vwb1*t12 + 0.5f*t12*t12*Gz+ Rwb1*mCurrentFrame.mpImuPreintegrated->GetUpdatedDeltaPosition(), + Vwb1 + Gz*t12 + Rwb1*mCurrentFrame.mpImuPreintegrated->GetUpdatedDeltaVelocity()); + } + + mnFirstImuFrameId = mCurrentFrame.mnId; +} + +void Tracking::NewDataset() +{ + mnNumDataset++; +} + +int Tracking::GetNumberDataset() +{ + return mnNumDataset; +} + +int Tracking::GetMatchesInliers() +{ + return mnMatchesInliers; +} + +void Tracking::SaveSubTrajectory(string strNameFile_frames, string strNameFile_kf, string strFolder) +{ + mpSystem->SaveTrajectoryEuRoC(strFolder + strNameFile_frames); + //mpSystem->SaveKeyFrameTrajectoryEuRoC(strFolder + strNameFile_kf); +} + +void Tracking::SaveSubTrajectory(string strNameFile_frames, string strNameFile_kf, Map* pMap) +{ + mpSystem->SaveTrajectoryEuRoC(strNameFile_frames, pMap); + if(!strNameFile_kf.empty()) + mpSystem->SaveKeyFrameTrajectoryEuRoC(strNameFile_kf, pMap); +} + +float Tracking::GetImageScale() +{ + return mImageScale; +} + +#ifdef REGISTER_LOOP +void Tracking::RequestStop() +{ + unique_lock lock(mMutexStop); + mbStopRequested = true; +} + +bool Tracking::Stop() +{ + unique_lock lock(mMutexStop); + if(mbStopRequested && !mbNotStop) + { + mbStopped = true; + cout << "Tracking STOP" << endl; + return true; + } + + return false; +} + +bool Tracking::stopRequested() +{ + unique_lock lock(mMutexStop); + return mbStopRequested; +} + +bool Tracking::isStopped() +{ + unique_lock lock(mMutexStop); + return mbStopped; +} + +void Tracking::Release() +{ + unique_lock lock(mMutexStop); + mbStopped = false; + mbStopRequested = false; +} +#endif + +} //namespace ORB_SLAM diff --git a/ORB-SLAM3-UESTC/Workspace/src/TwoViewReconstruction.cc b/ORB-SLAM3-UESTC/Workspace/src/TwoViewReconstruction.cc new file mode 100644 index 0000000..208da82 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/TwoViewReconstruction.cc @@ -0,0 +1,1223 @@ +/** + * This file is part of ORB-SLAM3 + * + * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * + * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public + * License as published by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even + * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with ORB-SLAM3. + * If not, see . + */ + +#include "TwoViewReconstruction.h" + +#include "Converter.h" +#include "GeometricTools.h" + +#include "Thirdparty/DBoW2/DUtils/Random.h" + +#include + +using namespace std; +namespace ORB_SLAM3 +{ +TwoViewReconstruction::TwoViewReconstruction(const Eigen::Matrix3f &k, float sigma, int iterations) +{ + mK = k; + + mSigma = sigma; + mSigma2 = sigma * sigma; + mMaxIterations = iterations; +} + +/** + * @brief 单目初始化重要的环节，先获得rt在通过三角化恢复3d点坐标 + * @param vKeys1 第一帧的关键点 + * @param vKeys2 第二帧的关键点 + * @param vMatches12 匹配关系，长度与vKeys1一样，对应位置存放vKeys2中关键点的下标 + * @param T21 顾名思义 + * @param vP3D 恢复出的三维点 + * @param vbTriangulated 是否三角化成功，用于统计匹配点数量 + */ +bool TwoViewReconstruction::Reconstruct( + const std::vector &vKeys1, const std::vector &vKeys2, const vector &vMatches12, + Sophus::SE3f &T21, vector &vP3D, vector &vbTriangulated) +{ + // 1. 准备工作，提取匹配关系及准备RANSAC + mvKeys1.clear(); + mvKeys2.clear(); + + mvKeys1 = vKeys1; + mvKeys2 = vKeys2; + + // Fill structures with current keypoints and matches with reference frame + // Reference Frame: 1, Current Frame: 2 + // 填写mvMatches12，里面存放的是vKeys1，vKeys2匹配点的索引 + // 存放匹配点的id + mvMatches12.clear(); + mvMatches12.reserve(mvKeys2.size()); + // 长度与vKeys1，表示对应位置的vKeys1中的点是否有匹配关系 + mvbMatched1.resize(mvKeys1.size()); + for (size_t i = 0, iend = vMatches12.size(); i < iend; i++) + { + if (vMatches12[i] >= 0) + { + mvMatches12.push_back(make_pair(i, vMatches12[i])); + mvbMatched1[i] = true; + } + else + mvbMatched1[i] = false; + } + + const int N = mvMatches12.size(); + + // Indices for minimum set selection + vector vAllIndices; + vAllIndices.reserve(N); + vector vAvailableIndices; + + // 使用vAllIndices为了保证8个点选不到同一个点 + for (int i = 0; i < N; i++) + { + vAllIndices.push_back(i); + } + + // Generate sets of 8 points for each RANSAC iteration + // 默认200次 + mvSets = vector>(mMaxIterations, vector(8, 0)); + + DUtils::Random::SeedRandOnce(0); + + // 2. 先遍历把200次先取好 + for (int it = 0; it < mMaxIterations; it++) + { + vAvailableIndices = vAllIndices; + + // Select a minimum set + for (size_t j = 0; j < 8; j++) + { + int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size() - 1); + int idx = vAvailableIndices[randi]; // 这句不多余，防止重复选择 + + mvSets[it][j] = idx; + + // 保证选不到同一个点，这么做的话可以删去vAvailableIndices已选点的索引 + vAvailableIndices[randi] = vAvailableIndices.back(); + vAvailableIndices.pop_back(); + } + } + + // Launch threads to compute in parallel a fundamental matrix and a homography + vector vbMatchesInliersH, vbMatchesInliersF; + float SH, SF; + Eigen::Matrix3f H, F; + + // 3. 双线程分别计算 + // 加ref为了提供引用 + thread threadH(&TwoViewReconstruction::FindHomography, this, ref(vbMatchesInliersH), ref(SH), ref(H)); + thread threadF(&TwoViewReconstruction::FindFundamental, this, ref(vbMatchesInliersF), ref(SF), ref(F)); + + // Wait until both threads have finished + threadH.join(); + threadF.join(); + + // Compute ratio of scores + if (SH + SF == 0.f) + return false; + float RH = SH / (SH + SF); + + float minParallax = 1.0; + + // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45) + // 4. 看哪个分高用哪个，分别是通过H重建与通过F重建 + // ORBSLAM2这里的值是0.4， TOSEE + if (RH > 0.50) // if(RH>0.40) + { + // cout << "Initialization from Homography" << endl; + return ReconstructH(vbMatchesInliersH, H, mK, T21, vP3D, vbTriangulated, minParallax, 50); + } + else // if(pF_HF>0.6) + { + // cout << "Initialization from Fundamental" << endl; + return ReconstructF(vbMatchesInliersF, F, mK, T21, vP3D, vbTriangulated, minParallax, 50); + } +} + +/** + * @brief 计算H矩阵，同时计算得分与内点 + * @param vbMatchesInliers 经过H矩阵验证，是否为内点，大小为mvMatches12 + * @param score 得分 + * @param H21 1到2的H矩阵 + */ +void TwoViewReconstruction::FindHomography(vector &vbMatchesInliers, float &score, Eigen::Matrix3f &H21) +{ + // Number of putative matches + // 匹配成功的个数 + const int N = mvMatches12.size(); + + // Normalize coordinates + vector vPn1, vPn2; + Eigen::Matrix3f T1, T2; + // 像素坐标标准化，先去均值，再除均长 + Normalize(mvKeys1, vPn1, T1); + Normalize(mvKeys2, vPn2, T2); + Eigen::Matrix3f T2inv = T2.inverse(); + + // Best Results variables + score = 0.0; + vbMatchesInliers = vector(N, false); + + // Iteration variables + vector vPn1i(8); + vector vPn2i(8); + Eigen::Matrix3f H21i, H12i; + vector vbCurrentInliers(N, false); + float currentScore; + + // Perform all RANSAC iterations and save the solution with highest score + // 计算归一化后的H矩阵 p2 = H21*p1 + for (int it = 0; it < mMaxIterations; it++) + { + // Select a minimum set + for (size_t j = 0; j < 8; j++) + { + int idx = mvSets[it][j]; + + vPn1i[j] = vPn1[mvMatches12[idx].first]; + vPn2i[j] = vPn2[mvMatches12[idx].second]; + } + // 计算标准化后的H矩阵 + Eigen::Matrix3f Hn = ComputeH21(vPn1i, vPn2i); + // 恢复正常H + H21i = T2inv * Hn * T1; + H12i = H21i.inverse(); + + currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma); // mSigma默认为1 + + if (currentScore > score) + { + H21 = H21i; + vbMatchesInliers = vbCurrentInliers; + score = currentScore; + } + } +} + +/** + * @brief 计算F矩阵，同时计算得分与内点 + * @param vbMatchesInliers 经过F矩阵验证，是否为内点，大小为mvMatches12 + * @param score 得分 + * @param F21 1到2的F矩阵 + */ +void TwoViewReconstruction::FindFundamental(vector &vbMatchesInliers, float &score, Eigen::Matrix3f &F21) +{ + // Number of putative matches + const int N = vbMatchesInliers.size(); + + // Normalize coordinates + vector vPn1, vPn2; + Eigen::Matrix3f T1, T2; + Normalize(mvKeys1, vPn1, T1); + Normalize(mvKeys2, vPn2, T2); + Eigen::Matrix3f T2t = T2.transpose(); + + // Best Results variables + score = 0.0; + vbMatchesInliers = vector(N, false); + + // Iteration variables + vector vPn1i(8); + vector vPn2i(8); + Eigen::Matrix3f F21i; + vector vbCurrentInliers(N, false); + float currentScore; + + // Perform all RANSAC iterations and save the solution with highest score + for (int it = 0; it < mMaxIterations; it++) + { + // Select a minimum set + for (int j = 0; j < 8; j++) + { + int idx = mvSets[it][j]; + + vPn1i[j] = vPn1[mvMatches12[idx].first]; + vPn2i[j] = vPn2[mvMatches12[idx].second]; + } + + Eigen::Matrix3f Fn = ComputeF21(vPn1i, vPn2i); + + // FN得到的是基于归一化坐标的F矩阵，必须乘上归一化过程矩阵才是最后的基于像素坐标的F + F21i = T2t * Fn * T1; + + currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma); + + if (currentScore > score) + { + F21 = F21i; + vbMatchesInliers = vbCurrentInliers; + score = currentScore; + } + } +} + +/** + * @brief 从特征点匹配求homography（normalized DLT） + * |x'| | h1 h2 h3 ||x| + * |y'| = a | h4 h5 h6 ||y| 简写: x' = a H x, a为一个尺度因子 + * |1 | | h7 h8 h9 ||1| + * 使用DLT(direct linear tranform)求解该模型 + * x' = a H x + * ---> (x') 叉乘 (H x) = 0 + * ---> Ah = 0 + * A = | 0 0 0 -x -y -1 xy' yy' y'| h = | h1 h2 h3 h4 h5 h6 h7 h8 h9 | + * |-x -y -1 0 0 0 xx' yx' x'| + * 通过SVD求解Ah = 0，A'A最小特征值对应的特征向量即为解 + * @param vP1 归一化后的点, in reference frame + * @param vP2 归一化后的点, in current frame + * @return 单应矩阵 + * @see Multiple View Geometry in Computer Vision - Algorithm 4.2 p109 + */ +Eigen::Matrix3f TwoViewReconstruction::ComputeH21(const vector &vP1, const vector &vP2) +{ + const int N = vP1.size(); + + Eigen::MatrixXf A(2 * N, 9); + + for (int i = 0; i < N; i++) + { + const float u1 = vP1[i].x; + const float v1 = vP1[i].y; + const float u2 = vP2[i].x; + const float v2 = vP2[i].y; + + A(2 * i, 0) = 0.0; + A(2 * i, 1) = 0.0; + A(2 * i, 2) = 0.0; + A(2 * i, 3) = -u1; + A(2 * i, 4) = -v1; + A(2 * i, 5) = -1; + A(2 * i, 6) = v2 * u1; + A(2 * i, 7) = v2 * v1; + A(2 * i, 8) = v2; + + A(2 * i + 1, 0) = u1; + A(2 * i + 1, 1) = v1; + A(2 * i + 1, 2) = 1; + A(2 * i + 1, 3) = 0.0; + A(2 * i + 1, 4) = 0.0; + A(2 * i + 1, 5) = 0.0; + A(2 * i + 1, 6) = -u2 * u1; + A(2 * i + 1, 7) = -u2 * v1; + A(2 * i + 1, 8) = -u2; + } + + Eigen::JacobiSVD svd(A, Eigen::ComputeFullV); + + Eigen::Matrix H(svd.matrixV().col(8).data()); + + return H; +} + +/** + * @brief 从特征点匹配求fundamental matrix（normalized 8点法） + * x'Fx = 0 整理可得：Af = 0 + * A = | x'x x'y x' y'x y'y y' x y 1 |, f = | f1 f2 f3 f4 f5 f6 f7 f8 f9 | + * 通过SVD求解Af = 0，A'A最小特征值对应的特征向量即为解 + * @param vP1 归一化后的点, in reference frame + * @param vP2 归一化后的点, in current frame + * @return 基础矩阵 + * @see Multiple View Geometry in Computer Vision - Algorithm 11.1 p282 (中文版 p191) + */ +Eigen::Matrix3f TwoViewReconstruction::ComputeF21(const vector &vP1, const vector &vP2) +{ + const int N = vP1.size(); + + Eigen::MatrixXf A(N, 9); + + for (int i = 0; i < N; i++) + { + const float u1 = vP1[i].x; + const float v1 = vP1[i].y; + const float u2 = vP2[i].x; + const float v2 = vP2[i].y; + + A(i, 0) = u2 * u1; + A(i, 1) = u2 * v1; + A(i, 2) = u2; + A(i, 3) = v2 * u1; + A(i, 4) = v2 * v1; + A(i, 5) = v2; + A(i, 6) = u1; + A(i, 7) = v1; + A(i, 8) = 1; + } + + Eigen::JacobiSVD svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); + + Eigen::Matrix Fpre(svd.matrixV().col(8).data()); + + Eigen::JacobiSVD svd2(Fpre, Eigen::ComputeFullU | Eigen::ComputeFullV); + + Eigen::Vector3f w = svd2.singularValues(); + // 这里注意计算完要强制让第三个奇异值为0 + w(2) = 0; + + return svd2.matrixU() * Eigen::DiagonalMatrix(w) * svd2.matrixV().transpose(); +} + +/** + * @brief 检查结果 + * @param H21 顾名思义 + * @param H12 顾名思义 + * @param vbMatchesInliers 匹配是否合法，大小为mvMatches12 + * @param sigma 默认为1 + */ +float TwoViewReconstruction::CheckHomography(const Eigen::Matrix3f &H21, const Eigen::Matrix3f &H12, vector &vbMatchesInliers, float sigma) +{ + const int N = mvMatches12.size(); + + const float h11 = H21(0, 0); + const float h12 = H21(0, 1); + const float h13 = H21(0, 2); + const float h21 = H21(1, 0); + const float h22 = H21(1, 1); + const float h23 = H21(1, 2); + const float h31 = H21(2, 0); + const float h32 = H21(2, 1); + const float h33 = H21(2, 2); + + const float h11inv = H12(0, 0); + const float h12inv = H12(0, 1); + const float h13inv = H12(0, 2); + const float h21inv = H12(1, 0); + const float h22inv = H12(1, 1); + const float h23inv = H12(1, 2); + const float h31inv = H12(2, 0); + const float h32inv = H12(2, 1); + const float h33inv = H12(2, 2); + + vbMatchesInliers.resize(N); + + float score = 0; + // 基于卡方检验计算出的阈值 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值 + const float th = 5.991; + + const float invSigmaSquare = 1.0 / (sigma * sigma); + + for (int i = 0; i < N; i++) + { + bool bIn = true; + + const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first]; + const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second]; + + const float u1 = kp1.pt.x; + const float v1 = kp1.pt.y; + const float u2 = kp2.pt.x; + const float v2 = kp2.pt.y; + + // Reprojection error in first image + // x2in1 = H12*x2 + + // 计算投影误差，2投1 1投2这么做，计算累计的卡方检验分数，分数越高证明内点与误差越优，这么做为了平衡误差与内点个数，不是说内点个数越高越好，也不是说误差越小越好 + const float w2in1inv = 1.0 / (h31inv * u2 + h32inv * v2 + h33inv); + const float u2in1 = (h11inv * u2 + h12inv * v2 + h13inv) * w2in1inv; + const float v2in1 = (h21inv * u2 + h22inv * v2 + h23inv) * w2in1inv; + + const float squareDist1 = (u1 - u2in1) * (u1 - u2in1) + (v1 - v2in1) * (v1 - v2in1); + + const float chiSquare1 = squareDist1 * invSigmaSquare; + + if (chiSquare1 > th) + bIn = false; + else + score += th - chiSquare1; + + // Reprojection error in second image + // x1in2 = H21*x1 + + const float w1in2inv = 1.0 / (h31 * u1 + h32 * v1 + h33); + const float u1in2 = (h11 * u1 + h12 * v1 + h13) * w1in2inv; + const float v1in2 = (h21 * u1 + h22 * v1 + h23) * w1in2inv; + + const float squareDist2 = (u2 - u1in2) * (u2 - u1in2) + (v2 - v1in2) * (v2 - v1in2); + + const float chiSquare2 = squareDist2 * invSigmaSquare; + + if (chiSquare2 > th) + bIn = false; + else + score += th - chiSquare2; + + if (bIn) + vbMatchesInliers[i] = true; + else + vbMatchesInliers[i] = false; + } + + return score; +} + +/** + * @brief 检查结果 + * @param F21 顾名思义 + * @param vbMatchesInliers 匹配是否合法，大小为mvMatches12 + * @param sigma 默认为1 + */ +float TwoViewReconstruction::CheckFundamental(const Eigen::Matrix3f &F21, vector &vbMatchesInliers, float sigma) +{ + const int N = mvMatches12.size(); + + const float f11 = F21(0, 0); + const float f12 = F21(0, 1); + const float f13 = F21(0, 2); + const float f21 = F21(1, 0); + const float f22 = F21(1, 1); + const float f23 = F21(1, 2); + const float f31 = F21(2, 0); + const float f32 = F21(2, 1); + const float f33 = F21(2, 2); + + vbMatchesInliers.resize(N); + + float score = 0; + + // 基于卡方检验计算出的阈值 自由度为1的卡方分布，显著性水平为0.05，对应的临界阈值 + const float th = 3.841; + // 基于卡方检验计算出的阈值 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值 + const float thScore = 5.991; + + const float invSigmaSquare = 1.0 / (sigma * sigma); + + for (int i = 0; i < N; i++) + { + bool bIn = true; + + const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first]; + const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second]; + + const float u1 = kp1.pt.x; + const float v1 = kp1.pt.y; + const float u2 = kp2.pt.x; + const float v2 = kp2.pt.y; + + // Reprojection error in second image + // l2=F21x1=(a2,b2,c2) + // 计算 img1 上的点在 img2 上投影得到的极线 l2 = F21 * p1 = (a2,b2,c2) + const float a2 = f11 * u1 + f12 * v1 + f13; + const float b2 = f21 * u1 + f22 * v1 + f23; + const float c2 = f31 * u1 + f32 * v1 + f33; + + // 计算误差 e = (a * p2.x + b * p2.y + c) / sqrt(a * a + b * b) + const float num2 = a2 * u2 + b2 * v2 + c2; + + const float squareDist1 = num2 * num2 / (a2 * a2 + b2 * b2); + + const float chiSquare1 = squareDist1 * invSigmaSquare; + + // 自由度为1是因为这里的计算是点到线的距离，判定分数自由度为2的原因可能是为了与H矩阵持平 + if (chiSquare1 > th) + bIn = false; + else + score += thScore - chiSquare1; + + // Reprojection error in second image + // l1 =x2tF21=(a1,b1,c1) + // 与上面相同只不过反过来了 + const float a1 = f11 * u2 + f21 * v2 + f31; + const float b1 = f12 * u2 + f22 * v2 + f32; + const float c1 = f13 * u2 + f23 * v2 + f33; + + const float num1 = a1 * u1 + b1 * v1 + c1; + + const float squareDist2 = num1 * num1 / (a1 * a1 + b1 * b1); + + const float chiSquare2 = squareDist2 * invSigmaSquare; + + if (chiSquare2 > th) + bIn = false; + else + score += thScore - chiSquare2; + + if (bIn) + vbMatchesInliers[i] = true; + else + vbMatchesInliers[i] = false; + } + + return score; +} + +/** + * @brief 通过基础矩阵重建 + * @param vbMatchesInliers 匹配是否合法，大小为mvMatches12 + * @param F21 顾名思义 + * @param K 相机内参 + * @param T21 旋转平移（要输出的） + * @param vP3D 恢复的三维点（要输出的） + * @param vbTriangulated 大小与mvKeys1一致，表示哪个点被重建了 + * @param minParallax 1 + * @param minTriangulated 50 + */ +bool TwoViewReconstruction::ReconstructF( + vector &vbMatchesInliers, Eigen::Matrix3f &F21, Eigen::Matrix3f &K, + Sophus::SE3f &T21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated) +{ + // 统计了合法的匹配，后面用于对比重建出的点数 + int N = 0; + for (size_t i = 0, iend = vbMatchesInliers.size(); i < iend; i++) + if (vbMatchesInliers[i]) + N++; + + // Compute Essential Matrix from Fundamental Matrix + // 1. 计算本质矩阵 + Eigen::Matrix3f E21 = K.transpose() * F21 * K; + + Eigen::Matrix3f R1, R2; + Eigen::Vector3f t; + + // Recover the 4 motion hypotheses + // 2. 分解本质矩阵，得到4对rt + DecomposeE(E21, R1, R2, t); + + Eigen::Vector3f t1 = t; + Eigen::Vector3f t2 = -t; + + // Reconstruct with the 4 hyphoteses and check + // 3. 使用四对结果分别重建 + vector vP3D1, vP3D2, vP3D3, vP3D4; + vector vbTriangulated1, vbTriangulated2, vbTriangulated3, vbTriangulated4; + float parallax1, parallax2, parallax3, parallax4; + + int nGood1 = CheckRT(R1, t1, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D1, 4.0 * mSigma2, vbTriangulated1, parallax1); + int nGood2 = CheckRT(R2, t1, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D2, 4.0 * mSigma2, vbTriangulated2, parallax2); + int nGood3 = CheckRT(R1, t2, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D3, 4.0 * mSigma2, vbTriangulated3, parallax3); + int nGood4 = CheckRT(R2, t2, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D4, 4.0 * mSigma2, vbTriangulated4, parallax4); + // 统计重建出点的数量最大值 + int maxGood = max(nGood1, max(nGood2, max(nGood3, nGood4))); + // 起码要重建出超过百分之90的匹配点 + int nMinGood = max(static_cast(0.9 * N), minTriangulated); + + // 4. 看看有没有脱颖而出的结果，且最大值要高于要求的最低值，如果大家都差不多说明有问题，因为4个结果中只有一个会正常 + // 大家都很多的话反而不正常了。。。 + int nsimilar = 0; + if (nGood1 > 0.7 * maxGood) + nsimilar++; + if (nGood2 > 0.7 * maxGood) + nsimilar++; + if (nGood3 > 0.7 * maxGood) + nsimilar++; + if (nGood4 > 0.7 * maxGood) + nsimilar++; + + // If there is not a clear winner or not enough triangulated points reject initialization + if (maxGood < nMinGood || nsimilar > 1) + { + return false; + } + + // If best reconstruction has enough parallax initialize + // 5. 使用最好的结果输出 + if (maxGood == nGood1) + { + if (parallax1 > minParallax) + { + vP3D = vP3D1; + vbTriangulated = vbTriangulated1; + + T21 = Sophus::SE3f(R1, t1); + return true; + } + } + else if (maxGood == nGood2) + { + if (parallax2 > minParallax) + { + vP3D = vP3D2; + vbTriangulated = vbTriangulated2; + + T21 = Sophus::SE3f(R2, t1); + return true; + } + } + else if (maxGood == nGood3) + { + if (parallax3 > minParallax) + { + vP3D = vP3D3; + vbTriangulated = vbTriangulated3; + + T21 = Sophus::SE3f(R1, t2); + return true; + } + } + else if (maxGood == nGood4) + { + if (parallax4 > minParallax) + { + vP3D = vP3D4; + vbTriangulated = vbTriangulated4; + + T21 = Sophus::SE3f(R2, t2); + return true; + } + } + + return false; +} + +/** + * @brief 从H恢复R t + * H矩阵分解常见有两种方法：Faugeras SVD-based decomposition 和 Zhang SVD-based decomposition + * 参考文献：Motion and structure from motion in a piecewise plannar environment + * 这篇参考文献和下面的代码使用了Faugeras SVD-based decomposition算法 + * @see + * - Faugeras et al, Motion and structure from motion in a piecewise planar environment. International Journal of Pattern Recognition and Artificial Intelligence, 1988. + * - Deeper understanding of the homography decomposition for vision-based control + * 设平面法向量 n = (a, b, c)^t 有一点(x, y, z)在平面上，则ax + by + cz = d 即 1/d * n^t * x = 1 其中d表示 + * x' = R*x + t 从下面开始x 与 x'都表示归一化坐标 + * λ2*x' = R*(λ1*x) + t = R*(λ1*x) + t * 1/d * n^t * (λ1*x) + * x' = λ*(R + t * 1/d * n^t) * x = H^ * x + * 对应图像坐标 u' = G * u G = KH^K.inv + * H^ ~= d*R + t * n^t = U∧V^t ∧ = U^t * H^ * V = d*U^t * R * V + (U^t * t) * (V^t * n)^t + * s = det(U) * det(V) s 有可能是 1 或 -1 ∧ = s^2 * d*U^t * R * V + (U^t * t) * (V^t * n)^t = (s*d) * s * U^t * R * V + (U^t * t) * (V^t * n)^t + * 令 R' = s * U^t * R * V t' = U^t * t n' = V^t * n d' = s * d + * ∧ = d' * R' + t' * n'^t 所以∧也可以认为是一个单应矩阵，其中加入s只为说明有符号相反的可能 + * 设∧ = | d1, 0, 0 | 取e1 = (1, 0, 0)^t e2 = (0, 1, 0)^t e3 = (0, 0, 1)^t + * | 0, d2, 0 | + * | 0, 0, d3 | + * n' = (a1, 0, 0)^t + (0, b1, 0)^t + (0, 0, c1)^t = a1*e1 + b1*e2 + c1*e3 + * + * ∧ = [d1*e1, d2*e2, d3*e3] = [d' * R' * e1, d' * R' * e2, d' * R' * e3] + [t' * a1, t' * b1, t' * c1] + * ==> d1*e1 = d' * R' * e1 + t' * a1 + * d2*e2 = d' * R' * e2 + t' * b1 + * d3*e3 = d' * R' * e3 + t' * c1 + * + * + * 上面式子每两个消去t可得 + * d'R'(b1e1 - a1e2) = d1b1e1 - d2a1e2 + * d'R'(c1e2 - b1e3) = d2c1e1 - d3b1e3 同时取二范数，因为旋转对二范数没影响，所以可以约去 + * d'R'(a1e3 - c1e1) = d3a1e3 - d1c1e1 + * + * (d'^2 - d2^2)*a1^2 + (d'^2 - d1^2)*b1^2 = 0 + * (d'^2 - d3^2)*b1^2 + (d'^2 - d2^2)*c1^2 = 0 令 d'^2 - d1^2 = x1 d'^2 - d2^2 = x2 d'^2 - d3^2 = x3 + * (d'^2 - d1^2)*c1^2 + (d'^2 - d3^2)*a1^2 = 0 + * + * + * | x2 x1 0 | | a1^2 | + * | 0 x3 x2 | * | b1^2 | = 0 ===> x1 * x2 * x3 = 0 有(d'^2 - d1^2) * (d'^2 - d2^2) * (d'^2 - d3^2) = 0 + * | x3 0 x1 | | c1^2 | + * 由于d1 >= d2 >= d3 所以d' = d2 or -d2 + * + * ----------------------------------------------------------------------------------------------------------------------------------------------------------------- + * 下面分情况讨论，当d' > 0 再根据a1^2 + b1^2 + c1^2 = 1 ??????哪来的根据，不晓得 + * 能够求得a1 = ε1 * sqrt((d1^2 - d2^2) / (d1^2 - d3^2)) + * b1 = 0 + * c1 = ε2 * sqrt((d2^2 - d3^2) / (d1^2 - d3^2)) 其中ε1 ε2 可以为正负1 + * 结果带入 d2*e2 = d' * R' * e2 + t' * b1 => R' * e2 = e2 + * | cosθ 0 -sinθ | + * ==> R' = | 0 1 0 | n' 与 R' 带入 d'R'(a1e3 - c1e1) = d3a1e3 - d1c1e1 + * | sinθ 0 cosθ | + * | cosθ 0 -sinθ | | -c1 | | -d1c1 | + * d' * | 0 1 0 | * | 0 | = | 0 | 能够解出 sinθ 与 cosθ + * | sinθ 0 cosθ | | a1 | | d3a1 | + * + * 到此为止得到了 R' 再根据 d1*e1 = d' * R' * e1 + t' * a1 + * d2*e2 = d' * R' * e2 + t' * b1 + * d3*e3 = d' * R' * e3 + t' * c1 + * + * 求得 t' = (d1 - d3) * (a1, 0, c1)^t + * @param vbMatchesInliers 匹配是否合法，大小为mvMatches12 + * @param H21 顾名思义 + * @param K 相机内参 + * @param T21 旋转平移（要输出的） + * @param vP3D 恢复的三维点（要输出的） + * @param vbTriangulated 大小与vbMatchesInliers，表示哪个点被重建了 + * @param minParallax 1 + * @param minTriangulated 50 + */ +bool TwoViewReconstruction::ReconstructH( + vector &vbMatchesInliers, Eigen::Matrix3f &H21, Eigen::Matrix3f &K, + Sophus::SE3f &T21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated) +{ + // 统计了合法的匹配，后面用于对比重建出的点数 + int N = 0; + for (size_t i = 0, iend = vbMatchesInliers.size(); i < iend; i++) + if (vbMatchesInliers[i]) + N++; + + // We recover 8 motion hypotheses using the method of Faugeras et al. + // Motion and structure from motion in a piecewise planar environment. + // International Journal of Pattern Recognition and Artificial Intelligence, 1988 + // step1：SVD分解Homography + // 因为特征点是图像坐标系，所以讲H矩阵由相机坐标系换算到图像坐标系 + Eigen::Matrix3f invK = K.inverse(); + Eigen::Matrix3f A = invK * H21 * K; + + Eigen::JacobiSVD svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Matrix3f U = svd.matrixU(); + Eigen::Matrix3f V = svd.matrixV(); + Eigen::Matrix3f Vt = V.transpose(); + Eigen::Vector3f w = svd.singularValues(); + + float s = U.determinant() * Vt.determinant(); + + float d1 = w(0); + float d2 = w(1); + float d3 = w(2); + + // SVD分解的正常情况是特征值降序排列 + if (d1 / d2 < 1.00001 || d2 / d3 < 1.00001) + { + return false; + } + + vector vR; + vector vt, vn; + vR.reserve(8); + vt.reserve(8); + vn.reserve(8); + + // n'=[x1 0 x3] 4 posibilities e1=e3=1, e1=1 e3=-1, e1=-1 e3=1, e1=e3=-1 + // step2：计算法向量 + // 法向量n'= [x1 0 x3] + float aux1 = sqrt((d1 * d1 - d2 * d2) / (d1 * d1 - d3 * d3)); + float aux3 = sqrt((d2 * d2 - d3 * d3) / (d1 * d1 - d3 * d3)); + float x1[] = {aux1, aux1, -aux1, -aux1}; + float x3[] = {aux3, -aux3, aux3, -aux3}; + + // case d'=d2 + // step3：恢复旋转矩阵 + // step3.1：计算 sin(theta)和cos(theta)，case d'=d2 + float aux_stheta = sqrt((d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)) / ((d1 + d3) * d2); + + float ctheta = (d2 * d2 + d1 * d3) / ((d1 + d3) * d2); + float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta}; + + // step3.2：计算四种旋转矩阵R，t + // 计算旋转矩阵 R‘，计算ppt中公式18 + // | ctheta 0 -aux_stheta| | aux1| + // Rp = | 0 1 0 | tp = | 0 | + // | aux_stheta 0 ctheta | |-aux3| + + // | ctheta 0 aux_stheta| | aux1| + // Rp = | 0 1 0 | tp = | 0 | + // |-aux_stheta 0 ctheta | | aux3| + + // | ctheta 0 aux_stheta| |-aux1| + // Rp = | 0 1 0 | tp = | 0 | + // |-aux_stheta 0 ctheta | |-aux3| + + // | ctheta 0 -aux_stheta| |-aux1| + // Rp = | 0 1 0 | tp = | 0 | + // | aux_stheta 0 ctheta | | aux3| + for (int i = 0; i < 4; i++) + { + Eigen::Matrix3f Rp; + Rp.setZero(); + Rp(0, 0) = ctheta; + Rp(0, 2) = -stheta[i]; + Rp(1, 1) = 1.f; + Rp(2, 0) = stheta[i]; + Rp(2, 2) = ctheta; + + Eigen::Matrix3f R = s * U * Rp * Vt; + vR.push_back(R); + + Eigen::Vector3f tp; + tp(0) = x1[i]; + tp(1) = 0; + tp(2) = -x3[i]; + tp *= d1 - d3; + + // 这里虽然对t有归一化，并没有决定单目整个SLAM过程的尺度 + // 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变 + Eigen::Vector3f t = U * tp; + vt.push_back(t / t.norm()); + + Eigen::Vector3f np; + np(0) = x1[i]; + np(1) = 0; + np(2) = x3[i]; + + Eigen::Vector3f n = V * np; + if (n(2) < 0) + n = -n; + vn.push_back(n); + } + + // case d'=-d2 + // step3.3：计算 sin(theta)和cos(theta)，case d'=-d2 + float aux_sphi = sqrt((d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)) / ((d1 - d3) * d2); + + float cphi = (d1 * d3 - d2 * d2) / ((d1 - d3) * d2); + float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi}; + + // step3.4：计算四种旋转矩阵R，t + // 计算旋转矩阵 R‘ + for (int i = 0; i < 4; i++) + { + Eigen::Matrix3f Rp; + Rp.setZero(); + Rp(0, 0) = cphi; + Rp(0, 2) = sphi[i]; + Rp(1, 1) = -1; + Rp(2, 0) = sphi[i]; + Rp(2, 2) = -cphi; + + Eigen::Matrix3f R = s * U * Rp * Vt; + vR.push_back(R); + + Eigen::Vector3f tp; + tp(0) = x1[i]; + tp(1) = 0; + tp(2) = x3[i]; + tp *= d1 + d3; + + Eigen::Vector3f t = U * tp; + vt.push_back(t / t.norm()); + + Eigen::Vector3f np; + np(0) = x1[i]; + np(1) = 0; + np(2) = x3[i]; + + Eigen::Vector3f n = V * np; + if (n(2) < 0) + n = -n; + vn.push_back(n); + } + + int bestGood = 0; + int secondBestGood = 0; + int bestSolutionIdx = -1; + float bestParallax = -1; + vector bestP3D; + vector bestTriangulated; + + // Instead of applying the visibility constraints proposed in the Faugeras' paper (which could fail for points seen with low parallax) + // We reconstruct all hypotheses and check in terms of triangulated points and parallax + // step4：d'=d2和d'=-d2分别对应8组(R t)，通过恢复3D点并判断是否在相机正前方的方法来确定最优解 + for (size_t i = 0; i < 8; i++) + { + float parallaxi; + vector vP3Di; + vector vbTriangulatedi; + int nGood = CheckRT(vR[i], vt[i], mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3Di, 4.0 * mSigma2, vbTriangulatedi, parallaxi); + + // 保留最优的和次优的 + if (nGood > bestGood) + { + secondBestGood = bestGood; + bestGood = nGood; + bestSolutionIdx = i; + bestParallax = parallaxi; + bestP3D = vP3Di; + bestTriangulated = vbTriangulatedi; + } + else if (nGood > secondBestGood) + { + secondBestGood = nGood; + } + } + + // step5：通过判断最优是否明显好于次优，从而判断该次Homography分解是否成功 + if (secondBestGood < 0.75 * bestGood && bestParallax >= minParallax && bestGood > minTriangulated && bestGood > 0.9 * N) + { + T21 = Sophus::SE3f(vR[bestSolutionIdx], vt[bestSolutionIdx]); + vbTriangulated = bestTriangulated; + + return true; + } + + return false; +} + +/** + * @brief 像素坐标标准化，计算点集的横纵均值，与均值偏差的均值。最后返回的是变化矩阵T 直接乘以像素坐标的齐次向量即可获得去中心去均值后的特征点坐标 + * @param vKeys 特征点 + * @param vNormalizedPoints 去中心去均值后的特征点坐标 + * @param T 变化矩阵 + */ +void TwoViewReconstruction::Normalize(const vector &vKeys, vector &vNormalizedPoints, Eigen::Matrix3f &T) +{ + float meanX = 0; + float meanY = 0; + const int N = vKeys.size(); + + vNormalizedPoints.resize(N); + + for (int i = 0; i < N; i++) + { + meanX += vKeys[i].pt.x; + meanY += vKeys[i].pt.y; + } + + // 1. 求均值 + meanX = meanX / N; + meanY = meanY / N; + + float meanDevX = 0; + float meanDevY = 0; + + for (int i = 0; i < N; i++) + { + vNormalizedPoints[i].x = vKeys[i].pt.x - meanX; + vNormalizedPoints[i].y = vKeys[i].pt.y - meanY; + + meanDevX += fabs(vNormalizedPoints[i].x); + meanDevY += fabs(vNormalizedPoints[i].y); + } + + // 2. 确定新原点后计算与新原点的距离均值 + meanDevX = meanDevX / N; + meanDevY = meanDevY / N; + + // 3. 去均值化 + float sX = 1.0 / meanDevX; + float sY = 1.0 / meanDevY; + + for (int i = 0; i < N; i++) + { + vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX; + vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY; + } + + // 4. 计算变化矩阵 + T.setZero(); + T(0, 0) = sX; + T(1, 1) = sY; + T(0, 2) = -meanX * sX; + T(1, 2) = -meanY * sY; + T(2, 2) = 1.f; +} + +/** + * @brief 进行cheirality check，从而进一步找出F分解后最合适的解 + * @param R 旋转 + * @param t 平移 + * @param vKeys1 特征点 + * @param vKeys2 特征点 + * @param vMatches12 匹配关系 + * @param vbMatchesInliers 匹配关系是否有效 + * @param K 内参 + * @param vP3D 三维点 + * @param th2 误差半径 + * @param vbGood 大小与mvKeys1一致，表示哪个点被重建了 + * @param parallax + */ +int TwoViewReconstruction::CheckRT( + const Eigen::Matrix3f &R, const Eigen::Vector3f &t, const vector &vKeys1, const vector &vKeys2, + const vector &vMatches12, vector &vbMatchesInliers, + const Eigen::Matrix3f &K, vector &vP3D, float th2, vector &vbGood, float ¶llax) +{ + // Calibration parameters + const float fx = K(0, 0); + const float fy = K(1, 1); + const float cx = K(0, 2); + const float cy = K(1, 2); + + vbGood = vector(vKeys1.size(), false); + vP3D.resize(vKeys1.size()); + + vector vCosParallax; + vCosParallax.reserve(vKeys1.size()); + + // Camera 1 Projection Matrix K[I|0] + // 步骤1：得到一个相机的投影矩阵 + // 以第一个相机的光心作为世界坐标系 + Eigen::Matrix P1; + P1.setZero(); + P1.block<3, 3>(0, 0) = K; + + Eigen::Vector3f O1; + O1.setZero(); + + // Camera 2 Projection Matrix K[R|t] + // 步骤2：得到第二个相机的投影矩阵 + Eigen::Matrix P2; + P2.block<3, 3>(0, 0) = R; + P2.block<3, 1>(0, 3) = t; + P2 = K * P2; + + // 第二个相机的光心在世界坐标系下的坐标 + Eigen::Vector3f O2 = -R.transpose() * t; + + int nGood = 0; + + for (size_t i = 0, iend = vMatches12.size(); i < iend; i++) + { + if (!vbMatchesInliers[i]) + continue; + + // kp1和kp2是匹配特征点 + const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first]; + const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second]; + + Eigen::Vector3f p3dC1; + Eigen::Vector3f x_p1(kp1.pt.x, kp1.pt.y, 1); + Eigen::Vector3f x_p2(kp2.pt.x, kp2.pt.y, 1); + + // 步骤3：利用三角法恢复三维点p3dC1 + GeometricTools::Triangulate(x_p1, x_p2, P1, P2, p3dC1); + + if (!isfinite(p3dC1(0)) || !isfinite(p3dC1(1)) || !isfinite(p3dC1(2))) + { + vbGood[vMatches12[i].first] = false; + continue; + } + + // Check parallax + // 步骤4：计算视差角余弦值 + Eigen::Vector3f normal1 = p3dC1 - O1; + float dist1 = normal1.norm(); + + Eigen::Vector3f normal2 = p3dC1 - O2; + float dist2 = normal2.norm(); + + float cosParallax = normal1.dot(normal2) / (dist1 * dist2); + + // 步骤5：判断3D点是否在两个摄像头前方 + // Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth) + // 步骤5.1：3D点深度为负，在第一个摄像头后方，淘汰 + if (p3dC1(2) <= 0 && cosParallax < 0.99998) + continue; + + // Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth) + // 步骤5.2：3D点深度为负，在第二个摄像头后方，淘汰 + Eigen::Vector3f p3dC2 = R * p3dC1 + t; + + if (p3dC2(2) <= 0 && cosParallax < 0.99998) + continue; + + // 步骤6：计算重投影误差 + // Check reprojection error in first image + // 计算3D点在第一个图像上的投影误差 + float im1x, im1y; + float invZ1 = 1.0 / p3dC1(2); + im1x = fx * p3dC1(0) * invZ1 + cx; + im1y = fy * p3dC1(1) * invZ1 + cy; + + float squareError1 = (im1x - kp1.pt.x) * (im1x - kp1.pt.x) + (im1y - kp1.pt.y) * (im1y - kp1.pt.y); + + // 步骤6.1：重投影误差太大，跳过淘汰 + // 一般视差角比较小时重投影误差比较大 + if (squareError1 > th2) + continue; + + // Check reprojection error in second image + // 计算3D点在第二个图像上的投影误差 + float im2x, im2y; + float invZ2 = 1.0 / p3dC2(2); + im2x = fx * p3dC2(0) * invZ2 + cx; + im2y = fy * p3dC2(1) * invZ2 + cy; + + float squareError2 = (im2x - kp2.pt.x) * (im2x - kp2.pt.x) + (im2y - kp2.pt.y) * (im2y - kp2.pt.y); + + // 步骤6.1：重投影误差太大，跳过淘汰 + // 一般视差角比较小时重投影误差比较大 + if (squareError2 > th2) + continue; + + // 步骤7：统计经过检验的3D点个数，记录3D点视差角 + vCosParallax.push_back(cosParallax); + vP3D[vMatches12[i].first] = cv::Point3f(p3dC1(0), p3dC1(1), p3dC1(2)); + nGood++; + + if (cosParallax < 0.99998) + vbGood[vMatches12[i].first] = true; + } + + // 7 得到3D点中较小的视差角，并且转换成为角度制表示 + if (nGood > 0) + { + // 从小到大排序，注意vCosParallax值越大，视差越小 + sort(vCosParallax.begin(), vCosParallax.end()); + + // !排序后并没有取最小的视差角，而是取一个较小的视差角 + // 作者的做法：如果经过检验过后的有效3D点小于50个，那么就取最后那个最小的视差角(cos值最大) + // 如果大于50个，就取排名第50个的较小的视差角即可，为了避免3D点太多时出现太小的视差角 + size_t idx = min(50, int(vCosParallax.size() - 1)); + // 将这个选中的角弧度制转换为角度制 + parallax = acos(vCosParallax[idx]) * 180 / CV_PI; + } + else + parallax = 0; + + return nGood; +} + +/** + * @brief 分解Essential矩阵 + * 解释的比较好的博客：https://blog.csdn.net/weixin_44580210/article/details/90344511 + * F矩阵通过结合内参可以得到Essential矩阵，分解E矩阵将得到4组解 + * 这4组解分别为[R1,t],[R1,-t],[R2,t],[R2,-t] + * ## 反对称矩阵性质 + * 多视图几何上定义：一个3×3的矩阵是本质矩阵的充要条件是它的奇异值中有两个相等而第三个是0，为什么呢？ + * 首先我们知道 E=[t]×​R=SR其中S为反对称矩阵，反对称矩阵有什么性质呢？ + * 结论1：如果 S 是实的反对称矩阵，那么S=UBU^T，其中 B 为形如diag(a1​Z，a2​Z...am​Z，0，0...0)的分块对角阵，其中 Z = [0, 1; -1, 0] + * 反对称矩阵的特征矢量都是纯虚数并且奇数阶的反对称矩阵必是奇异的 + * 那么根据这个结论我们可以将 S 矩阵写成 S=kUZU^⊤，而 Z 为 + * | 0, 1, 0 | + * |-1, 0, 0 | + * | 0, 0, 0 | + * Z = diag(1, 1, 0) * W W 为 + * | 0,-1, 0 | + * | 1, 0, 0 | + * | 0, 0, 1 | + * E=SR=Udiag(1,1,0)(WU^⊤R) 这样就证明了 E 拥有两个相等的奇异值 + * + * ## 恢复相机矩阵 + * 假定第一个摄像机矩阵是P=[I∣0]，为了计算第二个摄像机矩阵P′，必须把 E 矩阵分解为反对成举着和旋转矩阵的乘积 SR。 + * 还是根据上面的结论1，我们在相差一个常数因子的前提下有 S=UZU^T，我们假设旋转矩阵分解为UXV^T，注意这里是假设旋转矩阵分解形式为UXV^T，并不是旋转矩阵的svd分解， + * 其中 UV都是E矩阵分解出的 + * Udiag(1,1,0)V^T = E = SR = (UZU^T)(UXV^⊤) = U(ZX)V^T + * 则有 ZX = diag(1,1,0)，因此 x=W或者 X=W^T + * 结论：如果 E 的SVD分解为 Udiag(1,1,0)V^⊤，E = SR有两种分解形式，分别是： S = UZU^⊤ R = UWVTor UW^TV^⊤ + + * 接着分析，又因为St=0（自己和自己叉乘肯定为0嘛）以及∥t∥=1（对两个摄像机矩阵的基线的一种常用归一化），因此 t = U(0,0,1)^T = u3​， + * 即矩阵 U 的最后一列，这样的好处是不用再去求S了，应为t的符号不确定，R矩阵有两种可能，因此其分解有如下四种情况： + * P′=[UWV^T ∣ +u3​] or [UWV^T ∣ −u3​] or [UW^TV^T ∣ +u3​] or [UW^TV^T ∣ −u3​] + * @param E Essential Matrix + * @param R1 Rotation Matrix 1 + * @param R2 Rotation Matrix 2 + * @param t Translation + * @see Multiple View Geometry in Computer Vision - Result 9.19 p259 + */ +void TwoViewReconstruction::DecomposeE(const Eigen::Matrix3f &E, Eigen::Matrix3f &R1, Eigen::Matrix3f &R2, Eigen::Vector3f &t) +{ + + Eigen::JacobiSVD svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV); + + // 对 t 有归一化，但是这个地方并没有决定单目整个SLAM过程的尺度 + // 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变 + Eigen::Matrix3f U = svd.matrixU(); + Eigen::Matrix3f Vt = svd.matrixV().transpose(); + + t = U.col(2); + t = t / t.norm(); + + Eigen::Matrix3f W; + W.setZero(); + W(0, 1) = -1; + W(1, 0) = 1; + W(2, 2) = 1; + + R1 = U * W * Vt; + // 旋转矩阵有行列式为1的约束 + if (R1.determinant() < 0) + R1 = -R1; + + R2 = U * W.transpose() * Vt; + if (R2.determinant() < 0) + R2 = -R2; +} + +} // namespace ORB_SLAM diff --git a/ORB-SLAM3-UESTC/Workspace/src/Viewer.cc b/ORB-SLAM3-UESTC/Workspace/src/Viewer.cc new file mode 100644 index 0000000..9e99fc2 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/src/Viewer.cc @@ -0,0 +1,454 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + + +#include "Viewer.h" +#include + +#include + +namespace ORB_SLAM3 +{ + +Viewer::Viewer(System* pSystem, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Tracking *pTracking, const string &strSettingPath, Settings* settings): + both(false), mpSystem(pSystem), mpFrameDrawer(pFrameDrawer),mpMapDrawer(pMapDrawer), mpTracker(pTracking), + mbFinishRequested(false), mbFinished(true), mbStopped(true), mbStopRequested(false) +{ + if(settings){ + newParameterLoader(settings); + } + else{ + + cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); + + bool is_correct = ParseViewerParamFile(fSettings); + + if(!is_correct) + { + std::cerr << "**ERROR in the config file, the format is not correct**" << std::endl; + try + { + throw -1; + } + catch(exception &e) + { + + } + } + } + + mbStopTrack = false; +} + +void Viewer::newParameterLoader(Settings *settings) { + mImageViewerScale = 1.f; + + float fps = settings->fps(); + if(fps < 1) + fps=30; + + mT = 1e3/fps; + + cv::Size imSize = settings->newImSize(); + mImageHeight = imSize.height; + mImageWidth = imSize.width; + + mImageViewerScale = settings->imageViewerScale(); + mViewpointX = settings->viewPointX(); + mViewpointY = settings->viewPointY(); + mViewpointZ = settings->viewPointZ(); + mViewpointF = settings->viewPointF(); +} + +bool Viewer::ParseViewerParamFile(cv::FileStorage &fSettings) +{ + bool b_miss_params = false; + mImageViewerScale = 1.f; + + float fps = fSettings["Camera.fps"]; + if(fps < 1) + fps = 30; + + mT = 1e3/fps; + + cv::FileNode node = fSettings["Camera.width"]; + if(!node.empty()) + { + mImageWidth = node.real(); + } + else + { + std::cerr << "*Camera.width parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Camera.height"]; + if(!node.empty()) + { + mImageHeight = node.real(); + } + else + { + std::cerr << "*Camera.height parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Viewer.imageViewScale"]; + if(!node.empty()) + { + mImageViewerScale = node.real(); + } + + node = fSettings["Viewer.ViewpointX"]; + if(!node.empty()) + { + mViewpointX = node.real(); + } + else + { + std::cerr << "*Viewer.ViewpointX parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Viewer.ViewpointY"]; + if(!node.empty()) + { + mViewpointY = node.real(); + } + else + { + std::cerr << "*Viewer.ViewpointY parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Viewer.ViewpointZ"]; + if(!node.empty()) + { + mViewpointZ = node.real(); + } + else + { + std::cerr << "*Viewer.ViewpointZ parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + node = fSettings["Viewer.ViewpointF"]; + if(!node.empty()) + { + mViewpointF = node.real(); + } + else + { + std::cerr << "*Viewer.ViewpointF parameter doesn't exist or is not a real number*" << std::endl; + b_miss_params = true; + } + + return !b_miss_params; +} + +void Viewer::Run() +{ + mbFinished = false; + mbStopped = false; + + pangolin::CreateWindowAndBind("ORB-SLAM3: Map Viewer",1024,768); + + // 3D Mouse handler requires depth testing to be enabled + glEnable(GL_DEPTH_TEST); + + // Issue specific OpenGl we might need + glEnable (GL_BLEND); + glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + pangolin::CreatePanel("menu").SetBounds(0.0,1.0,0.0,pangolin::Attach::Pix(175)); + pangolin::Var menuFollowCamera("menu.Follow Camera",false,true); + pangolin::Var menuCamView("menu.Camera View",false,false); + pangolin::Var menuTopView("menu.Top View",false,false); + // pangolin::Var menuSideView("menu.Side View",false,false); + pangolin::Var menuShowPoints("menu.Show Points",true,true); + pangolin::Var menuShowKeyFrames("menu.Show KeyFrames",true,true); + pangolin::Var menuShowGraph("menu.Show Graph",false,true); + pangolin::Var menuShowInertialGraph("menu.Show Inertial Graph",true,true); + pangolin::Var menuLocalizationMode("menu.Localization Mode",false,true); + pangolin::Var menuReset("menu.Reset",false,false); + pangolin::Var menuStop("menu.Stop",false,false); + pangolin::Var menuStepByStep("menu.Step By Step",false,true); // false, true + pangolin::Var menuStep("menu.Step",false,false); + + pangolin::Var menuShowOptLba("menu.Show LBA opt", false, true); + // Define Camera Render Object (for view / scene browsing) + pangolin::OpenGlRenderState s_cam( + pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000), + pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0) + ); + + // Add named OpenGL viewport to window and provide 3D Handler + pangolin::View& d_cam = pangolin::CreateDisplay() + .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f/768.0f) + .SetHandler(new pangolin::Handler3D(s_cam)); + + pangolin::OpenGlMatrix Twc, Twr; + Twc.SetIdentity(); + pangolin::OpenGlMatrix Ow; // Oriented with g in the z axis + Ow.SetIdentity(); + cv::namedWindow("ORB-SLAM3: Current Frame"); + + bool bFollow = true; + bool bLocalizationMode = false; + bool bStepByStep = false; + bool bCameraView = true; + + if(mpTracker->mSensor == mpSystem->MONOCULAR || mpTracker->mSensor == mpSystem->STEREO || mpTracker->mSensor == mpSystem->RGBD) + { + menuShowGraph = true; + } + + float trackedImageScale = mpTracker->GetImageScale(); + + cout << "Starting the Viewer" << endl; + while(1) + { + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + mpMapDrawer->GetCurrentOpenGLCameraMatrix(Twc,Ow); + + if(mbStopTrack) + { + menuStepByStep = true; + mbStopTrack = false; + } + + if(menuFollowCamera && bFollow) + { + if(bCameraView) + s_cam.Follow(Twc); + else + s_cam.Follow(Ow); + } + else if(menuFollowCamera && !bFollow) + { + if(bCameraView) + { + s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000)); + s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0)); + s_cam.Follow(Twc); + } + else + { + s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,1000)); + s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,10, 0,0,0,0.0,0.0, 1.0)); + s_cam.Follow(Ow); + } + bFollow = true; + } + else if(!menuFollowCamera && bFollow) + { + bFollow = false; + } + + if(menuCamView) + { + menuCamView = false; + bCameraView = true; + s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,10000)); + s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0)); + s_cam.Follow(Twc); + } + + if(menuTopView && mpMapDrawer->mpAtlas->isImuInitialized()) + { + menuTopView = false; + bCameraView = false; + s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,10000)); + s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,50, 0,0,0,0.0,0.0, 1.0)); + s_cam.Follow(Ow); + } + + if(menuLocalizationMode && !bLocalizationMode) + { + mpSystem->ActivateLocalizationMode(); + bLocalizationMode = true; + } + else if(!menuLocalizationMode && bLocalizationMode) + { + mpSystem->DeactivateLocalizationMode(); + bLocalizationMode = false; + } + + if(menuStepByStep && !bStepByStep) + { + //cout << "Viewer: step by step" << endl; + mpTracker->SetStepByStep(true); + bStepByStep = true; + } + else if(!menuStepByStep && bStepByStep) + { + mpTracker->SetStepByStep(false); + bStepByStep = false; + } + + if(menuStep) + { + mpTracker->mbStep = true; + menuStep = false; + } + + + d_cam.Activate(s_cam); + glClearColor(1.0f,1.0f,1.0f,1.0f); + mpMapDrawer->DrawCurrentCamera(Twc); + if(menuShowKeyFrames || menuShowGraph || menuShowInertialGraph || menuShowOptLba) + mpMapDrawer->DrawKeyFrames(menuShowKeyFrames,menuShowGraph, menuShowInertialGraph, menuShowOptLba); + if(menuShowPoints) + mpMapDrawer->DrawMapPoints(); + + pangolin::FinishFrame(); + + cv::Mat toShow; + cv::Mat im = mpFrameDrawer->DrawFrame(trackedImageScale); + + if(both){ + cv::Mat imRight = mpFrameDrawer->DrawRightFrame(trackedImageScale); + cv::hconcat(im,imRight,toShow); + } + else{ + toShow = im; + } + + if(mImageViewerScale != 1.f) + { + int width = toShow.cols * mImageViewerScale; + int height = toShow.rows * mImageViewerScale; + cv::resize(toShow, toShow, cv::Size(width, height)); + } + + cv::imshow("ORB-SLAM3: Current Frame",toShow); + cv::waitKey(mT); + + if(menuReset) + { + menuShowGraph = true; + menuShowInertialGraph = true; + menuShowKeyFrames = true; + menuShowPoints = true; + menuLocalizationMode = false; + if(bLocalizationMode) + mpSystem->DeactivateLocalizationMode(); + bLocalizationMode = false; + bFollow = true; + menuFollowCamera = true; + mpSystem->ResetActiveMap(); + menuReset = false; + } + + if(menuStop) + { + if(bLocalizationMode) + mpSystem->DeactivateLocalizationMode(); + + // Stop all threads + mpSystem->Shutdown(); + + // Save camera trajectory + mpSystem->SaveTrajectoryEuRoC("CameraTrajectory.txt"); + mpSystem->SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt"); + menuStop = false; + } + + if(Stop()) + { + while(isStopped()) + { + usleep(3000); + } + } + + if(CheckFinish()) + break; + } + + SetFinish(); +} + +void Viewer::RequestFinish() +{ + unique_lock lock(mMutexFinish); + mbFinishRequested = true; +} + +bool Viewer::CheckFinish() +{ + unique_lock lock(mMutexFinish); + return mbFinishRequested; +} + +void Viewer::SetFinish() +{ + unique_lock lock(mMutexFinish); + mbFinished = true; +} + +bool Viewer::isFinished() +{ + unique_lock lock(mMutexFinish); + return mbFinished; +} + +void Viewer::RequestStop() +{ + unique_lock lock(mMutexStop); + if(!mbStopped) + mbStopRequested = true; +} + +bool Viewer::isStopped() +{ + unique_lock lock(mMutexStop); + return mbStopped; +} + +bool Viewer::Stop() +{ + unique_lock lock(mMutexStop); + unique_lock lock2(mMutexFinish); + + if(mbFinishRequested) + return false; + else if(mbStopRequested) + { + mbStopped = true; + mbStopRequested = false; + return true; + } + + return false; + +} + +void Viewer::Release() +{ + unique_lock lock(mMutexStop); + mbStopped = false; +} + +/*void Viewer::SetTrackingPause() +{ + mbStopTrack = true; +}*/ + +} diff --git a/build.sh b/build.sh deleted file mode 100755 index 8b00265..0000000 --- a/build.sh +++ /dev/null @@ -1,41 +0,0 @@ -echo "Configuring and building Thirdparty/DBoW2 ..." - -cd Thirdparty/DBoW2 -mkdir build -cd build -cmake .. -DCMAKE_BUILD_TYPE=Release -make -j - -cd ../../g2o - -echo "Configuring and building Thirdparty/g2o ..." - -mkdir build -cd build -cmake .. -DCMAKE_BUILD_TYPE=Release -make -j - -cd ../../Sophus - -echo "Configuring and building Thirdparty/Sophus ..." - -mkdir build -cd build -cmake .. -DCMAKE_BUILD_TYPE=Release -make -j - -cd ../../../ - -echo "Uncompress vocabulary ..." - -cd Vocabulary -tar -xf ORBvoc.txt.tar.gz -cd .. - -echo "Configuring and building ORB_SLAM3 ..." - -mkdir build -cd build -cmake .. -DCMAKE_BUILD_TYPE=Release -make -j4 -