logzhan
/
ORB-SLAM3-UESTC
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Update Initial Code

pull/1/head
詹力 2023-11-28 03:20:25 -08:00
parent 73949dadf3
commit 230c7860da
265 changed files with 81009 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
.gitignore vendored Normal file
View File

@ -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

496
Docs/Calibration_Tutorial.pdf Normal file
View File

@ -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 <Key, value> 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

49
Docs/Changelog.md Normal file
View File

@ -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.

3
Workspace/.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,3 @@
{
"ros.distro": "noetic"
}

69
Workspace/Application/cam2.yaml Executable file
View File

@ -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

203
Workspace/Application/mono_euroc_cap.cc Executable file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<opencv2/core/core.hpp>
#include<System.h>
using namespace std;
void LoadImages(const string &strImagePath, const string &strPathTimes,
vector<string> &vstrImages, vector<double> &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<string> > vstrImageFilenames;
vector< vector<double> > vTimestampsCam;
vector<int> nImages;
vstrImageFilenames.resize(num_seq);
vTimestampsCam.resize(num_seq);
nImages.resize(num_seq);
// Vector for tracking time statistics
vector<float> 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<num_seq; seq++)
{
// Main loop
cv::VideoCapture cap = cv::VideoCapture(0);
cap.set(cv::CAP_PROP_FRAME_WIDTH, 640);
cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
if (!cap.isOpened()) {
std::cerr << "Error: Could not open camera." << std::endl;
return -1;
}
int fps = cap.get(cv::CAP_PROP_FPS);
int delay = 1000/fps;
while(1)
{
cv::Mat im;
cap >> 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<std::chrono::nanoseconds>(currentTime.time_since_epoch());
double tframe = static_cast<double>(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<std::chrono::duration<double,std::milli> >(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<std::chrono::duration<double,std::milli> >(t2 - t1).count();
SLAM.InsertTrackTime(t_track);
#endif
double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
// std::cout<<"ttrack:"<<ttrack<<std::endl;
// vTimesTrack[ni]=ttrack;
// Wait to load the next frame
double T = 0;
// if(ni<nImages[seq]-1)
// T = vTimestampsCam[seq][ni+1]-tframe;
// else if(ni>0)
// T = tframe-vTimestampsCam[seq][ni-1];
// std::cout << "T: " << T << std::endl;
// std::cout << "ttrack: " << ttrack << std::endl;
// if(ttrack<T) {
// //std::cout << "usleep: " << (dT-ttrack) << std::endl;
// usleep((T-ttrack)*1e6); // 1e6
// }
}
if(seq < num_seq - 1)
{
string kf_file_submap = "./SubMaps/kf_SubMap_" + std::to_string(seq) + ".txt";
string f_file_submap = "./SubMaps/f_SubMap_" + std::to_string(seq) + ".txt";
SLAM.SaveTrajectoryEuRoC(f_file_submap);
SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file_submap);
cout << "Changing the dataset" << endl;
SLAM.ChangeDataset();
}
}
// Stop all threads
SLAM.Shutdown();
// Save camera trajectory
if (1)
{
const string kf_file = "kf_" + string(argv[argc-1]) + ".txt";
const string f_file = "f_" + string(argv[argc-1]) + ".txt";
SLAM.SaveTrajectoryEuRoC(f_file);
SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);
}
else
{
SLAM.SaveTrajectoryEuRoC("CameraTrajectory.txt");
SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
}
return 0;
}

399
Workspace/CMakeLists.txt Executable file
View File

@ -0,0 +1,399 @@
cmake_minimum_required(VERSION 2.8)
project(ORB_SLAM3)
IF(NOT CMAKE_BUILD_TYPE)
SET(CMAKE_BUILD_TYPE Release)
ENDIF()
MESSAGE("Build type: " ${CMAKE_BUILD_TYPE})
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3")
set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native")
set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native")
# Check C++14 or C++0x support
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
add_definitions(-DCOMPILEDWITHC11)
message(STATUS "Using flag -std=c++14.")
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
add_definitions(-DCOMPILEDWITHC0X)
message(STATUS "Using flag -std=c++0x.")
else()
message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
endif()
LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
# 要保证整个工程的opencv版本一致，包括dbow，源码以及ros相关的
# 3 4 都可以正常运行
# find_package(OpenCV 3.2)
find_package(OpenCV 3)
if(NOT OpenCV_FOUND)
message(FATAL_ERROR "OpenCV > 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
# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular)
# 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()

46
Workspace/Thirdparty/DBoW2/CMakeLists.txt vendored Normal file
View File

@ -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})

130
Workspace/Thirdparty/DBoW2/DBoW2/BowVector.cpp vendored Normal file
View File

@ -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 <iostream>
#include <fstream>
#include <vector>
#include <algorithm>
#include <cmath>
#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<unsigned int>::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

119
Workspace/Thirdparty/DBoW2/DBoW2/BowVector.h vendored Normal file
View File

@ -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 <iostream>
#include <map>
#include <vector>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/map.hpp>
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<WordId, WordValue>
{
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive& ar, const int version)
{
ar & boost::serialization::base_object<std::map<WordId, WordValue> >(*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

71
Workspace/Thirdparty/DBoW2/DBoW2/FClass.h vendored Normal file
View File

@ -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 <opencv2/core/core.hpp>
#include <vector>
#include <string>
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<pDescriptor> &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<TDescriptor> &descriptors,
cv::Mat &mat);
};
} // namespace DBoW2
#endif

193
Workspace/Thirdparty/DBoW2/DBoW2/FORB.cpp vendored Normal file
View File

@ -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 <vector>
#include <string>
#include <sstream>
#include <stdint-gcc.h>
#include "FORB.h"
using namespace std;
namespace DBoW2 {
// --------------------------------------------------------------------------
const int FORB::L=32;
void FORB::meanValue(const std::vector<FORB::pDescriptor> &descriptors,
FORB::TDescriptor &mean)
{
if(descriptors.empty())
{
mean.release();
return;
}
else if(descriptors.size() == 1)
{
mean = descriptors[0]->clone();
}
else
{
vector<int> 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<unsigned char>();
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<unsigned char>();
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<int32_t>();
const int *pb = b.ptr<int32_t>();
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<unsigned char>();
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<unsigned char>();
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<TDescriptor> &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<float>();
for(size_t i = 0; i < N; ++i)
{
const int C = descriptors[i].cols;
const unsigned char *desc = descriptors[i].ptr<unsigned char>();
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<TDescriptor> &descriptors,
cv::Mat &mat)
{
mat.create(descriptors.size(), 32, CV_8U);
unsigned char *p = mat.ptr<unsigned char>();
for(size_t i = 0; i < descriptors.size(); ++i, p += 32)
{
const unsigned char *d = descriptors[i].ptr<unsigned char>();
std::copy(d, d+32, p);
}
}
// --------------------------------------------------------------------------
} // namespace DBoW2

79
Workspace/Thirdparty/DBoW2/DBoW2/FORB.h vendored Normal file
View File

@ -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 <opencv2/core/core.hpp>
#include <vector>
#include <string>
#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<pDescriptor> &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<TDescriptor> &descriptors,
cv::Mat &mat);
static void toMat8U(const std::vector<TDescriptor> &descriptors,
cv::Mat &mat);
};
} // namespace DBoW2
#endif

85
Workspace/Thirdparty/DBoW2/DBoW2/FeatureVector.cpp vendored Normal file
View File

@ -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 <map>
#include <vector>
#include <iostream>
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<unsigned int>() ));
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<unsigned int>* 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

66
Workspace/Thirdparty/DBoW2/DBoW2/FeatureVector.h vendored Normal file
View File

@ -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 <map>
#include <vector>
#include <iostream>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/map.hpp>
namespace DBoW2 {
/// Vector of nodes with indexes of local features
class FeatureVector:
public std::map<NodeId, std::vector<unsigned int> >
{
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive& ar, const int version)
{
ar & boost::serialization::base_object<std::map<NodeId, std::vector<unsigned int> > >(*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

315
Workspace/Thirdparty/DBoW2/DBoW2/ScoringObject.cpp vendored Normal file
View File

@ -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 <cfloat>
#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
}
// ---------------------------------------------------------------------------
// ---------------------------------------------------------------------------

96
Workspace/Thirdparty/DBoW2/DBoW2/ScoringObject.h vendored Normal file
View File

@ -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

1665
Workspace/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

129
Workspace/Thirdparty/DBoW2/DUtils/Random.cpp vendored Normal file
View File

@ -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 <cstdlib>
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;
}
// ---------------------------------------------------------------------------

184
Workspace/Thirdparty/DBoW2/DUtils/Random.h vendored Normal file
View File

@ -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 <cstdlib>
#include <vector>
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 <class T>
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 <class T>
static T RandomValue(T min, T max){
return Random::RandomValue<T>() * (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 <class T>
static T RandomGaussianValue(T mean, T sigma)
{
// Box-Muller transformation
T x1, x2, w, y1;
do {
x1 = (T)2. * RandomValue<T>() - (T)1.;
x2 = (T)2. * RandomValue<T>() - (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<int> m_values;
};
}
#endif

246
Workspace/Thirdparty/DBoW2/DUtils/Timestamp.cpp vendored Normal file
View File

@ -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 <cstdio>
#include <cstdlib>
#include <ctime>
#include <cmath>
#include <sstream>
#include <iomanip>
#ifdef _WIN32
#ifndef WIN32
#define WIN32
#endif
#endif
#ifdef WIN32
#include <sys/timeb.h>
#define sprintf sprintf_s
#else
#include <sys/time.h>
#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();
}

204
Workspace/Thirdparty/DBoW2/DUtils/Timestamp.h vendored Normal file
View File

@ -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 <iostream>
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

7
Workspace/Thirdparty/DBoW2/README.txt vendored Normal file
View File

@ -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

87
Workspace/Thirdparty/Sophus/.clang-format vendored Normal file
View File

@ -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

4
Workspace/Thirdparty/Sophus/.gitignore vendored Normal file
View File

@ -0,0 +1,4 @@
build
CMakeLists.txt.user
*.pyc
.vscode/settings.json

83
Workspace/Thirdparty/Sophus/.travis.yml vendored Normal file
View File

@ -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

158
Workspace/Thirdparty/Sophus/CMakeLists.txt vendored Normal file
View File

@ -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
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>"
"$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>"
)
# 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
)

20
Workspace/Thirdparty/Sophus/LICENSE.txt vendored Normal file
View File

@ -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.

37
Workspace/Thirdparty/Sophus/README.rst vendored Normal file
View File

@ -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

7
Workspace/Thirdparty/Sophus/SophusConfig.cmake.in vendored Normal file
View File

@ -0,0 +1,7 @@
@PACKAGE_INIT@
include (CMakeFindDependencyMacro)
@Eigen3_DEPENDENCY@
include ("${CMAKE_CURRENT_LIST_DIR}/SophusTargets.cmake")

26
Workspace/Thirdparty/Sophus/appveyor.yml vendored Normal file
View File

@ -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

20
Workspace/Thirdparty/Sophus/doxyrest-config.lua vendored Normal file
View File

@ -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

8
Workspace/Thirdparty/Sophus/examples/CMakeLists.txt vendored Normal file
View File

@ -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)

38
Workspace/Thirdparty/Sophus/examples/HelloSO3.cpp vendored Normal file
View File

@ -0,0 +1,38 @@
#include <iostream>
#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<double>::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;
}

3
Workspace/Thirdparty/Sophus/make_docs.sh vendored Executable file
View File

@ -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

22
Workspace/Thirdparty/Sophus/package.xml vendored Normal file
View File

@ -0,0 +1,22 @@
<?xml version="1.0"?>
<package format="2">
<name>sophus</name>
<version>1.1.0</version>
<description>
C++ implementation of Lie Groups using Eigen.
</description>
<url type="repository">https://github.com/strasdat/sophus</url>
<url type="bugtracker">https://github.com/strasdat/sophus/issues</url>
<maintainer email="d.stonier@gmail.com">Daniel Stonier</maintainer>
<author>Hauke Strasdat</author>
<license>MIT</license>
<buildtool_depend>cmake</buildtool_depend>
<build_depend>eigen</build_depend>
<build_export_depend>eigen</build_export_depend>
<export>
<build_type>cmake</build_type>
</export>
</package>

22
Workspace/Thirdparty/Sophus/py/cpp_gencode/Se2_Dx_exp_x.cpp vendored Normal file
View File

@ -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];

13
Workspace/Thirdparty/Sophus/py/cpp_gencode/Se2_Dx_this_mul_exp_x_at_0.cpp vendored Normal file
View File

@ -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;

138
Workspace/Thirdparty/Sophus/py/cpp_gencode/Se3_Dx_exp_x.cpp vendored Normal file
View File

@ -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);

64
Workspace/Thirdparty/Sophus/py/cpp_gencode/Se3_Dx_this_mul_exp_x_at_0.cpp vendored Normal file
View File

@ -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;

2
Workspace/Thirdparty/Sophus/py/cpp_gencode/So2_Dx_exp_x.cpp vendored Normal file
View File

@ -0,0 +1,2 @@
result[0] = -sin(theta);
result[1] = cos(theta);

2
Workspace/Thirdparty/Sophus/py/cpp_gencode/So2_Dx_this_mul_exp_x_at_0.cpp vendored Normal file
View File

@ -0,0 +1,2 @@
result[0] = -c[1];
result[1] = c[0];

29
Workspace/Thirdparty/Sophus/py/cpp_gencode/So3_Dx_exp_x.cpp vendored Normal file
View File

@ -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];

19
Workspace/Thirdparty/Sophus/py/cpp_gencode/So3_Dx_this_mul_exp_x_at_0.cpp vendored Normal file
View File

@ -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;

13
Workspace/Thirdparty/Sophus/py/run_tests.sh vendored Executable file
View File

@ -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

7
Workspace/Thirdparty/Sophus/py/sophus/__init__.py vendored Normal file
View File

@ -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

112
Workspace/Thirdparty/Sophus/py/sophus/complex.py vendored Normal file
View File

@ -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')

17
Workspace/Thirdparty/Sophus/py/sophus/cse_codegen.py vendored Normal file
View File

@ -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

92
Workspace/Thirdparty/Sophus/py/sophus/dual_quaternion.py vendored Normal file
View File

@ -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()

60
Workspace/Thirdparty/Sophus/py/sophus/matrix.py vendored Normal file
View File

@ -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))

135
Workspace/Thirdparty/Sophus/py/sophus/quaternion.py vendored Normal file
View File

@ -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')

224
Workspace/Thirdparty/Sophus/py/sophus/se2.py vendored Normal file
View File

@ -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()

260
Workspace/Thirdparty/Sophus/py/sophus/se3.py vendored Normal file
View File

@ -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()

186
Workspace/Thirdparty/Sophus/py/sophus/so2.py vendored Normal file
View File

@ -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()

253
Workspace/Thirdparty/Sophus/py/sophus/so3.py vendored Normal file
View File

@ -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()

3
Workspace/Thirdparty/Sophus/py/sophus/so3_codegen.py vendored Normal file
View File

@ -0,0 +1,3 @@
import sophus
from sympy.utilities.codegen import codegen
import sympy

54
Workspace/Thirdparty/Sophus/rst-dir/conf.py vendored Normal file
View File

@ -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']

9
Workspace/Thirdparty/Sophus/rst-dir/page_index.rst vendored Normal file
View File

@ -0,0 +1,9 @@
Sophus - Lie groups for 2d/3d Geometry
=======================================
.. toctree::
:maxdepth: 2
:caption: Contents:
GitHub Page <https://github.com/strasdat/Sophus>
pysophus

23
Workspace/Thirdparty/Sophus/rst-dir/pysophus.rst vendored Normal file
View File

@ -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:

1
Workspace/Thirdparty/Sophus/run_format.sh vendored Executable file
View File

@ -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

21
Workspace/Thirdparty/Sophus/scripts/install_docs_deps.sh vendored Executable file
View File

@ -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 ../..

25
Workspace/Thirdparty/Sophus/scripts/install_linux_deps.sh vendored Executable file
View File

@ -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

21
Workspace/Thirdparty/Sophus/scripts/install_osx_deps.sh vendored Executable file
View File

@ -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

11
Workspace/Thirdparty/Sophus/scripts/run_cpp_tests.sh vendored Executable file
View File

@ -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

231
Workspace/Thirdparty/Sophus/sophus/average.hpp vendored Normal file
View File

@ -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 <class SequenceContainer>
optional<typename SequenceContainer::value_type> 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<Tangent>(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<Tangent>(
(foo_T_newaverage.inverse() * foo_T_average).log()) <
Constants<Scalar>::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 <class SequenceContainer, class Scalar>
optional<typename SequenceContainer::value_type> average(
SequenceContainer const& foo_Ts_bar);
#else
// Mean implementation for SO(2).
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, SO2<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
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<Scalar> foo_T_average = foo_Ts_bar.front();
Scalar w = Scalar(1. / N);
Scalar average(0);
for (SO2<Scalar> const& foo_T_bar : foo_Ts_bar) {
average += w * (foo_T_average.inverse() * foo_T_bar).log();
}
return foo_T_average * SO2<Scalar>::exp(average);
}
// Mean implementation for RxSO(2).
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, RxSO2<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
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<Scalar> foo_T_average = foo_Ts_bar.front();
Scalar w = Scalar(1. / N);
Vector2<Scalar> average(Scalar(0), Scalar(0));
for (RxSO2<Scalar> const& foo_T_bar : foo_Ts_bar) {
average += w * (foo_T_average.inverse() * foo_T_bar).log();
}
return foo_T_average * RxSO2<Scalar>::exp(average);
}
namespace details {
template <class T>
void getQuaternion(T const&);
template <class Scalar>
Eigen::Quaternion<Scalar> getUnitQuaternion(SO3<Scalar> const& R) {
return R.unit_quaternion();
}
template <class Scalar>
Eigen::Quaternion<Scalar> getUnitQuaternion(RxSO3<Scalar> const& sR) {
return sR.so3().unit_quaternion();
}
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
Eigen::Quaternion<Scalar> 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<Scalar, 4, Eigen::Dynamic> 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<Scalar, 4, 4> QQt = Q * Q.transpose();
// TODO: Figure out why we can't use SelfAdjointEigenSolver here.
Eigen::EigenSolver<Eigen::Matrix<Scalar, 4, 4> > es(QQt);
std::complex<Scalar> max_eigenvalue = es.eigenvalues()[0];
Eigen::Matrix<std::complex<Scalar>, 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<Scalar> 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 <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, SO3<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar) {
return SO3<Scalar>(details::averageUnitQuaternion(foo_Ts_bar));
}
// Mean implementation for R x SO(3).
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, RxSO3<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
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<Scalar> const& foo_T_bar : foo_Ts_bar) {
scale_sum += log(foo_T_bar.scale());
}
return RxSO3<Scalar>(exp(scale_sum / Scalar(N)),
SO3<Scalar>(details::averageUnitQuaternion(foo_Ts_bar)));
}
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, SE2<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
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 <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, Sim2<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) {
return iterativeMean(foo_Ts_bar, max_num_iterations);
}
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, SE3<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) {
return iterativeMean(foo_Ts_bar, max_num_iterations);
}
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, Sim3<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
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

178
Workspace/Thirdparty/Sophus/sophus/common.hpp vendored Normal file
View File

@ -0,0 +1,178 @@
/// @file
/// Common functionality.
#ifndef SOPHUS_COMMON_HPP
#define SOPHUS_COMMON_HPP
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <random>
#include <type_traits>
#include <Eigen/Core>
#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 <class... Args>
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>(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 <class Scalar>
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<float> {
SOPHUS_FUNC static float constexpr epsilon() {
return static_cast<float>(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 T>
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 <bool B, class T = void>
using enable_if_t = typename std::enable_if<B, T>::type;
template <class G>
struct IsUniformRandomBitGenerator {
static const bool value = std::is_unsigned<typename G::result_type>::value &&
std::is_unsigned<decltype(G::min())>::value &&
std::is_unsigned<decltype(G::max())>::value;
};
} // namespace Sophus
#endif // SOPHUS_COMMON_HPP

14
Workspace/Thirdparty/Sophus/sophus/example_ensure_handler.cpp vendored Normal file
View File

@ -0,0 +1,14 @@
#include "common.hpp"
#include <cstdio>
#include <cstdlib>
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

72
Workspace/Thirdparty/Sophus/sophus/formatstring.hpp vendored Normal file
View File

@ -0,0 +1,72 @@
/// @file
/// FormatString functionality
#ifndef SOPHUS_FORMATSTRING_HPP
#define SOPHUS_FORMATSTRING_HPP
#include <iostream>
namespace Sophus {
namespace details {
// Following: http://stackoverflow.com/a/22759544
template <class T>
class IsStreamable {
private:
template <class TT>
static auto test(int)
-> decltype(std::declval<std::stringstream&>() << std::declval<TT>(),
std::true_type());
template <class>
static auto test(...) -> std::false_type;
public:
static bool const value = decltype(test<T>(0))::value;
};
template <class T>
class ArgToStream {
public:
static void impl(std::stringstream& stream, T&& arg) {
stream << std::forward<T>(arg);
}
};
inline void FormatStream(std::stringstream& stream, char const* text) {
stream << text;
return;
}
// Following: http://en.cppreference.com/w/cpp/language/parameter_pack
template <class T, typename... Args>
void FormatStream(std::stringstream& stream, char const* text, T&& arg,
Args&&... args) {
static_assert(IsStreamable<T>::value,
"One of the args has no ostream overload!");
for (; *text != '\0'; ++text) {
if (*text == '%') {
ArgToStream<T&&>::impl(stream, std::forward<T>(arg));
FormatStream(stream, text + 1, std::forward<Args>(args)...);
return;
}
stream << *text;
}
stream << "\nFormat-Warning: There are " << sizeof...(Args) + 1
<< " args unused.";
return;
}
template <class... Args>
std::string FormatString(char const* text, Args&&... args) {
std::stringstream stream;
FormatStream(stream, text, std::forward<Args>(args)...);
return stream.str();
}
inline std::string FormatString() { return std::string(); }
} // namespace details
} // namespace Sophus
#endif //SOPHUS_FORMATSTRING_HPP

179
Workspace/Thirdparty/Sophus/sophus/geometry.hpp vendored Normal file
View File

@ -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 <class T>
Vector2<T> normalFromSO2(SO2<T> 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 <class T>
SO2<T> SO2FromNormal(Vector2<T> normal_foo) {
SOPHUS_ENSURE(normal_foo.squaredNorm() > Constants<T>::epsilon(), "%",
normal_foo.transpose());
normal_foo.normalize();
return SO2<T>(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 <class T>
Vector3<T> normalFromSO3(SO3<T> 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 <class T>
Matrix3<T> rotationFromNormal(Vector3<T> const& normal_foo,
Vector3<T> xDirHint_foo = Vector3<T>(T(1), T(0),
T(0)),
Vector3<T> yDirHint_foo = Vector3<T>(T(0), T(1),
T(0))) {
SOPHUS_ENSURE(xDirHint_foo.dot(yDirHint_foo) < Constants<T>::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<T>::epsilon(), "%",
xDirHint_foo.transpose());
SOPHUS_ENSURE(yDirHint_foo_sqr_length > Constants<T>::epsilon(), "%",
yDirHint_foo.transpose());
SOPHUS_ENSURE(normal_foo_sqr_length > Constants<T>::epsilon(), "%",
normal_foo.transpose());
Matrix3<T> basis_foo;
basis_foo.col(2) = normal_foo;
if (abs(xDirHint_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {
xDirHint_foo.normalize();
}
if (abs(yDirHint_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {
yDirHint_foo.normalize();
}
if (abs(normal_foo_sqr_length - T(1)) > Constants<T>::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<T>::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 <class T>
SO3<T> SO3FromNormal(Vector3<T> const& normal_foo) {
return SO3<T>(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 <class T>
Line2<T> lineFromSE2(SE2<T> const& T_foo_line) {
return Line2<T>(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 <class T>
SE2<T> SE2FromLine(Line2<T> const& line_foo) {
T const d = line_foo.offset();
Vector2<T> const n = line_foo.normal();
SO2<T> const R_foo_plane = SO2FromNormal(n);
return SE2<T>(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 <class T>
Plane3<T> planeFromSE3(SE3<T> const& T_foo_plane) {
return Plane3<T>(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 <class T>
SE3<T> SE3FromPlane(Plane3<T> const& plane_foo) {
T const d = plane_foo.offset();
Vector3<T> const n = plane_foo.normal();
SO3<T> const R_foo_plane = SO3FromNormal(n);
return SE3<T>(R_foo_plane, -d * n);
}
/// Takes in a hyperplane and returns unique representation by ensuring that the
/// ``offset`` is not negative.
///
template <class T, int N>
Eigen::Hyperplane<T, N> makeHyperplaneUnique(
Eigen::Hyperplane<T, N> const& plane) {
if (plane.offset() >= 0) {
return plane;
}
return Eigen::Hyperplane<T, N>(-plane.normal(), -plane.offset());
}
} // namespace Sophus
#endif // GEOMETRY_HPP

38
Workspace/Thirdparty/Sophus/sophus/interpolate.hpp vendored Normal file
View File

@ -0,0 +1,38 @@
/// @file
/// Interpolation for Lie groups.
#ifndef SOPHUS_INTERPOLATE_HPP
#define SOPHUS_INTERPOLATE_HPP
#include <Eigen/Eigenvalues>
#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 <class G, class Scalar2 = typename G::Scalar>
enable_if_t<interp_details::Traits<G>::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

104
Workspace/Thirdparty/Sophus/sophus/interpolate_details.hpp vendored Normal file
View File

@ -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 <class Group>
struct Traits;
template <class Scalar>
struct Traits<SO2<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(SO2<Scalar> const& foo_T_bar) {
using std::abs;
Scalar angle = foo_T_bar.log();
return abs(abs(angle) - Constants<Scalar>::pi()) <
Constants<Scalar>::epsilon();
}
};
template <class Scalar>
struct Traits<RxSO2<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(RxSO2<Scalar> const& foo_T_bar) {
return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so2());
}
};
template <class Scalar>
struct Traits<SO3<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(SO3<Scalar> const& foo_T_bar) {
using std::abs;
Scalar angle = foo_T_bar.logAndTheta().theta;
return abs(abs(angle) - Constants<Scalar>::pi()) <
Constants<Scalar>::epsilon();
}
};
template <class Scalar>
struct Traits<RxSO3<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(RxSO3<Scalar> const& foo_T_bar) {
return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so3());
}
};
template <class Scalar>
struct Traits<SE2<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(SE2<Scalar> const& foo_T_bar) {
return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so2());
}
};
template <class Scalar>
struct Traits<SE3<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(SE3<Scalar> const& foo_T_bar) {
return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so3());
}
};
template <class Scalar>
struct Traits<Sim2<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(Sim2<Scalar> const& foo_T_bar) {
return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(
foo_T_bar.rxso2().so2());
;
}
};
template <class Scalar>
struct Traits<Sim3<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(Sim3<Scalar> const& foo_T_bar) {
return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(
foo_T_bar.rxso3().so3());
;
}
};
} // namespace interp_details
} // namespace Sophus
#endif // SOPHUS_INTERPOLATE_DETAILS_HPP

93
Workspace/Thirdparty/Sophus/sophus/num_diff.hpp vendored Normal file
View File

@ -0,0 +1,93 @@
/// @file
/// Numerical differentiation using finite differences
#ifndef SOPHUS_NUM_DIFF_HPP
#define SOPHUS_NUM_DIFF_HPP
#include <functional>
#include <type_traits>
#include <utility>
#include "types.hpp"
namespace Sophus {
namespace details {
template <class Scalar>
class Curve {
public:
template <class Fn>
static auto num_diff(Fn curve, Scalar t, Scalar h) -> decltype(curve(t)) {
using ReturnType = decltype(curve(t));
static_assert(std::is_floating_point<Scalar>::value,
"Scalar must be a floating point type.");
static_assert(IsFloatingPoint<ReturnType>::value,
"ReturnType must be either a floating point scalar, "
"vector or matrix.");
return (curve(t + h) - curve(t - h)) / (Scalar(2) * h);
}
};
template <class Scalar, int N, int M>
class VectorField {
public:
static Eigen::Matrix<Scalar, N, M> num_diff(
std::function<Sophus::Vector<Scalar, N>(Sophus::Vector<Scalar, M>)>
vector_field,
Sophus::Vector<Scalar, M> const& a, Scalar eps) {
static_assert(std::is_floating_point<Scalar>::value,
"Scalar must be a floating point type.");
Eigen::Matrix<Scalar, N, M> J;
Sophus::Vector<Scalar, M> 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 Scalar, int N>
class VectorField<Scalar, N, 1> {
public:
static Eigen::Matrix<Scalar, N, 1> num_diff(
std::function<Sophus::Vector<Scalar, N>(Scalar)> vector_field,
Scalar const& a, Scalar eps) {
return details::Curve<Scalar>::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 <class Scalar, class Fn>
auto curveNumDiff(Fn curve, Scalar t,
Scalar h = Constants<Scalar>::epsilonSqrt())
-> decltype(details::Curve<Scalar>::num_diff(std::move(curve), t, h)) {
return details::Curve<Scalar>::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 <class Scalar, int N, int M, class ScalarOrVector, class Fn>
Eigen::Matrix<Scalar, N, M> vectorFieldNumDiff(
Fn vector_field, ScalarOrVector const& a,
Scalar eps = Constants<Scalar>::epsilonSqrt()) {
return details::VectorField<Scalar, N, M>::num_diff(std::move(vector_field),
a, eps);
}
} // namespace Sophus
#endif // SOPHUS_NUM_DIFF_HPP

84
Workspace/Thirdparty/Sophus/sophus/rotation_matrix.hpp vendored Normal file
View File

@ -0,0 +1,84 @@
/// @file
/// Rotation matrix helper functions.
#ifndef SOPHUS_ROTATION_MATRIX_HPP
#define SOPHUS_ROTATION_MATRIX_HPP
#include <Eigen/Dense>
#include <Eigen/SVD>
#include "types.hpp"
namespace Sophus {
/// Takes in arbitrary square matrix and returns true if it is
/// orthogonal.
template <class D>
SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase<D> 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<Scalar, N, N>::Identity()).norm() <
Constants<Scalar>::epsilon();
}
/// Takes in arbitrary square matrix and returns true if it is
/// "scaled-orthogonal" with positive determinant.
///
template <class D>
SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase<D> 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<Scalar, N, N>::Identity())
.template lpNorm<Eigen::Infinity>() <
sqrt(Constants<Scalar>::epsilon());
}
/// Takes in arbitrary square matrix (2x2 or larger) and returns closest
/// orthogonal matrix with positive determinant.
template <class D>
SOPHUS_FUNC enable_if_t<
std::is_floating_point<typename D::Scalar>::value,
Matrix<typename D::Scalar, D::RowsAtCompileTime, D::RowsAtCompileTime>>
makeRotationMatrix(Eigen::MatrixBase<D> 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<Matrix<Scalar, N, N>> 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<Scalar, N, N> Diag = Matrix<Scalar, N, N>::Identity();
Diag(N - 1, N - 1) = d;
return svd.matrixU() * Diag * svd.matrixV().transpose();
}
} // namespace Sophus
#endif // SOPHUS_ROTATION_MATRIX_HPP

656
Workspace/Thirdparty/Sophus/sophus/rxso2.hpp vendored Normal file
View File

@ -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 Scalar_, int Options = 0>
class RxSO2;
using RxSO2d = RxSO2<double>;
using RxSO2f = RxSO2<float>;
} // namespace Sophus
namespace Eigen {
namespace internal {
template <class Scalar_, int Options_>
struct traits<Sophus::RxSO2<Scalar_, Options_>> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using ComplexType = Sophus::Vector2<Scalar, Options>;
};
template <class Scalar_, int Options_>
struct traits<Map<Sophus::RxSO2<Scalar_>, Options_>>
: traits<Sophus::RxSO2<Scalar_, Options_>> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using ComplexType = Map<Sophus::Vector2<Scalar>, Options>;
};
template <class Scalar_, int Options_>
struct traits<Map<Sophus::RxSO2<Scalar_> const, Options_>>
: traits<Sophus::RxSO2<Scalar_, Options_> const> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using ComplexType = Map<Sophus::Vector2<Scalar> 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 Derived>
class RxSO2Base {
public:
static constexpr int Options = Eigen::internal::traits<Derived>::Options;
using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
using ComplexType = typename Eigen::internal::traits<Derived>::ComplexType;
using ComplexTemporaryType = Sophus::Vector2<Scalar, Options>;
/// 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<Scalar, N, N>;
using Point = Vector2<Scalar>;
using HomogeneousPoint = Vector3<Scalar>;
using Line = ParametrizedLine2<Scalar>;
using Tangent = Vector<Scalar, DoF>;
using Adjoint = Matrix<Scalar, DoF, DoF>;
/// 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 <typename OtherDerived>
using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
Scalar, typename OtherDerived::Scalar>::ReturnType;
template <typename OtherDerived>
using RxSO2Product = RxSO2<ReturnScalar<OtherDerived>>;
template <typename PointDerived>
using PointProduct = Vector2<ReturnScalar<PointDerived>>;
template <typename HPointDerived>
using HomogeneousPointProduct = Vector3<ReturnScalar<HPointDerived>>;
/// 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<Scalar>(complex()).log(); }
/// Returns copy of instance casted to NewScalarType.
///
template <class NewScalarType>
SOPHUS_FUNC RxSO2<NewScalarType> cast() const {
return RxSO2<NewScalarType>(complex().template cast<NewScalarType>());
}
/// 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<Scalar> inverse() const {
Scalar squared_scale = complex().squaredNorm();
return RxSO2<Scalar>(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<Scalar>(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 <class OtherDerived>
SOPHUS_FUNC RxSO2Base<Derived>& operator=(
RxSO2Base<OtherDerived> 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 <typename OtherDerived>
SOPHUS_FUNC RxSO2Product<OtherDerived> operator*(
RxSO2Base<OtherDerived> const& other) const {
using ResultT = ReturnScalar<OtherDerived>;
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<OtherDerived>::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<ResultT>::epsilon() * Constants<ResultT>::epsilon()) {
/// Saturation to ensure class invariant.
result_complex.normalize();
result_complex *= Constants<ResultT>::epsilon();
}
return RxSO2Product<OtherDerived>(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 <typename PointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<PointDerived, 2>::value>::type>
SOPHUS_FUNC PointProduct<PointDerived> operator*(
Eigen::MatrixBase<PointDerived> const& p) const {
return matrix() * p;
}
/// Group action on homogeneous 2-points. See above for more details.
///
template <typename HPointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<HPointDerived, 3>::value>::type>
SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
Eigen::MatrixBase<HPointDerived> const& p) const {
const auto rsp = *this * p.template head<2>();
return HomogeneousPointProduct<HPointDerived>(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 <typename OtherDerived,
typename = typename std::enable_if<
std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
SOPHUS_FUNC RxSO2Base<Derived>& operator*=(
RxSO2Base<OtherDerived> const& other) {
*static_cast<Derived*>(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<Scalar, num_parameters> params() const {
return complex();
}
/// Sets non-zero complex
///
/// Precondition: ``z`` must not be close to zero.
SOPHUS_FUNC void setComplex(Vector2<Scalar> const& z) {
SOPHUS_ENSURE(z.squaredNorm() > Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon(),
"Scale factor must be greater-equal epsilon.");
static_cast<Derived*>(this)->complex_nonconst() = z;
}
/// Accessor of complex.
///
SOPHUS_FUNC ComplexType const& complex() const {
return static_cast<Derived const*>(this)->complex();
}
/// Returns rotation matrix.
///
SOPHUS_FUNC Transformation rotationMatrix() const {
ComplexTemporaryType norm_quad = complex();
norm_quad.normalize();
return SO2<Scalar>(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<Scalar>(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<Scalar>(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<Scalar> const& so2) {
using std::sqrt;
Scalar saved_scale = scale();
complex_nonconst() = so2.unit_complex();
complex_nonconst() *= saved_scale;
}
SOPHUS_FUNC SO2<Scalar> so2() const { return SO2<Scalar>(complex()); }
private:
/// Mutator of complex is private to ensure class invariant.
///
SOPHUS_FUNC ComplexType& complex_nonconst() {
return static_cast<Derived*>(this)->complex_nonconst();
}
};
/// RxSO2 using storage; derived from RxSO2Base.
template <class Scalar_, int Options>
class RxSO2 : public RxSO2Base<RxSO2<Scalar_, Options>> {
public:
using Base = RxSO2Base<RxSO2<Scalar_, 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 ComplexMember = Eigen::Matrix<Scalar, 2, 1, Options>;
/// ``Base`` is friend so complex_nonconst can be accessed from ``Base``.
friend class RxSO2Base<RxSO2<Scalar_, Options>>;
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 <class OtherDerived>
SOPHUS_FUNC RxSO2(RxSO2Base<OtherDerived> 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<Scalar>(R).unit_complex()).eval()) {}
/// Constructor from scale factor and SO2
///
/// Precondition: ``scale`` must be close to zero.
///
SOPHUS_FUNC RxSO2(Scalar const& scale, SO2<Scalar> 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<Scalar> const& z) : complex_(z) {
SOPHUS_ENSURE(complex_.squaredNorm() >= Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon(),
"Scale factor must be greater-equal epsilon: % vs %",
complex_.squaredNorm(),
Constants<Scalar>::epsilon() * Constants<Scalar>::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<Scalar>(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<Scalar> exp(Tangent const& a) {
using std::exp;
Scalar const theta = a[0];
Scalar const sigma = a[1];
Scalar s = exp(sigma);
Vector2<Scalar> z = SO2<Scalar>::exp(theta).unit_complex();
z *= s;
return RxSO2<Scalar>(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<Scalar> 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 <class UniformRandomBitGenerator>
static RxSO2 sampleUniform(UniformRandomBitGenerator& generator) {
std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
using std::exp2;
return RxSO2(exp2(uniform(generator)),
SO2<Scalar>::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 Scalar_, int Options>
class Map<Sophus::RxSO2<Scalar_>, Options>
: public Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_>, Options>> {
using Base = Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_>, 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<Map<Sophus::RxSO2<Scalar_>, Options>>;
using Base::operator=;
using Base::operator*=;
using Base::operator*;
SOPHUS_FUNC Map(Scalar* coeffs) : complex_(coeffs) {}
/// Accessor of complex.
///
SOPHUS_FUNC
Map<Sophus::Vector2<Scalar>, Options> const& complex() const {
return complex_;
}
protected:
SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options>& complex_nonconst() {
return complex_;
}
Map<Sophus::Vector2<Scalar>, 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 Scalar_, int Options>
class Map<Sophus::RxSO2<Scalar_> const, Options>
: public Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_> const, Options>> {
public:
using Base = Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_> 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<Sophus::Vector2<Scalar> const, Options> const& complex() const {
return complex_;
}
protected:
Map<Sophus::Vector2<Scalar> const, Options> const complex_;
};
} // namespace Eigen
#endif /// SOPHUS_RXSO2_HPP

725
Workspace/Thirdparty/Sophus/sophus/rxso3.hpp vendored Normal file
View File

@ -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 Scalar_, int Options = 0>
class RxSO3;
using RxSO3d = RxSO3<double>;
using RxSO3f = RxSO3<float>;
} // namespace Sophus
namespace Eigen {
namespace internal {
template <class Scalar_, int Options_>
struct traits<Sophus::RxSO3<Scalar_, Options_>> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using QuaternionType = Eigen::Quaternion<Scalar, Options>;
};
template <class Scalar_, int Options_>
struct traits<Map<Sophus::RxSO3<Scalar_>, Options_>>
: traits<Sophus::RxSO3<Scalar_, Options_>> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using QuaternionType = Map<Eigen::Quaternion<Scalar>, Options>;
};
template <class Scalar_, int Options_>
struct traits<Map<Sophus::RxSO3<Scalar_> const, Options_>>
: traits<Sophus::RxSO3<Scalar_, Options_> const> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using QuaternionType = Map<Eigen::Quaternion<Scalar> 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 Derived>
class RxSO3Base {
public:
static constexpr int Options = Eigen::internal::traits<Derived>::Options;
using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
using QuaternionType =
typename Eigen::internal::traits<Derived>::QuaternionType;
using QuaternionTemporaryType = Eigen::Quaternion<Scalar, Options>;
/// 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<Scalar, N, N>;
using Point = Vector3<Scalar>;
using HomogeneousPoint = Vector4<Scalar>;
using Line = ParametrizedLine3<Scalar>;
using Tangent = Vector<Scalar, DoF>;
using Adjoint = Matrix<Scalar, DoF, DoF>;
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 <typename OtherDerived>
using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
Scalar, typename OtherDerived::Scalar>::ReturnType;
template <typename OtherDerived>
using RxSO3Product = RxSO3<ReturnScalar<OtherDerived>>;
template <typename PointDerived>
using PointProduct = Vector3<ReturnScalar<PointDerived>>;
template <typename HPointDerived>
using HomogeneousPointProduct = Vector4<ReturnScalar<HPointDerived>>;
/// 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 <class NewScalarType>
SOPHUS_FUNC RxSO3<NewScalarType> cast() const {
return RxSO3<NewScalarType>(quaternion().template cast<NewScalarType>());
}
/// 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<Scalar> inverse() const {
return RxSO3<Scalar>(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<Scalar>(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 <class OtherDerived>
SOPHUS_FUNC RxSO3Base<Derived>& operator=(
RxSO3Base<OtherDerived> 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 <typename OtherDerived>
SOPHUS_FUNC RxSO3Product<OtherDerived> operator*(
RxSO3Base<OtherDerived> const& other) const {
using ResultT = ReturnScalar<OtherDerived>;
typename RxSO3Product<OtherDerived>::QuaternionType result_quaternion(
quaternion() * other.quaternion());
ResultT scale = result_quaternion.squaredNorm();
if (scale < Constants<ResultT>::epsilon()) {
SOPHUS_ENSURE(scale > ResultT(0), "Scale must be greater zero.");
/// Saturation to ensure class invariant.
result_quaternion.normalize();
result_quaternion.coeffs() *= sqrt(Constants<Scalar>::epsilon());
}
return RxSO3Product<OtherDerived>(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 <typename PointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<PointDerived, 3>::value>::type>
SOPHUS_FUNC PointProduct<PointDerived> operator*(
Eigen::MatrixBase<PointDerived> const& p) const {
// Follows http:///eigen.tuxfamily.org/bz/show_bug.cgi?id=459
Scalar scale = quaternion().squaredNorm();
PointProduct<PointDerived> 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 <typename HPointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<HPointDerived, 4>::value>::type>
SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
Eigen::MatrixBase<HPointDerived> const& p) const {
const auto rsp = *this * p.template head<3>();
return HomogeneousPointProduct<HPointDerived>(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 <typename OtherDerived,
typename = typename std::enable_if<
std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
SOPHUS_FUNC RxSO3Base<Derived>& operator*=(
RxSO3Base<OtherDerived> const& other) {
*static_cast<Derived*>(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<Scalar, num_parameters> params() const {
return quaternion().coeffs();
}
/// Sets non-zero quaternion
///
/// Precondition: ``quat`` must not be close to zero.
SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) {
SOPHUS_ENSURE(quat.squaredNorm() > Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon(),
"Scale factor must be greater-equal epsilon.");
static_cast<Derived*>(this)->quaternion_nonconst() = quat;
}
/// Accessor of quaternion.
///
SOPHUS_FUNC QuaternionType const& quaternion() const {
return static_cast<Derived const*>(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<Scalar>::epsilon() *
Constants<Scalar>::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<Scalar> const& so3) {
using std::sqrt;
Scalar saved_scale = scale();
quaternion_nonconst() = so3.unit_quaternion();
quaternion_nonconst().coeffs() *= sqrt(saved_scale);
}
SOPHUS_FUNC SO3<Scalar> so3() const { return SO3<Scalar>(quaternion()); }
private:
/// Mutator of quaternion is private to ensure class invariant.
///
SOPHUS_FUNC QuaternionType& quaternion_nonconst() {
return static_cast<Derived*>(this)->quaternion_nonconst();
}
};
/// RxSO3 using storage; derived from RxSO3Base.
template <class Scalar_, int Options>
class RxSO3 : public RxSO3Base<RxSO3<Scalar_, Options>> {
public:
using Base = RxSO3Base<RxSO3<Scalar_, 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 QuaternionMember = Eigen::Quaternion<Scalar, Options>;
/// ``Base`` is friend so quaternion_nonconst can be accessed from ``Base``.
friend class RxSO3Base<RxSO3<Scalar_, Options>>;
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 <class OtherDerived>
SOPHUS_FUNC RxSO3(RxSO3Base<OtherDerived> 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<Scalar>::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<Scalar> const& so3)
: quaternion_(so3.unit_quaternion()) {
SOPHUS_ENSURE(scale >= Constants<Scalar>::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 <class D>
SOPHUS_FUNC explicit RxSO3(Eigen::QuaternionBase<D> const& quat)
: quaternion_(quat) {
static_assert(std::is_same<typename D::Scalar, Scalar>::value,
"must be same Scalar type.");
SOPHUS_ENSURE(quaternion_.squaredNorm() >= Constants<Scalar>::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<Scalar> 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<Scalar> expAndTheta(Tangent const& a,
Scalar* theta) {
SOPHUS_ENSURE(theta != nullptr, "must not be nullptr.");
using std::exp;
using std::sqrt;
Vector3<Scalar> const omega = a.template head<3>();
Scalar sigma = a[3];
Scalar sqrt_scale = sqrt(exp(sigma));
Eigen::Quaternion<Scalar> quat =
SO3<Scalar>::expAndTheta(omega, theta).unit_quaternion();
quat.coeffs() *= sqrt_scale;
return RxSO3<Scalar>(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<Scalar> const omega1 = a.template head<3>();
Vector3<Scalar> const omega2 = b.template head<3>();
Vector4<Scalar> 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 <class UniformRandomBitGenerator>
static RxSO3 sampleUniform(UniformRandomBitGenerator& generator) {
std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
using std::exp2;
return RxSO3(exp2(uniform(generator)),
SO3<Scalar>::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 Scalar_, int Options>
class Map<Sophus::RxSO3<Scalar_>, Options>
: public Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_>, Options>> {
public:
using Base = Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_>, 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<Map<Sophus::RxSO3<Scalar_>, Options>>;
using Base::operator=;
using Base::operator*=;
using Base::operator*;
SOPHUS_FUNC Map(Scalar* coeffs) : quaternion_(coeffs) {}
/// Accessor of quaternion.
///
SOPHUS_FUNC
Map<Eigen::Quaternion<Scalar>, Options> const& quaternion() const {
return quaternion_;
}
protected:
SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options>& quaternion_nonconst() {
return quaternion_;
}
Map<Eigen::Quaternion<Scalar>, 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 Scalar_, int Options>
class Map<Sophus::RxSO3<Scalar_> const, Options>
: public Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_> const, Options>> {
public:
using Base = Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_> 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<Eigen::Quaternion<Scalar> const, Options> const& quaternion() const {
return quaternion_;
}
protected:
Map<Eigen::Quaternion<Scalar> const, Options> const quaternion_;
};
} // namespace Eigen
#endif /// SOPHUS_RXSO3_HPP

835
Workspace/Thirdparty/Sophus/sophus/se2.hpp vendored Normal file
View File

@ -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 Scalar_, int Options = 0>
class SE2;
using SE2d = SE2<double>;
using SE2f = SE2<float>;
} // namespace Sophus
namespace Eigen {
namespace internal {
template <class Scalar_, int Options>
struct traits<Sophus::SE2<Scalar_, Options>> {
using Scalar = Scalar_;
using TranslationType = Sophus::Vector2<Scalar, Options>;
using SO2Type = Sophus::SO2<Scalar, Options>;
};
template <class Scalar_, int Options>
struct traits<Map<Sophus::SE2<Scalar_>, Options>>
: traits<Sophus::SE2<Scalar_, Options>> {
using Scalar = Scalar_;
using TranslationType = Map<Sophus::Vector2<Scalar>, Options>;
using SO2Type = Map<Sophus::SO2<Scalar>, Options>;
};
template <class Scalar_, int Options>
struct traits<Map<Sophus::SE2<Scalar_> const, Options>>
: traits<Sophus::SE2<Scalar_, Options> const> {
using Scalar = Scalar_;
using TranslationType = Map<Sophus::Vector2<Scalar> const, Options>;
using SO2Type = Map<Sophus::SO2<Scalar> 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 Derived>
class SE2Base {
public:
using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
using TranslationType =
typename Eigen::internal::traits<Derived>::TranslationType;
using SO2Type = typename Eigen::internal::traits<Derived>::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<Scalar, N, N>;
using Point = Vector2<Scalar>;
using HomogeneousPoint = Vector3<Scalar>;
using Line = ParametrizedLine2<Scalar>;
using Tangent = Vector<Scalar, DoF>;
using Adjoint = Matrix<Scalar, DoF, DoF>;
/// 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 <typename OtherDerived>
using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
Scalar, typename OtherDerived::Scalar>::ReturnType;
template <typename OtherDerived>
using SE2Product = SE2<ReturnScalar<OtherDerived>>;
template <typename PointDerived>
using PointProduct = Vector2<ReturnScalar<PointDerived>>;
template <typename HPointDerived>
using HomogeneousPointProduct = Vector3<ReturnScalar<HPointDerived>>;
/// 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<Scalar, 2, 2> 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 <class NewScalarType>
SOPHUS_FUNC SE2<NewScalarType> cast() const {
return SE2<NewScalarType>(so2().template cast<NewScalarType>(),
translation().template cast<NewScalarType>());
}
/// Returns derivative of this * exp(x) wrt x at x=0.
///
SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
const {
Matrix<Scalar, num_parameters, DoF> J;
Sophus::Vector2<Scalar> 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<Scalar> inverse() const {
SO2<Scalar> const invR = so2().inverse();
return SE2<Scalar>(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<Scalar> z = so2().unit_complex();
Scalar real_minus_one = z.x() - Scalar(1.);
if (abs(real_minus_one) < Constants<Scalar>::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<Scalar, 2, 2> 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, 1, 3>(Scalar(0), Scalar(0), Scalar(1));
return homogenious_matrix;
}
/// Returns the significant first two rows of the matrix above.
///
SOPHUS_FUNC Matrix<Scalar, 2, 3> matrix2x3() const {
Matrix<Scalar, 2, 3> matrix;
matrix.template topLeftCorner<2, 2>() = rotationMatrix();
matrix.col(2) = translation();
return matrix;
}
/// Assignment-like operator from OtherDerived.
///
template <class OtherDerived>
SOPHUS_FUNC SE2Base<Derived>& operator=(SE2Base<OtherDerived> const& other) {
so2() = other.so2();
translation() = other.translation();
return *this;
}
/// Group multiplication, which is rotation concatenation.
///
template <typename OtherDerived>
SOPHUS_FUNC SE2Product<OtherDerived> operator*(
SE2Base<OtherDerived> const& other) const {
return SE2Product<OtherDerived>(
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 <typename PointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<PointDerived, 2>::value>::type>
SOPHUS_FUNC PointProduct<PointDerived> operator*(
Eigen::MatrixBase<PointDerived> const& p) const {
return so2() * p + translation();
}
/// Group action on homogeneous 2-points. See above for more details.
///
template <typename HPointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<HPointDerived, 3>::value>::type>
SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
Eigen::MatrixBase<HPointDerived> const& p) const {
const PointProduct<HPointDerived> tp =
so2() * p.template head<2>() + p(2) * translation();
return HomogeneousPointProduct<HPointDerived>(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 <typename OtherDerived,
typename = typename std::enable_if<
std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
SOPHUS_FUNC SE2Base<Derived>& operator*=(SE2Base<OtherDerived> const& other) {
*static_cast<Derived*>(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<Scalar, num_parameters> params() const {
Sophus::Vector<Scalar, num_parameters> p;
p << so2().params(), translation();
return p;
}
/// Returns rotation matrix.
///
SOPHUS_FUNC Matrix<Scalar, 2, 2> 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<Scalar> 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<Scalar, 2, 2> 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<Derived*>(this)->so2(); }
/// Accessor of SO3 group.
///
SOPHUS_FUNC
SO2Type const& so2() const {
return static_cast<Derived const*>(this)->so2();
}
/// Mutator of translation vector.
///
SOPHUS_FUNC
TranslationType& translation() {
return static_cast<Derived*>(this)->translation();
}
/// Accessor of translation vector
///
SOPHUS_FUNC
TranslationType const& translation() const {
return static_cast<Derived const*>(this)->translation();
}
/// Accessor of unit complex number.
///
SOPHUS_FUNC
typename Eigen::internal::traits<Derived>::SO2Type::ComplexT const&
unit_complex() const {
return so2().unit_complex();
}
};
/// SE2 using default storage; derived from SE2Base.
template <class Scalar_, int Options>
class SE2 : public SE2Base<SE2<Scalar_, Options>> {
public:
using Base = SE2Base<SE2<Scalar_, Options>>;
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<Scalar, Options>;
using TranslationMember = Vector2<Scalar, Options>;
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 <class OtherDerived>
SOPHUS_FUNC SE2(SE2Base<OtherDerived> const& other)
: so2_(other.so2()), translation_(other.translation()) {
static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
"must be same Scalar type");
}
/// Constructor from SO3 and translation vector
///
template <class OtherDerived, class D>
SOPHUS_FUNC SE2(SO2Base<OtherDerived> const& so2,
Eigen::MatrixBase<D> const& translation)
: so2_(so2), translation_(translation) {
static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
"must be same Scalar type");
static_assert(std::is_same<typename D::Scalar, Scalar>::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<Scalar>::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<Scalar> 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<Scalar, num_parameters, DoF> Dx_exp_x(
Tangent const& upsilon_theta) {
using std::abs;
using std::cos;
using std::pow;
using std::sin;
Sophus::Matrix<Scalar, num_parameters, DoF> J;
Sophus::Vector<Scalar, 2> upsilon = upsilon_theta.template head<2>();
Scalar theta = upsilon_theta[2];
if (abs(theta) < Constants<Scalar>::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<Scalar, num_parameters, DoF>
Dx_exp_x_at_0() {
Sophus::Matrix<Scalar, num_parameters, DoF> 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<Scalar> exp(Tangent const& a) {
Scalar theta = a[2];
SO2<Scalar> so2 = SO2<Scalar>::exp(theta);
Scalar sin_theta_by_theta;
Scalar one_minus_cos_theta_by_theta;
using std::abs;
if (abs(theta) < Constants<Scalar>::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<Scalar> 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<Scalar>(so2, trans);
}
/// Returns closest SE3 given arbitrary 4x4 matrix.
///
template <class S = Scalar>
static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SE2>
fitToSE2(Matrix3<Scalar> const& T) {
return SE2(SO2<Scalar>::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<Scalar>::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<Scalar> upsilon1 = a.template head<2>();
Vector2<Scalar> 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<Scalar>(x), Sophus::Vector2<Scalar>::Zero());
}
/// Draw uniform sample from SE(2) manifold.
///
/// Translations are drawn component-wise from the range [-1, 1].
///
template <class UniformRandomBitGenerator>
static SE2 sampleUniform(UniformRandomBitGenerator& generator) {
std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
return SE2(SO2<Scalar>::sampleUniform(generator),
Vector2<Scalar>(uniform(generator), uniform(generator)));
}
/// Construct a translation only SE(2) instance.
///
template <class T0, class T1>
static SOPHUS_FUNC SE2 trans(T0 const& x, T1 const& y) {
return SE2(SO2<Scalar>(), Vector2<Scalar>(x, y));
}
static SOPHUS_FUNC SE2 trans(Vector2<Scalar> const& xy) {
return SE2(SO2<Scalar>(), 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<Scalar>::epsilon(),
"Omega: \n%", Omega);
Tangent upsilon_omega;
upsilon_omega.template head<2>() = Omega.col(2).template head<2>();
upsilon_omega[2] = SO2<Scalar>::vee(Omega.template topLeftCorner<2, 2>());
return upsilon_omega;
}
protected:
SO2Member so2_;
TranslationMember translation_;
};
template <class Scalar, int Options>
SE2<Scalar, Options>::SE2() : translation_(TranslationMember::Zero()) {
static_assert(std::is_standard_layout<SE2>::value,
"Assume standard layout for the use of offsetof check below.");
static_assert(
offsetof(SE2, so2_) + sizeof(Scalar) * SO2<Scalar>::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 Scalar_, int Options>
class Map<Sophus::SE2<Scalar_>, Options>
: public Sophus::SE2Base<Map<Sophus::SE2<Scalar_>, Options>> {
public:
using Base = Sophus::SE2Base<Map<Sophus::SE2<Scalar_>, 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<Scalar>::num_parameters) {}
/// Mutator of SO3
///
SOPHUS_FUNC Map<Sophus::SO2<Scalar>, Options>& so2() { return so2_; }
/// Accessor of SO3
///
SOPHUS_FUNC Map<Sophus::SO2<Scalar>, Options> const& so2() const {
return so2_;
}
/// Mutator of translation vector
///
SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options>& translation() {
return translation_;
}
/// Accessor of translation vector
///
SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options> const& translation() const {
return translation_;
}
protected:
Map<Sophus::SO2<Scalar>, Options> so2_;
Map<Sophus::Vector2<Scalar>, Options> translation_;
};
/// Specialization of Eigen::Map for ``SE2 const``; derived from SE2Base.
///
/// Allows us to wrap SE2 objects around POD array.
template <class Scalar_, int Options>
class Map<Sophus::SE2<Scalar_> const, Options>
: public Sophus::SE2Base<Map<Sophus::SE2<Scalar_> const, Options>> {
public:
using Base = Sophus::SE2Base<Map<Sophus::SE2<Scalar_> 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<Scalar>::num_parameters) {}
/// Accessor of SO3
///
SOPHUS_FUNC Map<Sophus::SO2<Scalar> const, Options> const& so2() const {
return so2_;
}
/// Accessor of translation vector
///
SOPHUS_FUNC Map<Sophus::Vector2<Scalar> const, Options> const& translation()
const {
return translation_;
}
protected:
Map<Sophus::SO2<Scalar> const, Options> const so2_;
Map<Sophus::Vector2<Scalar> const, Options> const translation_;
};
} // namespace Eigen
#endif

1076
Workspace/Thirdparty/Sophus/sophus/se3.hpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

722
Workspace/Thirdparty/Sophus/sophus/sim2.hpp vendored Normal file
View File

@ -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 Scalar_, int Options = 0>
class Sim2;
using Sim2d = Sim2<double>;
using Sim2f = Sim2<float>;
} // namespace Sophus
namespace Eigen {
namespace internal {
template <class Scalar_, int Options>
struct traits<Sophus::Sim2<Scalar_, Options>> {
using Scalar = Scalar_;
using TranslationType = Sophus::Vector2<Scalar, Options>;
using RxSO2Type = Sophus::RxSO2<Scalar, Options>;
};
template <class Scalar_, int Options>
struct traits<Map<Sophus::Sim2<Scalar_>, Options>>
: traits<Sophus::Sim2<Scalar_, Options>> {
using Scalar = Scalar_;
using TranslationType = Map<Sophus::Vector2<Scalar>, Options>;
using RxSO2Type = Map<Sophus::RxSO2<Scalar>, Options>;
};
template <class Scalar_, int Options>
struct traits<Map<Sophus::Sim2<Scalar_> const, Options>>
: traits<Sophus::Sim2<Scalar_, Options> const> {
using Scalar = Scalar_;
using TranslationType = Map<Sophus::Vector2<Scalar> const, Options>;
using RxSO2Type = Map<Sophus::RxSO2<Scalar> 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 Derived>
class Sim2Base {
public:
using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
using TranslationType =
typename Eigen::internal::traits<Derived>::TranslationType;
using RxSO2Type = typename Eigen::internal::traits<Derived>::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<Scalar, N, N>;
using Point = Vector2<Scalar>;
using HomogeneousPoint = Vector3<Scalar>;
using Line = ParametrizedLine2<Scalar>;
using Tangent = Vector<Scalar, DoF>;
using Adjoint = Matrix<Scalar, DoF, DoF>;
/// 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 <typename OtherDerived>
using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
Scalar, typename OtherDerived::Scalar>::ReturnType;
template <typename OtherDerived>
using Sim2Product = Sim2<ReturnScalar<OtherDerived>>;
template <typename PointDerived>
using PointProduct = Vector2<ReturnScalar<PointDerived>>;
template <typename HPointDerived>
using HomogeneousPointProduct = Vector3<ReturnScalar<HPointDerived>>;
/// 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 <class NewScalarType>
SOPHUS_FUNC Sim2<NewScalarType> cast() const {
return Sim2<NewScalarType>(rxso2().template cast<NewScalarType>(),
translation().template cast<NewScalarType>());
}
/// Returns group inverse.
///
SOPHUS_FUNC Sim2<Scalar> inverse() const {
RxSO2<Scalar> invR = rxso2().inverse();
return Sim2<Scalar>(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<Scalar> const theta_sigma = rxso2().log();
Scalar const theta = theta_sigma[0];
Scalar const sigma = theta_sigma[1];
Matrix2<Scalar> const Omega = SO2<Scalar>::hat(theta);
Matrix2<Scalar> const W_inv =
details::calcWInv<Scalar, 2>(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, 3, 1>(Scalar(0), Scalar(0), Scalar(1));
return homogenious_matrix;
}
/// Returns the significant first two rows of the matrix above.
///
SOPHUS_FUNC Matrix<Scalar, 2, 3> matrix2x3() const {
Matrix<Scalar, 2, 3> matrix;
matrix.template topLeftCorner<2, 2>() = rxso2().matrix();
matrix.col(2) = translation();
return matrix;
}
/// Assignment-like operator from OtherDerived.
///
template <class OtherDerived>
SOPHUS_FUNC Sim2Base<Derived>& operator=(
Sim2Base<OtherDerived> 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 <typename OtherDerived>
SOPHUS_FUNC Sim2Product<OtherDerived> operator*(
Sim2Base<OtherDerived> const& other) const {
return Sim2Product<OtherDerived>(
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 <typename PointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<PointDerived, 2>::value>::type>
SOPHUS_FUNC PointProduct<PointDerived> operator*(
Eigen::MatrixBase<PointDerived> const& p) const {
return rxso2() * p + translation();
}
/// Group action on homogeneous 2-points. See above for more details.
///
template <typename HPointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<HPointDerived, 3>::value>::type>
SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
Eigen::MatrixBase<HPointDerived> const& p) const {
const PointProduct<HPointDerived> tp =
rxso2() * p.template head<2>() + p(2) * translation();
return HomogeneousPointProduct<HPointDerived>(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<Scalar, num_parameters> params() const {
Sophus::Vector<Scalar, num_parameters> 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 <typename OtherDerived,
typename = typename std::enable_if<
std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
SOPHUS_FUNC Sim2Base<Derived>& operator*=(
Sim2Base<OtherDerived> const& other) {
*static_cast<Derived*>(this) = *this * other;
return *this;
}
/// Setter of non-zero complex number.
///
/// Precondition: ``z`` must not be close to zero.
///
SOPHUS_FUNC void setComplex(Vector2<Scalar> const& z) {
rxso2().setComplex(z);
}
/// Accessor of complex number.
///
SOPHUS_FUNC
typename Eigen::internal::traits<Derived>::RxSO2Type::ComplexType const&
complex() const {
return rxso2().complex();
}
/// Returns Rotation matrix
///
SOPHUS_FUNC Matrix2<Scalar> rotationMatrix() const {
return rxso2().rotationMatrix();
}
/// Mutator of SO2 group.
///
SOPHUS_FUNC RxSO2Type& rxso2() {
return static_cast<Derived*>(this)->rxso2();
}
/// Accessor of SO2 group.
///
SOPHUS_FUNC RxSO2Type const& rxso2() const {
return static_cast<Derived const*>(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<Scalar>& 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<Scalar> const& sR) {
rxso2().setScaledRotationMatrix(sR);
}
/// Mutator of translation vector
///
SOPHUS_FUNC TranslationType& translation() {
return static_cast<Derived*>(this)->translation();
}
/// Accessor of translation vector
///
SOPHUS_FUNC TranslationType const& translation() const {
return static_cast<Derived const*>(this)->translation();
}
};
/// Sim2 using default storage; derived from Sim2Base.
template <class Scalar_, int Options>
class Sim2 : public Sim2Base<Sim2<Scalar_, Options>> {
public:
using Base = Sim2Base<Sim2<Scalar_, 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 RxSo2Member = RxSO2<Scalar, Options>;
using TranslationMember = Vector2<Scalar, Options>;
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 <class OtherDerived>
SOPHUS_FUNC Sim2(Sim2Base<OtherDerived> const& other)
: rxso2_(other.rxso2()), translation_(other.translation()) {
static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
"must be same Scalar type");
}
/// Constructor from RxSO2 and translation vector
///
template <class OtherDerived, class D>
SOPHUS_FUNC Sim2(RxSO2Base<OtherDerived> const& rxso2,
Eigen::MatrixBase<D> const& translation)
: rxso2_(rxso2), translation_(translation) {
static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
"must be same Scalar type");
static_assert(std::is_same<typename D::Scalar, Scalar>::value,
"must be same Scalar type");
}
/// Constructor from complex number and translation vector.
///
/// Precondition: complex number must not be close to zero.
///
template <class D>
SOPHUS_FUNC Sim2(Vector2<Scalar> const& complex_number,
Eigen::MatrixBase<D> const& translation)
: rxso2_(complex_number), translation_(translation) {
static_assert(std::is_same<typename D::Scalar, Scalar>::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<Scalar, 3, 3> 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<Scalar> 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<Scalar> const upsilon = a.segment(0, 2);
Scalar const theta = a[2];
Scalar const sigma = a[3];
RxSO2<Scalar> rxso2 = RxSO2<Scalar>::exp(a.template tail<2>());
Matrix2<Scalar> const Omega = SO2<Scalar>::hat(theta);
Matrix2<Scalar> const W = details::calcW<Scalar, 2>(Omega, theta, sigma);
return Sim2<Scalar>(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<Scalar>::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<Scalar> const upsilon1 = a.template head<2>();
Vector2<Scalar> 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 <class UniformRandomBitGenerator>
static Sim2 sampleUniform(UniformRandomBitGenerator& generator) {
std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
return Sim2(RxSO2<Scalar>::sampleUniform(generator),
Vector2<Scalar>(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<Scalar>::vee(Omega.template topLeftCorner<2, 2>());
return upsilon_omega_sigma;
}
protected:
RxSo2Member rxso2_;
TranslationMember translation_;
};
template <class Scalar, int Options>
Sim2<Scalar, Options>::Sim2() : translation_(TranslationMember::Zero()) {
static_assert(std::is_standard_layout<Sim2>::value,
"Assume standard layout for the use of offsetof check below.");
static_assert(
offsetof(Sim2, rxso2_) + sizeof(Scalar) * RxSO2<Scalar>::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 Scalar_, int Options>
class Map<Sophus::Sim2<Scalar_>, Options>
: public Sophus::Sim2Base<Map<Sophus::Sim2<Scalar_>, Options>> {
public:
using Base = Sophus::Sim2Base<Map<Sophus::Sim2<Scalar_>, 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<Scalar>::num_parameters) {}
/// Mutator of RxSO2
///
SOPHUS_FUNC Map<Sophus::RxSO2<Scalar>, Options>& rxso2() { return rxso2_; }
/// Accessor of RxSO2
///
SOPHUS_FUNC Map<Sophus::RxSO2<Scalar>, Options> const& rxso2() const {
return rxso2_;
}
/// Mutator of translation vector
///
SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options>& translation() {
return translation_;
}
/// Accessor of translation vector
SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options> const& translation() const {
return translation_;
}
protected:
Map<Sophus::RxSO2<Scalar>, Options> rxso2_;
Map<Sophus::Vector2<Scalar>, Options> translation_;
};
/// Specialization of Eigen::Map for ``Sim2 const``; derived from Sim2Base.
///
/// Allows us to wrap RxSO2 objects around POD array.
template <class Scalar_, int Options>
class Map<Sophus::Sim2<Scalar_> const, Options>
: public Sophus::Sim2Base<Map<Sophus::Sim2<Scalar_> const, Options>> {
public:
using Base = Sophus::Sim2Base<Map<Sophus::Sim2<Scalar_> 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<Scalar>::num_parameters) {}
/// Accessor of RxSO2
///
SOPHUS_FUNC Map<Sophus::RxSO2<Scalar> const, Options> const& rxso2() const {
return rxso2_;
}
/// Accessor of translation vector
///
SOPHUS_FUNC Map<Sophus::Vector2<Scalar> const, Options> const& translation()
const {
return translation_;
}
protected:
Map<Sophus::RxSO2<Scalar> const, Options> const rxso2_;
Map<Sophus::Vector2<Scalar> const, Options> const translation_;
};
} // namespace Eigen
#endif

740
Workspace/Thirdparty/Sophus/sophus/sim3.hpp vendored Normal file
View File

@ -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 Scalar_, int Options = 0>
class Sim3;
using Sim3d = Sim3<double>;
using Sim3f = Sim3<float>;
} // namespace Sophus
namespace Eigen {
namespace internal {
template <class Scalar_, int Options>
struct traits<Sophus::Sim3<Scalar_, Options>> {
using Scalar = Scalar_;
using TranslationType = Sophus::Vector3<Scalar, Options>;
using RxSO3Type = Sophus::RxSO3<Scalar, Options>;
};
template <class Scalar_, int Options>
struct traits<Map<Sophus::Sim3<Scalar_>, Options>>
: traits<Sophus::Sim3<Scalar_, Options>> {
using Scalar = Scalar_;
using TranslationType = Map<Sophus::Vector3<Scalar>, Options>;
using RxSO3Type = Map<Sophus::RxSO3<Scalar>, Options>;
};
template <class Scalar_, int Options>
struct traits<Map<Sophus::Sim3<Scalar_> const, Options>>
: traits<Sophus::Sim3<Scalar_, Options> const> {
using Scalar = Scalar_;
using TranslationType = Map<Sophus::Vector3<Scalar> const, Options>;
using RxSO3Type = Map<Sophus::RxSO3<Scalar> 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 Derived>
class Sim3Base {
public:
using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
using TranslationType =
typename Eigen::internal::traits<Derived>::TranslationType;
using RxSO3Type = typename Eigen::internal::traits<Derived>::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<Scalar, N, N>;
using Point = Vector3<Scalar>;
using HomogeneousPoint = Vector4<Scalar>;
using Line = ParametrizedLine3<Scalar>;
using Tangent = Vector<Scalar, DoF>;
using Adjoint = Matrix<Scalar, DoF, DoF>;
/// 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 <typename OtherDerived>
using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
Scalar, typename OtherDerived::Scalar>::ReturnType;
template <typename OtherDerived>
using Sim3Product = Sim3<ReturnScalar<OtherDerived>>;
template <typename PointDerived>
using PointProduct = Vector3<ReturnScalar<PointDerived>>;
template <typename HPointDerived>
using HomogeneousPointProduct = Vector4<ReturnScalar<HPointDerived>>;
/// 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<Scalar> 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<Scalar>::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 <class NewScalarType>
SOPHUS_FUNC Sim3<NewScalarType> cast() const {
return Sim3<NewScalarType>(rxso3().template cast<NewScalarType>(),
translation().template cast<NewScalarType>());
}
/// Returns group inverse.
///
SOPHUS_FUNC Sim3<Scalar> inverse() const {
RxSO3<Scalar> invR = rxso3().inverse();
return Sim3<Scalar>(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<Scalar> const omega =
omega_sigma_and_theta.tangent.template head<3>();
Scalar sigma = omega_sigma_and_theta.tangent[3];
Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);
Matrix3<Scalar> const W_inv = details::calcWInv<Scalar, 3>(
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, 4, 1>(Scalar(0), Scalar(0), Scalar(0), Scalar(1));
return homogenious_matrix;
}
/// Returns the significant first three rows of the matrix above.
///
SOPHUS_FUNC Matrix<Scalar, 3, 4> matrix3x4() const {
Matrix<Scalar, 3, 4> matrix;
matrix.template topLeftCorner<3, 3>() = rxso3().matrix();
matrix.col(3) = translation();
return matrix;
}
/// Assignment-like operator from OtherDerived.
///
template <class OtherDerived>
SOPHUS_FUNC Sim3Base<Derived>& operator=(
Sim3Base<OtherDerived> 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 <typename OtherDerived>
SOPHUS_FUNC Sim3Product<OtherDerived> operator*(
Sim3Base<OtherDerived> const& other) const {
return Sim3Product<OtherDerived>(
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 <typename PointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<PointDerived, 3>::value>::type>
SOPHUS_FUNC PointProduct<PointDerived> operator*(
Eigen::MatrixBase<PointDerived> const& p) const {
return rxso3() * p + translation();
}
/// Group action on homogeneous 3-points. See above for more details.
///
template <typename HPointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<HPointDerived, 4>::value>::type>
SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
Eigen::MatrixBase<HPointDerived> const& p) const {
const PointProduct<HPointDerived> tp =
rxso3() * p.template head<3>() + p(3) * translation();
return HomogeneousPointProduct<HPointDerived>(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 <typename OtherDerived,
typename = typename std::enable_if<
std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
SOPHUS_FUNC Sim3Base<Derived>& operator*=(
Sim3Base<OtherDerived> const& other) {
*static_cast<Derived*>(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<Scalar, num_parameters> params() const {
Sophus::Vector<Scalar, num_parameters> 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<Scalar> const& quat) {
rxso3().setQuaternion(quat);
}
/// Accessor of quaternion.
///
SOPHUS_FUNC QuaternionType const& quaternion() const {
return rxso3().quaternion();
}
/// Returns Rotation matrix
///
SOPHUS_FUNC Matrix3<Scalar> rotationMatrix() const {
return rxso3().rotationMatrix();
}
/// Mutator of SO3 group.
///
SOPHUS_FUNC RxSO3Type& rxso3() {
return static_cast<Derived*>(this)->rxso3();
}
/// Accessor of SO3 group.
///
SOPHUS_FUNC RxSO3Type const& rxso3() const {
return static_cast<Derived const*>(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<Scalar>& 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<Scalar> const& sR) {
rxso3().setScaledRotationMatrix(sR);
}
/// Mutator of translation vector
///
SOPHUS_FUNC TranslationType& translation() {
return static_cast<Derived*>(this)->translation();
}
/// Accessor of translation vector
///
SOPHUS_FUNC TranslationType const& translation() const {
return static_cast<Derived const*>(this)->translation();
}
};
/// Sim3 using default storage; derived from Sim3Base.
template <class Scalar_, int Options>
class Sim3 : public Sim3Base<Sim3<Scalar_, Options>> {
public:
using Base = Sim3Base<Sim3<Scalar_, 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 RxSo3Member = RxSO3<Scalar, Options>;
using TranslationMember = Vector3<Scalar, Options>;
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 <class OtherDerived>
SOPHUS_FUNC Sim3(Sim3Base<OtherDerived> const& other)
: rxso3_(other.rxso3()), translation_(other.translation()) {
static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
"must be same Scalar type");
}
/// Constructor from RxSO3 and translation vector
///
template <class OtherDerived, class D>
SOPHUS_FUNC Sim3(RxSO3Base<OtherDerived> const& rxso3,
Eigen::MatrixBase<D> const& translation)
: rxso3_(rxso3), translation_(translation) {
static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
"must be same Scalar type");
static_assert(std::is_same<typename D::Scalar, Scalar>::value,
"must be same Scalar type");
}
/// Constructor from quaternion and translation vector.
///
/// Precondition: quaternion must not be close to zero.
///
template <class D1, class D2>
SOPHUS_FUNC Sim3(Eigen::QuaternionBase<D1> const& quaternion,
Eigen::MatrixBase<D2> const& translation)
: rxso3_(quaternion), translation_(translation) {
static_assert(std::is_same<typename D1::Scalar, Scalar>::value,
"must be same Scalar type");
static_assert(std::is_same<typename D2::Scalar, Scalar>::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<Scalar, 4, 4> 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<Scalar> 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<Scalar> const upsilon = a.segment(0, 3);
Vector3<Scalar> const omega = a.segment(3, 3);
Scalar const sigma = a[6];
Scalar theta;
RxSO3<Scalar> rxso3 =
RxSO3<Scalar>::expAndTheta(a.template tail<4>(), &theta);
Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);
Matrix3<Scalar> const W = details::calcW<Scalar, 3>(Omega, theta, sigma);
return Sim3<Scalar>(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<Scalar>::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<Scalar> const upsilon1 = a.template head<3>();
Vector3<Scalar> const upsilon2 = b.template head<3>();
Vector3<Scalar> const omega1 = a.template segment<3>(3);
Vector3<Scalar> const omega2 = b.template segment<3>(3);
Scalar sigma1 = a[6];
Scalar sigma2 = b[6];
Tangent res;
res.template head<3>() = SO3<Scalar>::hat(omega1) * upsilon2 +
SO3<Scalar>::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 <class UniformRandomBitGenerator>
static Sim3 sampleUniform(UniformRandomBitGenerator& generator) {
std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
return Sim3(RxSO3<Scalar>::sampleUniform(generator),
Vector3<Scalar>(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<Scalar>::vee(Omega.template topLeftCorner<3, 3>());
return upsilon_omega_sigma;
}
protected:
RxSo3Member rxso3_;
TranslationMember translation_;
};
template <class Scalar, int Options>
Sim3<Scalar, Options>::Sim3() : translation_(TranslationMember::Zero()) {
static_assert(std::is_standard_layout<Sim3>::value,
"Assume standard layout for the use of offsetof check below.");
static_assert(
offsetof(Sim3, rxso3_) + sizeof(Scalar) * RxSO3<Scalar>::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 Scalar_, int Options>
class Map<Sophus::Sim3<Scalar_>, Options>
: public Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_>, Options>> {
public:
using Base = Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_>, 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<Scalar>::num_parameters) {}
/// Mutator of RxSO3
///
SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options>& rxso3() { return rxso3_; }
/// Accessor of RxSO3
///
SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options> const& rxso3() const {
return rxso3_;
}
/// Mutator of translation vector
///
SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options>& translation() {
return translation_;
}
/// Accessor of translation vector
SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options> const& translation() const {
return translation_;
}
protected:
Map<Sophus::RxSO3<Scalar>, Options> rxso3_;
Map<Sophus::Vector3<Scalar>, Options> translation_;
};
/// Specialization of Eigen::Map for ``Sim3 const``; derived from Sim3Base.
///
/// Allows us to wrap RxSO3 objects around POD array.
template <class Scalar_, int Options>
class Map<Sophus::Sim3<Scalar_> const, Options>
: public Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_> const, Options>> {
using Base = Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_> 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<Scalar>::num_parameters) {}
/// Accessor of RxSO3
///
SOPHUS_FUNC Map<Sophus::RxSO3<Scalar> const, Options> const& rxso3() const {
return rxso3_;
}
/// Accessor of translation vector
///
SOPHUS_FUNC Map<Sophus::Vector3<Scalar> const, Options> const& translation()
const {
return translation_;
}
protected:
Map<Sophus::RxSO3<Scalar> const, Options> const rxso3_;
Map<Sophus::Vector3<Scalar> const, Options> const translation_;
};
} // namespace Eigen
#endif

103
Workspace/Thirdparty/Sophus/sophus/sim_details.hpp vendored Normal file
View File

@ -0,0 +1,103 @@
#ifndef SOPHUS_SIM_DETAILS_HPP
#define SOPHUS_SIM_DETAILS_HPP
#include "types.hpp"
namespace Sophus {
namespace details {
template <class Scalar, int N>
Matrix<Scalar, N, N> calcW(Matrix<Scalar, N, N> const& Omega,
Scalar const theta, Scalar const sigma) {
using std::abs;
using std::cos;
using std::exp;
using std::sin;
static Matrix<Scalar, N, N> const I = Matrix<Scalar, N, N>::Identity();
static Scalar const one(1);
static Scalar const half(0.5);
Matrix<Scalar, N, N> const Omega2 = Omega * Omega;
Scalar const scale = exp(sigma);
Scalar A, B, C;
if (abs(sigma) < Constants<Scalar>::epsilon()) {
C = one;
if (abs(theta) < Constants<Scalar>::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<Scalar>::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 <class Scalar, int N>
Matrix<Scalar, N, N> calcWInv(Matrix<Scalar, N, N> const& Omega,
Scalar const theta, Scalar const sigma,
Scalar const scale) {
using std::abs;
using std::cos;
using std::sin;
static Matrix<Scalar, N, N> const I = Matrix<Scalar, N, N>::Identity();
static Scalar const half(0.5);
static Scalar const one(1);
static Scalar const two(2);
Matrix<Scalar, N, N> 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<Scalar>::epsilon()) {
c = one - half * sigma;
a = -half;
if (abs(theta_sq) < Constants<Scalar>::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<Scalar>::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

621
Workspace/Thirdparty/Sophus/sophus/so2.hpp vendored Normal file
View File

@ -0,0 +1,621 @@
/// @file
/// Special orthogonal group SO(2) - rotation in 2d.
#ifndef SOPHUS_SO2_HPP
#define SOPHUS_SO2_HPP
#include <complex>
#include <type_traits>
// Include only the selective set of Eigen headers that we need.
// This helps when using Sophus with unusual compilers, like nvcc.
#include <Eigen/LU>
#include "rotation_matrix.hpp"
#include "types.hpp"
namespace Sophus {
template <class Scalar_, int Options = 0>
class SO2;
using SO2d = SO2<double>;
using SO2f = SO2<float>;
} // namespace Sophus
namespace Eigen {
namespace internal {
template <class Scalar_, int Options_>
struct traits<Sophus::SO2<Scalar_, Options_>> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using ComplexType = Sophus::Vector2<Scalar, Options>;
};
template <class Scalar_, int Options_>
struct traits<Map<Sophus::SO2<Scalar_>, Options_>>
: traits<Sophus::SO2<Scalar_, Options_>> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using ComplexType = Map<Sophus::Vector2<Scalar>, Options>;
};
template <class Scalar_, int Options_>
struct traits<Map<Sophus::SO2<Scalar_> const, Options_>>
: traits<Sophus::SO2<Scalar_, Options_> const> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using ComplexType = Map<Sophus::Vector2<Scalar> 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 Derived>
class SO2Base {
public:
static constexpr int Options = Eigen::internal::traits<Derived>::Options;
using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
using ComplexT = typename Eigen::internal::traits<Derived>::ComplexType;
using ComplexTemporaryType = Sophus::Vector2<Scalar, Options>;
/// 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<Scalar, N, N>;
using Point = Vector2<Scalar>;
using HomogeneousPoint = Vector3<Scalar>;
using Line = ParametrizedLine2<Scalar>;
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 <typename OtherDerived>
using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
Scalar, typename OtherDerived::Scalar>::ReturnType;
template <typename OtherDerived>
using SO2Product = SO2<ReturnScalar<OtherDerived>>;
template <typename PointDerived>
using PointProduct = Vector2<ReturnScalar<PointDerived>>;
template <typename HPointDerived>
using HomogeneousPointProduct = Vector3<ReturnScalar<HPointDerived>>;
/// 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 <class NewScalarType>
SOPHUS_FUNC SO2<NewScalarType> cast() const {
return SO2<NewScalarType>(unit_complex().template cast<NewScalarType>());
}
/// 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<Scalar> inverse() const {
return SO2<Scalar>(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<Scalar>::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 <class OtherDerived>
SOPHUS_FUNC SO2Base<Derived>& operator=(SO2Base<OtherDerived> const& other) {
unit_complex_nonconst() = other.unit_complex();
return *this;
}
/// Group multiplication, which is rotation concatenation.
///
template <typename OtherDerived>
SOPHUS_FUNC SO2Product<OtherDerived> operator*(
SO2Base<OtherDerived> const& other) const {
using ResultT = ReturnScalar<OtherDerived>;
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<OtherDerived>(result_real * scale, result_imag * scale);
}
return SO2Product<OtherDerived>(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 <typename PointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<PointDerived, 2>::value>::type>
SOPHUS_FUNC PointProduct<PointDerived> operator*(
Eigen::MatrixBase<PointDerived> const& p) const {
Scalar const& real = unit_complex().x();
Scalar const& imag = unit_complex().y();
return PointProduct<PointDerived>(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 <typename HPointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<HPointDerived, 3>::value>::type>
SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
Eigen::MatrixBase<HPointDerived> const& p) const {
Scalar const& real = unit_complex().x();
Scalar const& imag = unit_complex().y();
return HomogeneousPointProduct<HPointDerived>(
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 <typename OtherDerived,
typename = typename std::enable_if<
std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
SOPHUS_FUNC SO2Base<Derived> operator*=(SO2Base<OtherDerived> const& other) {
*static_cast<Derived*>(this) = *this * other;
return *this;
}
/// Returns derivative of this * SO2::exp(x) wrt. x at x=0.
///
SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
const {
return Matrix<Scalar, num_parameters, DoF>(-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<Scalar, num_parameters> 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<Derived const*>(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<Derived*>(this)->unit_complex_nonconst();
}
};
/// SO2 using default storage; derived from SO2Base.
template <class Scalar_, int Options>
class SO2 : public SO2Base<SO2<Scalar_, Options>> {
public:
using Base = SO2Base<SO2<Scalar_, Options>>;
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<Scalar, Options>;
/// ``Base`` is friend so unit_complex_nonconst can be accessed from ``Base``.
friend class SO2Base<SO2<Scalar, Options>>;
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 <class OtherDerived>
SOPHUS_FUNC SO2(SO2Base<OtherDerived> 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 <class D>
SOPHUS_FUNC explicit SO2(Eigen::MatrixBase<D> const& complex)
: unit_complex_(complex) {
static_assert(std::is_same<typename D::Scalar, Scalar>::value,
"must be same Scalar type");
Base::normalize();
}
/// Constructor from an rotation angle.
///
SOPHUS_FUNC explicit SO2(Scalar theta) {
unit_complex_nonconst() = SO2<Scalar>::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<Scalar> exp(Tangent const& theta) {
using std::cos;
using std::sin;
return SO2<Scalar>(cos(theta), sin(theta));
}
/// Returns derivative of exp(x) wrt. x.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
Tangent const& theta) {
using std::cos;
using std::sin;
return Sophus::Matrix<Scalar, num_parameters, DoF>(-sin(theta), cos(theta));
}
/// Returns derivative of exp(x) wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
Dx_exp_x_at_0() {
return Sophus::Matrix<Scalar, num_parameters, DoF>(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 <class S = Scalar>
static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::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 <class UniformRandomBitGenerator>
static SO2 sampleUniform(UniformRandomBitGenerator& generator) {
static_assert(IsUniformRandomBitGenerator<UniformRandomBitGenerator>::value,
"generator must meet the UniformRandomBitGenerator concept");
std::uniform_real_distribution<Scalar> uniform(-Constants<Scalar>::pi(),
Constants<Scalar>::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 Scalar_, int Options>
class Map<Sophus::SO2<Scalar_>, Options>
: public Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>> {
public:
using Base = Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, 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<Map<Sophus::SO2<Scalar_>, 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<Sophus::Vector2<Scalar>, Options> const& unit_complex() const {
return unit_complex_;
}
protected:
/// Mutator of unit_complex is protected to ensure class invariant.
///
SOPHUS_FUNC
Map<Sophus::Vector2<Scalar>, Options>& unit_complex_nonconst() {
return unit_complex_;
}
Map<Matrix<Scalar, 2, 1>, 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 Scalar_, int Options>
class Map<Sophus::SO2<Scalar_> const, Options>
: public Sophus::SO2Base<Map<Sophus::SO2<Scalar_> const, Options>> {
public:
using Base = Sophus::SO2Base<Map<Sophus::SO2<Scalar_> 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<Sophus::Vector2<Scalar> const, Options> const& unit_complex()
const {
return unit_complex_;
}
protected:
/// Mutator of unit_complex is protected to ensure class invariant.
///
Map<Matrix<Scalar, 2, 1> const, Options> const unit_complex_;
};
} // namespace Eigen
#endif // SOPHUS_SO2_HPP

850
Workspace/Thirdparty/Sophus/sophus/so3.hpp vendored Normal file
View File

@ -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 <Eigen/src/Geometry/OrthoMethods.h>
#include <Eigen/src/Geometry/Quaternion.h>
#include <Eigen/src/Geometry/RotationBase.h>
namespace Sophus {
template <class Scalar_, int Options = 0>
class SO3;
using SO3d = SO3<double>;
using SO3f = SO3<float>;
} // namespace Sophus
namespace Eigen {
namespace internal {
template <class Scalar_, int Options_>
struct traits<Sophus::SO3<Scalar_, Options_>> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using QuaternionType = Eigen::Quaternion<Scalar, Options>;
};
template <class Scalar_, int Options_>
struct traits<Map<Sophus::SO3<Scalar_>, Options_>>
: traits<Sophus::SO3<Scalar_, Options_>> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using QuaternionType = Map<Eigen::Quaternion<Scalar>, Options>;
};
template <class Scalar_, int Options_>
struct traits<Map<Sophus::SO3<Scalar_> const, Options_>>
: traits<Sophus::SO3<Scalar_, Options_> const> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using QuaternionType = Map<Eigen::Quaternion<Scalar> 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 Derived>
class SO3Base {
public:
static constexpr int Options = Eigen::internal::traits<Derived>::Options;
using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
using QuaternionType =
typename Eigen::internal::traits<Derived>::QuaternionType;
using QuaternionTemporaryType = Eigen::Quaternion<Scalar, Options>;
/// 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<Scalar, N, N>;
using Point = Vector3<Scalar>;
using HomogeneousPoint = Vector4<Scalar>;
using Line = ParametrizedLine3<Scalar>;
using Tangent = Vector<Scalar, DoF>;
using Adjoint = Matrix<Scalar, DoF, DoF>;
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 <typename OtherDerived>
using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
Scalar, typename OtherDerived::Scalar>::ReturnType;
template <typename OtherDerived>
using SO3Product = SO3<ReturnScalar<OtherDerived>>;
template <typename PointDerived>
using PointProduct = Vector3<ReturnScalar<PointDerived>>;
template <typename HPointDerived>
using HomogeneousPointProduct = Vector4<ReturnScalar<HPointDerived>>;
/// 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 <class S = Scalar>
SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleX() const {
Sophus::Matrix3<Scalar> R = matrix();
Sophus::Matrix2<Scalar> Rx = R.template block<2, 2>(1, 1);
return SO2<Scalar>(makeRotationMatrix(Rx)).log();
}
/// Extract rotation angle about canonical Y-axis
///
template <class S = Scalar>
SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleY() const {
Sophus::Matrix3<Scalar> R = matrix();
Sophus::Matrix2<Scalar> Ry;
// clang-format off
Ry <<
R(0, 0), R(2, 0),
R(0, 2), R(2, 2);
// clang-format on
return SO2<Scalar>(makeRotationMatrix(Ry)).log();
}
/// Extract rotation angle about canonical Z-axis
///
template <class S = Scalar>
SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleZ() const {
Sophus::Matrix3<Scalar> R = matrix();
Sophus::Matrix2<Scalar> Rz = R.template block<2, 2>(0, 0);
return SO2<Scalar>(makeRotationMatrix(Rz)).log();
}
/// Returns copy of instance casted to NewScalarType.
///
template <class NewScalarType>
SOPHUS_FUNC SO3<NewScalarType> cast() const {
return SO3<NewScalarType>(unit_quaternion().template cast<NewScalarType>());
}
/// 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<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
const {
Matrix<Scalar, num_parameters, DoF> J;
Eigen::Quaternion<Scalar> 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<Scalar, num_parameters> params() const {
return unit_quaternion().coeffs();
}
/// Returns group inverse.
///
SOPHUS_FUNC SO3<Scalar> inverse() const {
return SO3<Scalar>(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<Scalar>::epsilon() * Constants<Scalar>::epsilon()) {
// If quaternion is normalized and n=0, then w should be 1;
// w=0 should never happen here!
SOPHUS_ENSURE(abs(w) >= Constants<Scalar>::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<Scalar>::epsilon()) {
if (w > Scalar(0)) {
two_atan_nbyw_by_n = Constants<Scalar>::pi() / n;
} else {
two_atan_nbyw_by_n = -Constants<Scalar>::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<Scalar>::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 <class OtherDerived>
SOPHUS_FUNC SO3Base<Derived>& operator=(SO3Base<OtherDerived> const& other) {
unit_quaternion_nonconst() = other.unit_quaternion();
return *this;
}
/// Group multiplication, which is rotation concatenation.
///
template <typename OtherDerived>
SOPHUS_FUNC SO3Product<OtherDerived> operator*(
SO3Base<OtherDerived> const& other) const {
using QuaternionProductType =
typename SO3Product<OtherDerived>::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<OtherDerived>(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 <typename PointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<PointDerived, 3>::value>::type>
SOPHUS_FUNC PointProduct<PointDerived> operator*(
Eigen::MatrixBase<PointDerived> 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<PointDerived> 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 <typename HPointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<HPointDerived, 4>::value>::type>
SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
Eigen::MatrixBase<HPointDerived> const& p) const {
const auto rp = *this * p.template head<3>();
return HomogeneousPointProduct<HPointDerived>(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 <typename OtherDerived,
typename = typename std::enable_if<
std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
SOPHUS_FUNC SO3Base<Derived>& operator*=(SO3Base<OtherDerived> const& other) {
*static_cast<Derived*>(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<Scalar> const& quaternion) {
unit_quaternion_nonconst() = quaternion;
normalize();
}
/// Accessor of unit quaternion.
///
SOPHUS_FUNC QuaternionType const& unit_quaternion() const {
return static_cast<Derived const*>(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<Derived*>(this)->unit_quaternion_nonconst();
}
};
/// SO3 using default storage; derived from SO3Base.
template <class Scalar_, int Options>
class SO3 : public SO3Base<SO3<Scalar_, Options>> {
public:
using Base = SO3Base<SO3<Scalar_, Options>>;
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<Scalar, Options>;
/// ``Base`` is friend so unit_quaternion_nonconst can be accessed from
/// ``Base``.
friend class SO3Base<SO3<Scalar, Options>>;
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 <class OtherDerived>
SOPHUS_FUNC SO3(SO3Base<OtherDerived> 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 <class D>
SOPHUS_FUNC explicit SO3(Eigen::QuaternionBase<D> const& quat)
: unit_quaternion_(quat) {
static_assert(
std::is_same<typename Eigen::QuaternionBase<D>::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<Scalar, num_parameters, DoF> 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<Scalar>::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<Scalar, num_parameters, DoF> 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<Scalar, num_parameters, DoF>
Dx_exp_x_at_0() {
Sophus::Matrix<Scalar, num_parameters, DoF> 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<Scalar> 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<Scalar> 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<Scalar>::epsilon() * Constants<Scalar>::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<Scalar>::epsilon(),
"SO3::exp failed! omega: %, real: %, img: %",
omega.transpose(), real_factor, imag_factor);
return q;
}
/// Returns closest SO3 given arbitrary 3x3 matrix.
///
template <class S = Scalar>
static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::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<Scalar>(x, Scalar(0), Scalar(0)));
}
/// Construct y-axis rotation.
///
static SOPHUS_FUNC SO3 rotY(Scalar const& y) {
return SO3::exp(Sophus::Vector3<Scalar>(Scalar(0), y, Scalar(0)));
}
/// Construct z-axis rotation.
///
static SOPHUS_FUNC SO3 rotZ(Scalar const& z) {
return SO3::exp(Sophus::Vector3<Scalar>(Scalar(0), Scalar(0), z));
}
/// Draw uniform sample from SO(3) manifold.
/// Based on: http://planning.cs.uiuc.edu/node198.html
///
template <class UniformRandomBitGenerator>
static SO3 sampleUniform(UniformRandomBitGenerator& generator) {
static_assert(IsUniformRandomBitGenerator<UniformRandomBitGenerator>::value,
"generator must meet the UniformRandomBitGenerator concept");
std::uniform_real_distribution<Scalar> uniform(Scalar(0), Scalar(1));
std::uniform_real_distribution<Scalar> uniform_twopi(
Scalar(0), 2 * Constants<Scalar>::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 Scalar_, int Options>
class Map<Sophus::SO3<Scalar_>, Options>
: public Sophus::SO3Base<Map<Sophus::SO3<Scalar_>, Options>> {
public:
using Base = Sophus::SO3Base<Map<Sophus::SO3<Scalar_>, 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<Map<Sophus::SO3<Scalar_>, 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<Eigen::Quaternion<Scalar>, Options> const& unit_quaternion()
const {
return unit_quaternion_;
}
protected:
/// Mutator of unit_quaternion is protected to ensure class invariant.
///
SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options>&
unit_quaternion_nonconst() {
return unit_quaternion_;
}
Map<Eigen::Quaternion<Scalar>, 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 Scalar_, int Options>
class Map<Sophus::SO3<Scalar_> const, Options>
: public Sophus::SO3Base<Map<Sophus::SO3<Scalar_> const, Options>> {
public:
using Base = Sophus::SO3Base<Map<Sophus::SO3<Scalar_> 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<Eigen::Quaternion<Scalar> const, Options> const&
unit_quaternion() const {
return unit_quaternion_;
}
protected:
/// Mutator of unit_quaternion is protected to ensure class invariant.
///
Map<Eigen::Quaternion<Scalar> const, Options> const unit_quaternion_;
};
} // namespace Eigen
#endif

129
Workspace/Thirdparty/Sophus/sophus/test_macros.hpp vendored Normal file
View File

@ -0,0 +1,129 @@
#ifndef SOPUHS_TESTS_MACROS_HPP
#define SOPUHS_TESTS_MACROS_HPP
#include <sophus/types.hpp>
#include <sophus/formatstring.hpp>
namespace Sophus {
namespace details {
template <class Scalar>
class Pretty {
public:
static std::string impl(Scalar s) { return FormatString("%", s); }
};
template <class Scalar, int M, int N>
class Pretty<Eigen::Matrix<Scalar, M, N>> {
public:
static std::string impl(Matrix<Scalar, M, N> const& v) {
return FormatString("\n%\n", v);
}
};
template <class T>
std::string pretty(T const& v) {
return Pretty<T>::impl(v);
}
template <class... Args>
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

241
Workspace/Thirdparty/Sophus/sophus/types.hpp vendored Normal file
View File

@ -0,0 +1,241 @@
/// @file
/// Common type aliases.
#ifndef SOPHUS_TYPES_HPP
#define SOPHUS_TYPES_HPP
#include <type_traits>
#include "common.hpp"
namespace Sophus {
template <class Scalar, int M, int Options = 0>
using Vector = Eigen::Matrix<Scalar, M, 1, Options>;
template <class Scalar, int Options = 0>
using Vector2 = Vector<Scalar, 2, Options>;
using Vector2f = Vector2<float>;
using Vector2d = Vector2<double>;
template <class Scalar, int Options = 0>
using Vector3 = Vector<Scalar, 3, Options>;
using Vector3f = Vector3<float>;
using Vector3d = Vector3<double>;
template <class Scalar>
using Vector4 = Vector<Scalar, 4>;
using Vector4f = Vector4<float>;
using Vector4d = Vector4<double>;
template <class Scalar>
using Vector6 = Vector<Scalar, 6>;
using Vector6f = Vector6<float>;
using Vector6d = Vector6<double>;
template <class Scalar>
using Vector7 = Vector<Scalar, 7>;
using Vector7f = Vector7<float>;
using Vector7d = Vector7<double>;
template <class Scalar, int M, int N>
using Matrix = Eigen::Matrix<Scalar, M, N>;
template <class Scalar>
using Matrix2 = Matrix<Scalar, 2, 2>;
using Matrix2f = Matrix2<float>;
using Matrix2d = Matrix2<double>;
template <class Scalar>
using Matrix3 = Matrix<Scalar, 3, 3>;
using Matrix3f = Matrix3<float>;
using Matrix3d = Matrix3<double>;
template <class Scalar>
using Matrix4 = Matrix<Scalar, 4, 4>;
using Matrix4f = Matrix4<float>;
using Matrix4d = Matrix4<double>;
template <class Scalar>
using Matrix6 = Matrix<Scalar, 6, 6>;
using Matrix6f = Matrix6<float>;
using Matrix6d = Matrix6<double>;
template <class Scalar>
using Matrix7 = Matrix<Scalar, 7, 7>;
using Matrix7f = Matrix7<float>;
using Matrix7d = Matrix7<double>;
template <class Scalar, int N, int Options = 0>
using ParametrizedLine = Eigen::ParametrizedLine<Scalar, N, Options>;
template <class Scalar, int Options = 0>
using ParametrizedLine3 = ParametrizedLine<Scalar, 3, Options>;
using ParametrizedLine3f = ParametrizedLine3<float>;
using ParametrizedLine3d = ParametrizedLine3<double>;
template <class Scalar, int Options = 0>
using ParametrizedLine2 = ParametrizedLine<Scalar, 2, Options>;
using ParametrizedLine2f = ParametrizedLine2<float>;
using ParametrizedLine2d = ParametrizedLine2<double>;
namespace details {
template <class Scalar>
class MaxMetric {
public:
static Scalar impl(Scalar s0, Scalar s1) {
using std::abs;
return abs(s0 - s1);
}
};
template <class Scalar, int M, int N>
class MaxMetric<Matrix<Scalar, M, N>> {
public:
static Scalar impl(Matrix<Scalar, M, N> const& p0,
Matrix<Scalar, M, N> const& p1) {
return (p0 - p1).template lpNorm<Eigen::Infinity>();
}
};
template <class Scalar>
class SetToZero {
public:
static void impl(Scalar& s) { s = Scalar(0); }
};
template <class Scalar, int M, int N>
class SetToZero<Matrix<Scalar, M, N>> {
public:
static void impl(Matrix<Scalar, M, N>& v) { v.setZero(); }
};
template <class T1, class Scalar>
class SetElementAt;
template <class Scalar>
class SetElementAt<Scalar, Scalar> {
public:
static void impl(Scalar& s, Scalar value, int at) {
SOPHUS_ENSURE(at == 0, "is %", at);
s = value;
}
};
template <class Scalar, int N>
class SetElementAt<Vector<Scalar, N>, Scalar> {
public:
static void impl(Vector<Scalar, N>& v, Scalar value, int at) {
SOPHUS_ENSURE(at >= 0 && at < N, "is %", at);
v[at] = value;
}
};
template <class Scalar>
class SquaredNorm {
public:
static Scalar impl(Scalar const& s) { return s * s; }
};
template <class Scalar, int N>
class SquaredNorm<Matrix<Scalar, N, 1>> {
public:
static Scalar impl(Matrix<Scalar, N, 1> const& s) { return s.squaredNorm(); }
};
template <class Scalar>
class Transpose {
public:
static Scalar impl(Scalar const& s) { return s; }
};
template <class Scalar, int M, int N>
class Transpose<Matrix<Scalar, M, N>> {
public:
static Matrix<Scalar, M, N> impl(Matrix<Scalar, M, N> 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 <class T>
auto maxMetric(T const& p0, T const& p1)
-> decltype(details::MaxMetric<T>::impl(p0, p1)) {
return details::MaxMetric<T>::impl(p0, p1);
}
/// Sets point ``p`` to zero, with ``p`` being a matrix or a scalar.
///
template <class T>
void setToZero(T& p) {
return details::SetToZero<T>::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 <class T, class Scalar>
void setElementAt(T& p, Scalar value, int i) {
return details::SetElementAt<T, Scalar>::impl(p, value, i);
}
/// Returns the squared 2-norm of ``p``, with ``p`` being a vector or a scalar.
///
template <class T>
auto squaredNorm(T const& p) -> decltype(details::SquaredNorm<T>::impl(p)) {
return details::SquaredNorm<T>::impl(p);
}
/// Returns ``p.transpose()`` if ``p`` is a matrix, and simply ``p`` if m is a
/// scalar.
///
template <class T>
auto transpose(T const& p) -> decltype(details::Transpose<T>::impl(T())) {
return details::Transpose<T>::impl(p);
}
template <class Scalar>
struct IsFloatingPoint {
static bool const value = std::is_floating_point<Scalar>::value;
};
template <class Scalar, int M, int N>
struct IsFloatingPoint<Matrix<Scalar, M, N>> {
static bool const value = std::is_floating_point<Scalar>::value;
};
template <class Scalar_>
struct GetScalar {
using Scalar = Scalar_;
};
template <class Scalar_, int M, int N>
struct GetScalar<Matrix<Scalar_, M, N>> {
using Scalar = Scalar_;
};
/// If the Vector type is of fixed size, then IsFixedSizeVector::value will be
/// true.
template <typename Vector, int NumDimensions,
typename = typename std::enable_if<
Vector::RowsAtCompileTime == NumDimensions &&
Vector::ColsAtCompileTime == 1>::type>
struct IsFixedSizeVector : std::true_type {};
/// Planes in 3d are hyperplanes.
template <class T>
using Plane3 = Eigen::Hyperplane<T, 3>;
using Plane3d = Plane3<double>;
using Plane3f = Plane3<float>;
/// Lines in 2d are hyperplanes.
template <class T>
using Line2 = Eigen::Hyperplane<T, 2>;
using Line2d = Line2<double>;
using Line2f = Line2<float>;
} // namespace Sophus
#endif // SOPHUS_TYPES_HPP

74
Workspace/Thirdparty/Sophus/sophus/velocities.hpp vendored Normal file
View File

@ -0,0 +1,74 @@
#ifndef SOPHUS_VELOCITIES_HPP
#define SOPHUS_VELOCITIES_HPP
#include <functional>
#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 <class Scalar>
Vector3<Scalar> transformVelocity(SO3<Scalar> const& foo_R_bar,
Vector3<Scalar> 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 <class Scalar>
Vector3<Scalar> transformVelocity(SE3<Scalar> const& foo_T_bar,
Vector3<Scalar> const& vel_bar) {
return transformVelocity(foo_T_bar.so3(), vel_bar);
}
// finite difference approximation of instantanious velocity in frame foo
//
template <class Scalar>
Vector3<Scalar> finiteDifferenceRotationalVelocity(
std::function<SO3<Scalar>(Scalar)> const& foo_R_bar, Scalar t,
Scalar h = Constants<Scalar>::epsilon()) {
// https://en.wikipedia.org/w/index.php?title=Angular_velocity&oldid=791867792#Angular_velocity_tensor
//
// W = dR(t)/dt * R^{-1}(t)
Matrix3<Scalar> dR_dt_in_frame_foo = curveNumDiff(
[&foo_R_bar](Scalar t0) -> Matrix3<Scalar> {
return foo_R_bar(t0).matrix();
},
t, h);
// velocity tensor
Matrix3<Scalar> W_in_frame_foo =
dR_dt_in_frame_foo * (foo_R_bar(t)).inverse().matrix();
return SO3<Scalar>::vee(W_in_frame_foo);
}
// finite difference approximation of instantanious velocity in frame foo
//
template <class Scalar>
Vector3<Scalar> finiteDifferenceRotationalVelocity(
std::function<SE3<Scalar>(Scalar)> const& foo_T_bar, Scalar t,
Scalar h = Constants<Scalar>::epsilon()) {
return finiteDifferenceRotationalVelocity<Scalar>(
[&foo_T_bar](Scalar t) -> SO3<Scalar> { return foo_T_bar(t).so3(); }, t,
h);
}
} // namespace experimental
} // namespace Sophus
#endif // SOPHUS_VELOCITIES_HPP

2
Workspace/Thirdparty/Sophus/test/CMakeLists.txt vendored Normal file
View File

@ -0,0 +1,2 @@
ADD_SUBDIRECTORY(core)
ADD_SUBDIRECTORY(ceres)

20
Workspace/Thirdparty/Sophus/test/ceres/CMakeLists.txt vendored Normal file
View File

@ -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 )

47
Workspace/Thirdparty/Sophus/test/ceres/local_parameterization_se3.hpp vendored Normal file
View File

@ -0,0 +1,47 @@
#ifndef SOPHUS_TEST_LOCAL_PARAMETERIZATION_SE3_HPP
#define SOPHUS_TEST_LOCAL_PARAMETERIZATION_SE3_HPP
#include <ceres/local_parameterization.h>
#include <sophus/se3.hpp>
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<SE3d const> const T(T_raw);
Eigen::Map<Vector6d const> const delta(delta_raw);
Eigen::Map<SE3d> 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<SE3d const> T(T_raw);
Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> 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

192
Workspace/Thirdparty/Sophus/test/ceres/test_ceres_se3.cpp vendored Normal file
View File

@ -0,0 +1,192 @@
#include <ceres/ceres.h>
#include <iostream>
#include <sophus/se3.hpp>
#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<T,N>) 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 <class T, int N, typename NewType>
struct cast_impl<ceres::Jet<T, N>, NewType> {
EIGEN_DEVICE_FUNC
static inline NewType run(ceres::Jet<T, N> const& x) {
return static_cast<NewType>(x.a);
}
};
} // namespace internal
} // namespace Eigen
struct TestSE3CostFunctor {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
TestSE3CostFunctor(Sophus::SE3d T_aw) : T_aw(T_aw) {}
template <class T>
bool operator()(T const* const sT_wa, T* sResiduals) const {
Eigen::Map<Sophus::SE3<T> const> const T_wa(sT_wa);
Eigen::Map<Eigen::Matrix<T, 6, 1> > 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<T>()).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 <class T>
bool operator()(T const* const sT_wa, T const* const spoint_b,
T* sResiduals) const {
using Vector3T = Eigen::Matrix<T, 3, 1>;
Eigen::Map<Sophus::SE3<T> const> const T_wa(sT_wa);
Eigen::Map<Vector3T const> point_b(spoint_b);
Eigen::Map<Vector3T> 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<T>() * 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<T>();
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<TestSE3CostFunctor, Sophus::SE3d::DoF,
Sophus::SE3d::num_parameters>(
new TestSE3CostFunctor(T_w_targ.inverse()));
problem.AddResidualBlock(cost_function1, NULL, T_wr.data());
ceres::CostFunction* cost_function2 =
new ceres::AutoDiffCostFunction<TestPointCostFunctor, kNumPointParameters,
Sophus::SE3d::num_parameters,
kNumPointParameters>(
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<double>::epsilon();
options.function_tolerance = 0.01 * Sophus::Constants<double>::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<double>::epsilon();
return passed;
}
template <typename Scalar>
bool CreateSE3FromMatrix(const Eigen::Matrix<Scalar, 4, 4>& mat) {
Sophus::SE3<Scalar> se3 = Sophus::SE3<Scalar>(mat);
std::cout << se3.translation().x() << std::endl;
return true;
}
int main(int, char**) {
using SE3Type = Sophus::SE3<double>;
using SO3Type = Sophus::SO3<double>;
using Point = SE3Type::Point;
double const kPi = Sophus::Constants<double>::pi();
std::vector<SE3Type> 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> 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<ceres::Jet<double, 28>, 4, 4> mat;
mat.setIdentity();
std::cout << CreateSE3FromMatrix(mat) << std::endl;
return 0;
}

15
Workspace/Thirdparty/Sophus/test/core/CMakeLists.txt vendored Normal file
View File

@ -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)

61
Workspace/Thirdparty/Sophus/test/core/test_common.cpp vendored Normal file
View File

@ -0,0 +1,61 @@
#include <iostream>
#include <sophus/test_macros.hpp>
#include <sophus/formatstring.hpp>
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<float, 1, 2> row = transpose(Vector2f(1, 7));
Matrix<float, 1, 2> expected_row(1, 7);
SOPHUS_TEST_EQUAL(passed, row, expected_row);
optional<int> 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(); }

130
Workspace/Thirdparty/Sophus/test/core/test_geometry.cpp vendored Normal file
View File

@ -0,0 +1,130 @@
#include <iostream>
#include <sophus/geometry.hpp>
#include <sophus/test_macros.hpp>
#include "tests.hpp"
namespace Sophus {
namespace {
template <class T>
bool test2dGeometry() {
bool passed = true;
T const eps = Constants<T>::epsilon();
for (int i = 0; i < 20; ++i) {
// Roundtrip test:
Vector2<T> normal_foo = Vector2<T>::Random().normalized();
Sophus::SO2<T> R_foo_plane = SO2FromNormal(normal_foo);
Vector2<T> 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<T> line_foo = makeHyperplaneUnique(
Line2<T>(Vector2<T>::Random().normalized(), Vector2<T>::Random()));
Sophus::SE2<T> T_foo_plane = SE2FromLine(line_foo);
Line2<T> 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<SE2<T>, Eigen::aligned_allocator<SE2<T>>> Ts_foo_line =
getTestSE2s<T>();
for (SE2<T> const& T_foo_line : Ts_foo_line) {
Line2<T> line_foo = lineFromSE2(T_foo_line);
SE2<T> T2_foo_line = SE2FromLine(line_foo);
Line2<T> 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 <class T>
bool test3dGeometry() {
bool passed = true;
T const eps = Constants<T>::epsilon();
Vector3<T> normal_foo = Vector3<T>(1, 2, 0).normalized();
Matrix3<T> 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<T> R2_foo_plane = rotationFromNormal((T(0.9) * normal_foo).eval());
SOPHUS_TEST_APPROX(passed, normal_foo, R2_foo_plane.col(2).eval(), eps);
Matrix3<T> R3_foo_plane =
rotationFromNormal(normal_foo, Vector3<T>(T(0.9), T(0), T(0)),
Vector3<T>(T(0), T(1.1), T(0)));
SOPHUS_TEST_APPROX(passed, normal_foo, R3_foo_plane.col(2).eval(), eps);
normal_foo = Vector3<T>(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<T>(0, 1, 0), R_foo_plane.col(1).eval(),
eps);
normal_foo = Vector3<T>(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<T>(1, 0, 0), R_foo_plane.col(0).eval(),
eps);
for (int i = 0; i < 20; ++i) {
// Roundtrip test:
Vector3<T> normal_foo = Vector3<T>::Random().normalized();
Sophus::SO3<T> R_foo_plane = SO3FromNormal(normal_foo);
Vector3<T> 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<T> plane_foo = makeHyperplaneUnique(
Plane3<T>(Vector3<T>::Random().normalized(), Vector3<T>::Random()));
Sophus::SE3<T> T_foo_plane = SE3FromPlane(plane_foo);
Plane3<T> 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<SE3<T>, Eigen::aligned_allocator<SE3<T>>> Ts_foo_plane =
getTestSE3s<T>();
for (SE3<T> const& T_foo_plane : Ts_foo_plane) {
Plane3<T> plane_foo = planeFromSE3(T_foo_plane);
SE3<T> T2_foo_plane = SE3FromPlane(plane_foo);
Plane3<T> 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<double>();
passed = test3dGeometry<double>();
processTestResult(passed);
std::cerr << "Float tests: " << std::endl;
passed = test2dGeometry<float>();
passed = test3dGeometry<float>();
processTestResult(passed);
}
} // namespace
} // namespace Sophus
int main() { Sophus::runAll(); }

261
Workspace/Thirdparty/Sophus/test/core/test_rxso2.cpp vendored Normal file
View File

@ -0,0 +1,261 @@
#include <iostream>
#include <sophus/rxso2.hpp>
#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<Sophus::RxSO2<double>>;
template class Map<Sophus::RxSO2<double> const>;
} // namespace Eigen
namespace Sophus {
template class RxSO2<double, Eigen::AutoAlign>;
template class RxSO2<float, Eigen::DontAlign>;
#if SOPHUS_CERES
template class RxSO2<ceres::Jet<double, 3>>;
#endif
template <class Scalar>
class Tests {
public:
using SO2Type = SO2<Scalar>;
using RxSO2Type = RxSO2<Scalar>;
using RotationMatrixType = typename SO2<Scalar>::Transformation;
using Point = typename RxSO2<Scalar>::Point;
using Tangent = typename RxSO2<Scalar>::Tangent;
Scalar const kPi = Constants<Scalar>::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 <class S = Scalar>
enable_if_t<std::is_floating_point<S>::value, bool> testFit() {
bool passed = true;
for (int i = 0; i < 10; ++i) {
Matrix2<Scalar> M = Matrix2<Scalar>::Random();
for (Scalar scale : {Scalar(0.01), Scalar(0.99), Scalar(1), Scalar(10)}) {
Matrix2<Scalar> R = makeRotationMatrix(M);
Matrix2<Scalar> sR = scale * R;
SOPHUS_TEST(passed, isScaledOrthogonalAndPositive(sR),
"isScaledOrthogonalAndPositive(sR): % *\n%", scale, R);
Matrix2<Scalar> 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 <class S = Scalar>
enable_if_t<!std::is_floating_point<S>::value, bool> testFit() {
return true;
}
void runAll() {
bool passed = testLieProperties();
passed &= testSaturation();
passed &= testRawDataAcces();
passed &= testConstructors();
passed &= testFit();
processTestResult(passed);
}
private:
bool testLieProperties() {
LieGroupTests<RxSO2Type> tests(rxso2_vec_, tangent_vec_, point_vec_);
return tests.doAllTestsPass();
}
bool testSaturation() {
bool passed = true;
RxSO2Type small1(Scalar(1.1) * Constants<Scalar>::epsilon(), SO2Type());
RxSO2Type small2(Scalar(1.1) * Constants<Scalar>::epsilon(),
SO2Type::exp(Constants<Scalar>::pi()));
RxSO2Type saturated_product = small1 * small2;
SOPHUS_TEST_APPROX(passed, saturated_product.scale(),
Constants<Scalar>::epsilon(),
Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, saturated_product.so2().matrix(),
(small1.so2() * small2.so2()).matrix(),
Constants<Scalar>::epsilon());
return passed;
}
bool testRawDataAcces() {
bool passed = true;
Eigen::Matrix<Scalar, 2, 1> raw = {0, 1};
Eigen::Map<RxSO2Type const> map_of_const_rxso2(raw.data());
SOPHUS_TEST_APPROX(passed, map_of_const_rxso2.complex().eval(), raw,
Constants<Scalar>::epsilon());
SOPHUS_TEST_EQUAL(passed, map_of_const_rxso2.complex().data(), raw.data());
Eigen::Map<RxSO2Type const> const_shallow_copy = map_of_const_rxso2;
SOPHUS_TEST_EQUAL(passed, const_shallow_copy.complex().eval(),
map_of_const_rxso2.complex().eval());
Eigen::Matrix<Scalar, 2, 1> raw2 = {1, 0};
Eigen::Map<RxSO2Type> map_of_rxso2(raw2.data());
SOPHUS_TEST_APPROX(passed, map_of_rxso2.complex().eval(), raw2,
Constants<Scalar>::epsilon());
SOPHUS_TEST_EQUAL(passed, map_of_rxso2.complex().data(), raw2.data());
Eigen::Map<RxSO2Type> 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<Scalar, 2, 1> raw3 = {Scalar(2), Scalar(0)};
Eigen::Map<RxSO2Type> map_of_rxso2_3(raw3.data());
Eigen::Map<const RxSO2Type> 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<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, map_of_rxso2_3.complex().eval(),
rxso2_copy3.complex().eval(),
Constants<Scalar>::epsilon());
const RotationMatrixType r_const = const_map_of_rxso2_3.rotationMatrix();
SOPHUS_TEST_APPROX(passed, r_ref, r_const, Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, const_map_of_rxso2_3.complex().eval(),
rxso2_copy3.complex().eval(),
Constants<Scalar>::epsilon());
Eigen::Matrix<Scalar, 2, 1> data1, data2;
data1 << Scalar(.1), Scalar(.2);
data2 << Scalar(.5), Scalar(.4);
Eigen::Map<RxSO2Type> 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<Scalar>::epsilon(), "setScale");
Scalar angle(0.2);
rxso2.setAngle(angle);
SOPHUS_TEST_APPROX(passed, angle, rxso2.angle(),
Constants<Scalar>::epsilon(), "setAngle");
SOPHUS_TEST_APPROX(passed, scale, rxso2.scale(),
Constants<Scalar>::epsilon(),
"setAngle leaves scale as is");
auto so2 = rxso2_vec_[0].so2();
rxso2.setSO2(so2);
SOPHUS_TEST_APPROX(passed, scale, rxso2.scale(),
Constants<Scalar>::epsilon(), "setSO2");
SOPHUS_TEST_APPROX(passed, RxSO2Type(scale, so2).matrix(), rxso2.matrix(),
Constants<Scalar>::epsilon(), "RxSO2(scale, SO2)");
SOPHUS_TEST_APPROX(passed, RxSO2Type(scale, so2.matrix()).matrix(),
rxso2.matrix(), Constants<Scalar>::epsilon(),
"RxSO2(scale, SO2)");
Matrix2<Scalar> R = SO2<Scalar>::exp(Scalar(0.2)).matrix();
Matrix2<Scalar> sR = R * Scalar(1.3);
SOPHUS_TEST_APPROX(passed, RxSO2Type(sR).matrix(), sR,
Constants<Scalar>::epsilon(), "RxSO2(sR)");
rxso2.setScaledRotationMatrix(sR);
SOPHUS_TEST_APPROX(passed, sR, rxso2.matrix(), Constants<Scalar>::epsilon(),
"setScaleRotationMatrix");
rxso2.setScale(scale);
rxso2.setRotationMatrix(R);
SOPHUS_TEST_APPROX(passed, R, rxso2.rotationMatrix(),
Constants<Scalar>::epsilon(), "setRotationMatrix");
SOPHUS_TEST_APPROX(passed, scale, rxso2.scale(),
Constants<Scalar>::epsilon(), "setScale");
return passed;
}
std::vector<RxSO2Type, Eigen::aligned_allocator<RxSO2Type>> rxso2_vec_;
std::vector<Tangent, Eigen::aligned_allocator<Tangent>> tangent_vec_;
std::vector<Point, Eigen::aligned_allocator<Point>> point_vec_;
};
int test_rxso2() {
using std::cerr;
using std::endl;
cerr << "Test RxSO2" << endl << endl;
cerr << "Double tests: " << endl;
Tests<double>().runAll();
cerr << "Float tests: " << endl;
Tests<float>().runAll();
#if SOPHUS_CERES
cerr << "ceres::Jet<double, 3> tests: " << endl;
Tests<ceres::Jet<double, 3>>().runAll();
#endif
return 0;
}
} // namespace Sophus
int main() { return Sophus::test_rxso2(); }

283
Workspace/Thirdparty/Sophus/test/core/test_rxso3.cpp vendored Normal file
View File

@ -0,0 +1,283 @@
#include <iostream>
#include <sophus/rxso3.hpp>
#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<Sophus::RxSO3<double>>;
template class Map<Sophus::RxSO3<double> const>;
} // namespace Eigen
namespace Sophus {
template class RxSO3<double, Eigen::AutoAlign>;
template class RxSO3<float, Eigen::DontAlign>;
#if SOPHUS_CERES
template class RxSO3<ceres::Jet<double, 3>>;
#endif
template <class Scalar_>
class Tests {
public:
using Scalar = Scalar_;
using SO3Type = SO3<Scalar>;
using RxSO3Type = RxSO3<Scalar>;
using RotationMatrixType = typename SO3<Scalar>::Transformation;
using Point = typename RxSO3<Scalar>::Point;
using Tangent = typename RxSO3<Scalar>::Tangent;
Scalar const kPi = Constants<Scalar>::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<RxSO3Type> tests(rxso3_vec_, tangent_vec_, point_vec_);
return tests.doAllTestsPass();
}
bool testSaturation() {
bool passed = true;
RxSO3Type small1(Constants<Scalar>::epsilon(), SO3Type());
RxSO3Type small2(Constants<Scalar>::epsilon(),
SO3Type::exp(Vector3<Scalar>(Constants<Scalar>::pi(),
Scalar(0), Scalar(0))));
RxSO3Type saturated_product = small1 * small2;
SOPHUS_TEST_APPROX(passed, saturated_product.scale(),
Constants<Scalar>::epsilon(),
Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, saturated_product.so3().matrix(),
(small1.so3() * small2.so3()).matrix(),
Constants<Scalar>::epsilon());
return passed;
}
bool testRawDataAcces() {
bool passed = true;
Eigen::Matrix<Scalar, 4, 1> raw = {Scalar(0), Scalar(1), Scalar(0),
Scalar(0)};
Eigen::Map<RxSO3Type const> map_of_const_rxso3(raw.data());
SOPHUS_TEST_APPROX(passed, map_of_const_rxso3.quaternion().coeffs().eval(),
raw, Constants<Scalar>::epsilon());
SOPHUS_TEST_EQUAL(passed, map_of_const_rxso3.quaternion().coeffs().data(),
raw.data());
Eigen::Map<RxSO3Type const> 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<Scalar, 4, 1> raw2 = {Scalar(1), Scalar(0), Scalar(0),
Scalar(0)};
Eigen::Map<RxSO3Type> map_of_rxso3(raw.data());
Eigen::Quaternion<Scalar> quat;
quat.coeffs() = raw2;
map_of_rxso3.setQuaternion(quat);
SOPHUS_TEST_APPROX(passed, map_of_rxso3.quaternion().coeffs().eval(), raw2,
Constants<Scalar>::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<RxSO3Type> 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<Scalar, 4, 1> raw3 = {Scalar(2), Scalar(0), Scalar(0),
Scalar(0)};
Eigen::Map<RxSO3Type> map_of_rxso3_3(raw3.data());
Eigen::Map<const RxSO3Type> 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<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, map_of_rxso3_3.quaternion().coeffs().eval(),
rxso3_copy3.quaternion().coeffs().eval(),
Constants<Scalar>::epsilon());
const RotationMatrixType r_const = const_map_of_rxso3_3.rotationMatrix();
SOPHUS_TEST_APPROX(passed, r_ref, r_const, Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(
passed, const_map_of_rxso3_3.quaternion().coeffs().eval(),
rxso3_copy3.quaternion().coeffs().eval(), Constants<Scalar>::epsilon());
Eigen::Matrix<Scalar, 4, 1> data1, data2;
data1 << Scalar(.1), Scalar(.2), Scalar(.3), Scalar(.4);
data2 << Scalar(.5), Scalar(.4), Scalar(.3), Scalar(.2);
Eigen::Map<RxSO3Type> 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<Scalar>::epsilon(), "setScale");
auto so3 = rxso3_vec_[0].so3();
rxso3.setSO3(so3);
SOPHUS_TEST_APPROX(passed, scale, rxso3.scale(),
Constants<Scalar>::epsilon(), "setScale");
SOPHUS_TEST_APPROX(passed, RxSO3Type(scale, so3).matrix(), rxso3.matrix(),
Constants<Scalar>::epsilon(), "RxSO3(scale, SO3)");
SOPHUS_TEST_APPROX(passed, RxSO3Type(scale, so3.matrix()).matrix(),
rxso3.matrix(), Constants<Scalar>::epsilon(),
"RxSO3(scale, SO3)");
Matrix3<Scalar> R =
SO3<Scalar>::exp(Point(Scalar(0.2), Scalar(0.5), Scalar(-1.0)))
.matrix();
Matrix3<Scalar> sR = R * Scalar(1.3);
SOPHUS_TEST_APPROX(passed, RxSO3Type(sR).matrix(), sR,
Constants<Scalar>::epsilon(), "RxSO3(sR)");
rxso3.setScaledRotationMatrix(sR);
SOPHUS_TEST_APPROX(passed, sR, rxso3.matrix(), Constants<Scalar>::epsilon(),
"setScaleRotationMatrix");
rxso3.setScale(scale);
rxso3.setRotationMatrix(R);
SOPHUS_TEST_APPROX(passed, R, rxso3.rotationMatrix(),
Constants<Scalar>::epsilon(), "setRotationMatrix");
SOPHUS_TEST_APPROX(passed, scale, rxso3.scale(),
Constants<Scalar>::epsilon(), "setScale");
return passed;
}
template <class S = Scalar>
enable_if_t<std::is_floating_point<S>::value, bool> testFit() {
bool passed = true;
for (int i = 0; i < 10; ++i) {
Matrix3<Scalar> M = Matrix3<Scalar>::Random();
for (Scalar scale : {Scalar(0.01), Scalar(0.99), Scalar(1), Scalar(10)}) {
Matrix3<Scalar> R = makeRotationMatrix(M);
Matrix3<Scalar> sR = scale * R;
SOPHUS_TEST(passed, isScaledOrthogonalAndPositive(sR),
"isScaledOrthogonalAndPositive(sR): % *\n%", scale, R);
Matrix3<Scalar> 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 <class S = Scalar>
enable_if_t<!std::is_floating_point<S>::value, bool> testFit() {
return true;
}
std::vector<RxSO3Type, Eigen::aligned_allocator<RxSO3Type>> rxso3_vec_;
std::vector<Tangent, Eigen::aligned_allocator<Tangent>> tangent_vec_;
std::vector<Point, Eigen::aligned_allocator<Point>> point_vec_;
};
int test_rxso3() {
using std::cerr;
using std::endl;
cerr << "Test RxSO3" << endl << endl;
cerr << "Double tests: " << endl;
Tests<double>().runAll();
cerr << "Float tests: " << endl;
Tests<float>().runAll();
#if SOPHUS_CERES
cerr << "ceres::Jet<double, 3> tests: " << endl;
Tests<ceres::Jet<double, 3>>().runAll();
#endif
return 0;
}
} // namespace Sophus
int main() { return Sophus::test_rxso3(); }

260
Workspace/Thirdparty/Sophus/test/core/test_se2.cpp vendored Normal file
View File

@ -0,0 +1,260 @@
#include <iostream>
#include <sophus/se2.hpp>
#include <unsupported/Eigen/MatrixFunctions>
#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<Sophus::SE2<double>>;
template class Map<Sophus::SE2<double> const>;
} // namespace Eigen
namespace Sophus {
template class SE2<double, Eigen::AutoAlign>;
template class SE2<double, Eigen::DontAlign>;
#if SOPHUS_CERES
template class SE2<ceres::Jet<double, 3>>;
#endif
template <class Scalar>
class Tests {
public:
using SE2Type = SE2<Scalar>;
using SO2Type = SO2<Scalar>;
using Point = typename SE2<Scalar>::Point;
using Tangent = typename SE2<Scalar>::Tangent;
Scalar const kPi = Constants<Scalar>::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<SE2Type> tests(se2_vec_, tangent_vec_, point_vec_);
return tests.doAllTestsPass();
}
bool testRawDataAcces() {
bool passed = true;
Eigen::Matrix<Scalar, 4, 1> raw;
raw << Scalar(0), Scalar(1), Scalar(0), Scalar(3);
Eigen::Map<SE2Type const> const_se2_map(raw.data());
SOPHUS_TEST_APPROX(passed, const_se2_map.unit_complex().eval(),
raw.template head<2>().eval(),
Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, const_se2_map.translation().eval(),
raw.template tail<2>().eval(),
Constants<Scalar>::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<SE2Type const> 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<Scalar, 4, 1> raw2;
raw2 << Scalar(1), Scalar(0), Scalar(3), Scalar(1);
Eigen::Map<SE2Type> 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<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, map_of_se3.translation().eval(),
raw2.template tail<2>().eval(),
Constants<Scalar>::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<SE2Type> 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<SE2Type> 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<Scalar>::epsilon());
trans = SE2Type::transY(Scalar(0.7));
SOPHUS_TEST_APPROX(passed, trans.translation().y(), Scalar(0.7),
Constants<Scalar>::epsilon());
Eigen::Matrix<Scalar, 4, 1> data1, data2;
data1 << Scalar(0), Scalar(1), Scalar(1), Scalar(2);
data1 << Scalar(1), Scalar(0), Scalar(2), Scalar(1);
Eigen::Map<SE2Type> 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<Scalar>::epsilon());
Eigen::Matrix<Scalar, 4, 1> raw;
raw << Scalar(1), Scalar(0), Scalar(3), Scalar(1);
Eigen::Map<SE2Type> map_of_se2(raw.data());
map_of_se2.setRotationMatrix(R.matrix());
SOPHUS_TEST_APPROX(passed, map_of_se2.rotationMatrix(), R.matrix(),
Constants<Scalar>::epsilon());
return passed;
}
bool testConstructors() {
bool passed = true;
Matrix3<Scalar> I = Matrix3<Scalar>::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<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, SE2Type(so2, translation).matrix(), se2.matrix(),
Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, SE2Type(so2.matrix(), translation).matrix(),
se2.matrix(), Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed,
SE2Type(so2.unit_complex(), translation).matrix(),
se2.matrix(), Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, SE2Type(se2.matrix()).matrix(), se2.matrix(),
Constants<Scalar>::epsilon());
return passed;
}
template <class S = Scalar>
enable_if_t<std::is_floating_point<S>::value, bool> testFit() {
bool passed = true;
for (int i = 0; i < 100; ++i) {
Matrix3<Scalar> T = Matrix3<Scalar>::Random();
SE2Type se2 = SE2Type::fitToSE2(T);
SE2Type se2_2 = SE2Type::fitToSE2(se2.matrix());
SOPHUS_TEST_APPROX(passed, se2.matrix(), se2_2.matrix(),
Constants<Scalar>::epsilon());
}
return passed;
}
template <class S = Scalar>
enable_if_t<!std::is_floating_point<S>::value, bool> testFit() {
return true;
}
std::vector<SE2Type, Eigen::aligned_allocator<SE2Type>> se2_vec_;
std::vector<Tangent, Eigen::aligned_allocator<Tangent>> tangent_vec_;
std::vector<Point, Eigen::aligned_allocator<Point>> point_vec_;
};
int test_se2() {
using std::cerr;
using std::endl;
cerr << "Test SE2" << endl << endl;
cerr << "Double tests: " << endl;
Tests<double>().runAll();
cerr << "Float tests: " << endl;
Tests<float>().runAll();
#if SOPHUS_CERES
cerr << "ceres::Jet<double, 3> tests: " << endl;
Tests<ceres::Jet<double, 3>>().runAll();
#endif
return 0;
}
} // namespace Sophus
int main() { return Sophus::test_se2(); }

267
Workspace/Thirdparty/Sophus/test/core/test_se3.cpp vendored Normal file
View File

@ -0,0 +1,267 @@
#include <iostream>
#include <sophus/se3.hpp>
#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<Sophus::SE3<double>>;
template class Map<Sophus::SE3<double> const>;
} // namespace Eigen
namespace Sophus {
template class SE3<double, Eigen::AutoAlign>;
template class SE3<float, Eigen::DontAlign>;
#if SOPHUS_CERES
template class SE3<ceres::Jet<double, 3>>;
#endif
template <class Scalar>
class Tests {
public:
using SE3Type = SE3<Scalar>;
using SO3Type = SO3<Scalar>;
using Point = typename SE3<Scalar>::Point;
using Tangent = typename SE3<Scalar>::Tangent;
Scalar const kPi = Constants<Scalar>::pi();
Tests() {
se3_vec_ = getTestSE3s<Scalar>();
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<SE3Type> tests(se3_vec_, tangent_vec_, point_vec_);
return tests.doAllTestsPass();
}
bool testRawDataAcces() {
bool passed = true;
Eigen::Matrix<Scalar, 7, 1> raw;
raw << Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1), Scalar(3),
Scalar(2);
Eigen::Map<SE3Type const> map_of_const_se3(raw.data());
SOPHUS_TEST_APPROX(
passed, map_of_const_se3.unit_quaternion().coeffs().eval(),
raw.template head<4>().eval(), Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, map_of_const_se3.translation().eval(),
raw.template tail<3>().eval(),
Constants<Scalar>::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<SE3Type const> 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<Scalar, 7, 1> raw2;
raw2 << Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(3), Scalar(2),
Scalar(1);
Eigen::Map<SE3Type> map_of_se3(raw.data());
Eigen::Quaternion<Scalar> 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<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, map_of_se3.translation().eval(),
raw2.template tail<3>().eval(),
Constants<Scalar>::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<SE3Type> 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<SE3Type> 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<Scalar>::epsilon());
trans = SE3Type::transY(Scalar(0.7));
SOPHUS_TEST_APPROX(passed, trans.translation().y(), Scalar(0.7),
Constants<Scalar>::epsilon());
trans = SE3Type::transZ(Scalar(-0.2));
SOPHUS_TEST_APPROX(passed, trans.translation().z(), Scalar(-0.2),
Constants<Scalar>::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<Scalar, 7, 1> 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<SE3Type> 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<Scalar>::epsilon());
return passed;
}
bool testConstructors() {
bool passed = true;
Eigen::Matrix<Scalar, 4, 4> I = Eigen::Matrix<Scalar, 4, 4>::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<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, SE3Type(so3.matrix(), translation).matrix(),
se3.matrix(), Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed,
SE3Type(so3.unit_quaternion(), translation).matrix(),
se3.matrix(), Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, SE3Type(se3.matrix()).matrix(), se3.matrix(),
Constants<Scalar>::epsilon());
return passed;
}
template <class S = Scalar>
enable_if_t<std::is_floating_point<S>::value, bool> testFit() {
bool passed = true;
for (int i = 0; i < 100; ++i) {
Matrix4<Scalar> T = Matrix4<Scalar>::Random();
SE3Type se3 = SE3Type::fitToSE3(T);
SE3Type se3_2 = SE3Type::fitToSE3(se3.matrix());
SOPHUS_TEST_APPROX(passed, se3.matrix(), se3_2.matrix(),
Constants<Scalar>::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<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, SE3Type::rotY(angle).angleY(), angle,
Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, SE3Type::rotZ(angle).angleZ(), angle,
Constants<Scalar>::epsilon());
}
return passed;
}
template <class S = Scalar>
enable_if_t<!std::is_floating_point<S>::value, bool> testFit() {
return true;
}
std::vector<SE3Type, Eigen::aligned_allocator<SE3Type>> se3_vec_;
std::vector<Tangent, Eigen::aligned_allocator<Tangent>> tangent_vec_;
std::vector<Point, Eigen::aligned_allocator<Point>> point_vec_;
};
int test_se3() {
using std::cerr;
using std::endl;
cerr << "Test SE3" << endl << endl;
cerr << "Double tests: " << endl;
Tests<double>().runAll();
cerr << "Float tests: " << endl;
Tests<float>().runAll();
#if SOPHUS_CERES
cerr << "ceres::Jet<double, 3> tests: " << endl;
Tests<ceres::Jet<double, 3>>().runAll();
#endif
return 0;
}
} // namespace Sophus
int main() { return Sophus::test_se3(); }

225
Workspace/Thirdparty/Sophus/test/core/test_sim2.cpp vendored Normal file
View File

@ -0,0 +1,225 @@
#include <iostream>
#include <unsupported/Eigen/MatrixFunctions>
#include <sophus/sim2.hpp>
#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<Sophus::Sim2<double>>;
template class Map<Sophus::Sim2<double> const>;
} // namespace Eigen
namespace Sophus {
template class Sim2<double, Eigen::AutoAlign>;
template class Sim2<float, Eigen::DontAlign>;
#if SOPHUS_CERES
template class Sim2<ceres::Jet<double, 3>>;
#endif
template <class Scalar>
class Tests {
public:
using Sim2Type = Sim2<Scalar>;
using RxSO2Type = RxSO2<Scalar>;
using Point = typename Sim2<Scalar>::Point;
using Vector2Type = Vector2<Scalar>;
using Tangent = typename Sim2<Scalar>::Tangent;
Scalar const kPi = Constants<Scalar>::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<Sim2Type> tests(sim2_vec_, tangent_vec_, point_vec_);
return tests.doAllTestsPass();
}
bool testRawDataAcces() {
bool passed = true;
Eigen::Matrix<Scalar, 4, 1> raw;
raw << Scalar(0), Scalar(1), Scalar(3), Scalar(2);
Eigen::Map<Sim2Type const> map_of_const_sim2(raw.data());
SOPHUS_TEST_APPROX(passed, map_of_const_sim2.complex().eval(),
raw.template head<2>().eval(),
Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, map_of_const_sim2.translation().eval(),
raw.template tail<2>().eval(),
Constants<Scalar>::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<Sim2Type const> 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<Scalar, 4, 1> raw2;
raw2 << Scalar(1), Scalar(0), Scalar(2), Scalar(1);
Eigen::Map<Sim2Type> map_of_sim2(raw.data());
Vector2<Scalar> 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<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, map_of_sim2.translation().eval(),
raw2.template tail<2>().eval(),
Constants<Scalar>::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<Sim2Type> 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<Sim2Type> 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<Scalar, 4, 1> data1, data2;
data1 << Scalar(0), Scalar(2), Scalar(1), Scalar(2);
data2 << Scalar(2), Scalar(0), Scalar(2), Scalar(1);
Eigen::Map<Sim2Type> 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<Scalar, 3, 3> I = Eigen::Matrix<Scalar, 3, 3>::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<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, Sim2Type(rxso2.complex(), translation).matrix(),
sim2.matrix(), Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, Sim2Type(sim2.matrix()).matrix(), sim2.matrix(),
Constants<Scalar>::epsilon());
Scalar scale(1.2);
sim2.setScale(scale);
SOPHUS_TEST_APPROX(passed, scale, sim2.scale(),
Constants<Scalar>::epsilon(), "setScale");
sim2.setComplex(sim2_vec_[0].rxso2().complex());
SOPHUS_TEST_APPROX(passed, sim2_vec_[0].rxso2().complex(),
sim2_vec_[0].rxso2().complex(),
Constants<Scalar>::epsilon(), "setComplex");
return passed;
}
std::vector<Sim2Type, Eigen::aligned_allocator<Sim2Type>> sim2_vec_;
std::vector<Tangent, Eigen::aligned_allocator<Tangent>> tangent_vec_;
std::vector<Point, Eigen::aligned_allocator<Point>> point_vec_;
};
int test_sim3() {
using std::cerr;
using std::endl;
cerr << "Test Sim2" << endl << endl;
cerr << "Double tests: " << endl;
Tests<double>().runAll();
cerr << "Float tests: " << endl;
Tests<float>().runAll();
#if SOPHUS_CERES
cerr << "ceres::Jet<double, 3> tests: " << endl;
Tests<ceres::Jet<double, 3>>().runAll();
#endif
return 0;
}
} // namespace Sophus
int main() { return Sophus::test_sim3(); }

271
Workspace/Thirdparty/Sophus/test/core/test_sim3.cpp vendored Normal file
View File

@ -0,0 +1,271 @@
#include <iostream>
#include <unsupported/Eigen/MatrixFunctions>
#include <sophus/sim3.hpp>
#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<Sophus::Sim3<double>>;
template class Map<Sophus::Sim3<double> const>;
} // namespace Eigen
namespace Sophus {
template class Sim3<double, Eigen::AutoAlign>;
template class Sim3<float, Eigen::DontAlign>;
#if SOPHUS_CERES
template class Sim3<ceres::Jet<double, 3>>;
#endif
template <class Scalar>
class Tests {
public:
using Sim3Type = Sim3<Scalar>;
using RxSO3Type = RxSO3<Scalar>;
using Point = typename Sim3<Scalar>::Point;
using Vector4Type = Vector4<Scalar>;
using Tangent = typename Sim3<Scalar>::Tangent;
Scalar const kPi = Constants<Scalar>::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<Sim3Type> tests(sim3_vec_, tangent_vec_, point_vec_);
return tests.doAllTestsPass();
}
bool testRawDataAcces() {
bool passed = true;
Eigen::Matrix<Scalar, 7, 1> raw;
raw << Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1), Scalar(3),
Scalar(2);
Eigen::Map<Sim3Type const> map_of_const_sim3(raw.data());
SOPHUS_TEST_APPROX(passed, map_of_const_sim3.quaternion().coeffs().eval(),
raw.template head<4>().eval(),
Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, map_of_const_sim3.translation().eval(),
raw.template tail<3>().eval(),
Constants<Scalar>::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<Sim3Type const> 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<Scalar, 7, 1> raw2;
raw2 << Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(3), Scalar(2),
Scalar(1);
Eigen::Map<Sim3Type> map_of_sim3(raw.data());
Eigen::Quaternion<Scalar> 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<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, map_of_sim3.translation().eval(),
raw2.template tail<3>().eval(),
Constants<Scalar>::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<Sim3Type> 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<Sim3Type> 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<Scalar, 7, 1> 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<Sim3Type> 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<Scalar, 4, 4> I = Eigen::Matrix<Scalar, 4, 4>::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<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed,
Sim3Type(rxso3.quaternion(), translation).matrix(),
sim3.matrix(), Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, Sim3Type(sim3.matrix()).matrix(), sim3.matrix(),
Constants<Scalar>::epsilon());
Scalar scale(1.2);
sim3.setScale(scale);
SOPHUS_TEST_APPROX(passed, scale, sim3.scale(),
Constants<Scalar>::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<Scalar>::epsilon(), "setQuaternion");
return passed;
}
std::vector<Sim3Type, Eigen::aligned_allocator<Sim3Type>> sim3_vec_;
std::vector<Tangent, Eigen::aligned_allocator<Tangent>> tangent_vec_;
std::vector<Point, Eigen::aligned_allocator<Point>> point_vec_;
};
int test_sim3() {
using std::cerr;
using std::endl;
cerr << "Test Sim3" << endl << endl;
cerr << "Double tests: " << endl;
Tests<double>().runAll();
cerr << "Float tests: " << endl;
Tests<float>().runAll();
#if SOPHUS_CERES
cerr << "ceres::Jet<double, 3> tests: " << endl;
Tests<ceres::Jet<double, 3>>().runAll();
#endif
return 0;
}
} // namespace Sophus
int main() { return Sophus::test_sim3(); }

190
Workspace/Thirdparty/Sophus/test/core/test_so2.cpp vendored Normal file
View File

@ -0,0 +1,190 @@
#include <iostream>
#include <sophus/so2.hpp>
#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<Sophus::SO2<double>>;
template class Map<Sophus::SO2<double> const>;
} // namespace Eigen
namespace Sophus {
template class SO2<double, Eigen::AutoAlign>;
template class SO2<float, Eigen::DontAlign>;
#if SOPHUS_CERES
template class SO2<ceres::Jet<double, 3>>;
#endif
template <class Scalar>
class Tests {
public:
using SO2Type = SO2<Scalar>;
using Point = typename SO2<Scalar>::Point;
using Tangent = typename SO2<Scalar>::Tangent;
Scalar const kPi = Constants<Scalar>::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<SO2Type> 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<Scalar>::epsilon(), "Magnitude drift");
return passed;
}
bool testRawDataAcces() {
bool passed = true;
Vector2<Scalar> raw = {0, 1};
Eigen::Map<SO2Type const> map_of_const_so2(raw.data());
SOPHUS_TEST_APPROX(passed, map_of_const_so2.unit_complex().eval(), raw,
Constants<Scalar>::epsilon());
SOPHUS_TEST_EQUAL(passed, map_of_const_so2.unit_complex().data(),
raw.data());
Eigen::Map<SO2Type const> 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<Scalar> raw2 = {1, 0};
Eigen::Map<SO2Type> map_of_so2(raw.data());
map_of_so2.setComplex(raw2);
SOPHUS_TEST_APPROX(passed, map_of_so2.unit_complex().eval(), raw2,
Constants<Scalar>::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<SO2Type> 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<Scalar> data1 = {1, 0}, data2 = {0, 1};
Eigen::Map<SO2Type> 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<Scalar> R = so2_vec_.front().matrix();
SO2Type so2(R);
SOPHUS_TEST_APPROX(passed, R, so2.matrix(), Constants<Scalar>::epsilon());
return passed;
}
template <class S = Scalar>
enable_if_t<std::is_floating_point<S>::value, bool> testFit() {
bool passed = true;
for (int i = 0; i < 100; ++i) {
Matrix2<Scalar> R = Matrix2<Scalar>::Random();
SO2Type so2 = SO2Type::fitToSO2(R);
SO2Type so2_2 = SO2Type::fitToSO2(so2.matrix());
SOPHUS_TEST_APPROX(passed, so2.matrix(), so2_2.matrix(),
Constants<Scalar>::epsilon());
}
return passed;
}
template <class S = Scalar>
enable_if_t<!std::is_floating_point<S>::value, bool> testFit() {
return true;
}
std::vector<SO2Type, Eigen::aligned_allocator<SO2Type>> so2_vec_;
std::vector<Tangent, Eigen::aligned_allocator<Tangent>> tangent_vec_;
std::vector<Point, Eigen::aligned_allocator<Point>> point_vec_;
};
int test_so2() {
using std::cerr;
using std::endl;
cerr << "Test SO2" << endl << endl;
cerr << "Double tests: " << endl;
Tests<double>().runAll();
cerr << "Float tests: " << endl;
Tests<float>().runAll();
#if SOPHUS_CERES
cerr << "ceres::Jet<double, 3> tests: " << endl;
Tests<ceres::Jet<double, 3>>().runAll();
#endif
return 0;
}
} // namespace Sophus
int main() { return Sophus::test_so2(); }

287
Workspace/Thirdparty/Sophus/test/core/test_so3.cpp vendored Normal file
View File

@ -0,0 +1,287 @@
#include <iostream>
#include <sophus/interpolate.hpp>
#include <sophus/so3.hpp>
#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<Sophus::SO3<double>>;
template class Map<Sophus::SO3<double> const>;
} // namespace Eigen
namespace Sophus {
template class SO3<double, Eigen::AutoAlign>;
template class SO3<float, Eigen::DontAlign>;
#if SOPHUS_CERES
template class SO3<ceres::Jet<double, 3>>;
#endif
template <class Scalar>
class Tests {
public:
using SO3Type = SO3<Scalar>;
using Point = typename SO3<Scalar>::Point;
using Tangent = typename SO3<Scalar>::Tangent;
Scalar const kPi = Constants<Scalar>::pi();
Tests() {
so3_vec_.push_back(SO3Type(Eigen::Quaternion<Scalar>(
Scalar(0.1e-11), Scalar(0.), Scalar(1.), Scalar(0.))));
so3_vec_.push_back(SO3Type(Eigen::Quaternion<Scalar>(
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<SO3Type> 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<Scalar>::epsilon(), "Magnitude drift");
return passed;
}
bool testRawDataAcces() {
bool passed = true;
Eigen::Matrix<Scalar, 4, 1> raw = {Scalar(0), Scalar(1), Scalar(0),
Scalar(0)};
Eigen::Map<SO3Type const> map_of_const_so3(raw.data());
SOPHUS_TEST_APPROX(passed,
map_of_const_so3.unit_quaternion().coeffs().eval(), raw,
Constants<Scalar>::epsilon());
SOPHUS_TEST_EQUAL(
passed, map_of_const_so3.unit_quaternion().coeffs().data(), raw.data());
Eigen::Map<SO3Type const> 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<Scalar, 4, 1> raw2 = {Scalar(1), Scalar(0), Scalar(0),
Scalar(0)};
Eigen::Map<SO3Type> map_of_so3(raw.data());
Eigen::Quaternion<Scalar> quat;
quat.coeffs() = raw2;
map_of_so3.setQuaternion(quat);
SOPHUS_TEST_APPROX(passed, map_of_so3.unit_quaternion().coeffs().eval(),
raw2, Constants<Scalar>::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<SO3Type> 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<Scalar> data1(Scalar{1}, Scalar{0}, Scalar{0}, Scalar{0});
Vector4<Scalar> data2(Scalar{0}, Scalar{1}, Scalar{0}, Scalar{0});
Eigen::Map<SO3Type> 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<Scalar> R = so3_vec_.front().matrix();
SO3Type so3(R);
SOPHUS_TEST_APPROX(passed, R, so3.matrix(), Constants<Scalar>::epsilon());
return passed;
}
template <class S = Scalar>
enable_if_t<std::is_floating_point<S>::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<Scalar> 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<double>(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 <class S = Scalar>
enable_if_t<!std::is_floating_point<S>::value, bool> testSampleUniformSymmetry() {
return true;
}
template <class S = Scalar>
enable_if_t<std::is_floating_point<S>::value, bool> testFit() {
bool passed = true;
for (int i = 0; i < 100; ++i) {
Matrix3<Scalar> R = Matrix3<Scalar>::Random();
SO3Type so3 = SO3Type::fitToSO3(R);
SO3Type so3_2 = SO3Type::fitToSO3(so3.matrix());
SOPHUS_TEST_APPROX(passed, so3.matrix(), so3_2.matrix(),
Constants<Scalar>::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<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, SO3Type::rotY(angle).angleY(), angle,
Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, SO3Type::rotZ(angle).angleZ(), angle,
Constants<Scalar>::epsilon());
}
return passed;
}
template <class S = Scalar>
enable_if_t<!std::is_floating_point<S>::value, bool> testFit() {
return true;
}
std::vector<SO3Type, Eigen::aligned_allocator<SO3Type>> so3_vec_;
std::vector<Tangent, Eigen::aligned_allocator<Tangent>> tangent_vec_;
std::vector<Point, Eigen::aligned_allocator<Point>> point_vec_;
};
int test_so3() {
using std::cerr;
using std::endl;
cerr << "Test SO3" << endl << endl;
cerr << "Double tests: " << endl;
Tests<double>().runAll();
cerr << "Float tests: " << endl;
Tests<float>().runAll();
#if SOPHUS_CERES
cerr << "ceres::Jet<double, 3> tests: " << endl;
Tests<ceres::Jet<double, 3>>().runAll();
#endif
return 0;
}
} // namespace Sophus
int main() { return Sophus::test_so3(); }

Some files were not shown because too many files have changed in this diff Show More