├── .github └── workflows │ └── wheels.yml ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── Makefile ├── README.md ├── __init__.py ├── benchmarks ├── M3500.txt ├── README.md ├── sphere.g2o ├── sphere_bignoise_vertex3.g2o └── sphere_gt.g2o ├── doc ├── README.md └── doxyfile.conf ├── make.bat ├── mrobpy ├── CMakeLists.txt ├── FGraphPy.cpp ├── PCPlanesPy.cpp ├── PCRegistrationPy.cpp ├── README.md ├── SE3py.cpp ├── examples │ ├── SE3_covariance_visualization_examples.py │ ├── SE3_second_vs_fourth_compounding_order.py │ ├── test_utils.py │ └── vis_utils.py ├── mrobPy.cpp └── tests │ ├── fgraph_test.py │ ├── point_cloud_test.py │ ├── se3_test.py │ └── se3cov_test.py ├── pyproject.toml ├── python_examples ├── FGraph_2d.py ├── FGraph_2d_anchor.py ├── FGraph_M3500.py ├── FGraph_landmark_2d_example.py ├── FGraph_landmark_3d_example.py ├── FGraph_plane4d_example.py ├── FGraph_point_plane_align.py ├── FGraph_sphere.py ├── PC_align.py ├── README.md ├── SE3_examples.ipynb └── SE3_examples.py ├── scripts ├── build-wheel-linux.sh ├── build-wheel-macOS.sh └── build-wheels-windows.cmd ├── setup.cfg ├── setup.py ├── source ├── conf.py └── index.rst └── src ├── EigenFactors ├── CMakeLists.txt ├── README.md ├── examples │ ├── CMakeLists.txt │ ├── example_solve_1_poses.cpp │ └── test_EF_Hessian.cpp ├── mrob │ ├── factor_graph_ef.hpp │ └── plane_factor.hpp └── plane_factor.cpp ├── FGraph ├── CMakeLists.txt ├── README.md ├── examples │ ├── CMakeLists.txt │ ├── example_solver_2d.cpp │ ├── example_solver_3d.cpp │ ├── example_solver_3d_landmarks.cpp │ └── example_solver_dense_3d.cpp ├── factor.cpp ├── factor_graph.cpp ├── factor_graph_solve.cpp ├── factor_graph_solve_dense.cpp ├── factors │ ├── factor1Pose1Landmark2d.cpp │ ├── factor1Pose1Landmark3d.cpp │ ├── factor1Pose1Plane4d.cpp │ ├── factor1Pose2d.cpp │ ├── factor1Pose3d.cpp │ ├── factor2Poses2d.cpp │ ├── factor2Poses3d.cpp │ ├── nodeLandmark2d.cpp │ ├── nodeLandmark3d.cpp │ ├── nodePlane4d.cpp │ ├── nodePose2d.cpp │ └── nodePose3d.cpp ├── mrob │ ├── factor.hpp │ ├── factor_graph.hpp │ ├── factor_graph_solve.hpp │ ├── factor_graph_solve_dense.hpp │ ├── factors │ │ ├── factor1Pose1Landmark2d.hpp │ │ ├── factor1Pose1Landmark3d.hpp │ │ ├── factor1Pose1Plane4d.hpp │ │ ├── factor1Pose2d.hpp │ │ ├── factor1Pose3d.hpp │ │ ├── factor2Poses2d.hpp │ │ ├── factor2Poses3d.hpp │ │ ├── nodeLandmark2d.hpp │ │ ├── nodeLandmark3d.hpp │ │ ├── nodePlane4d.hpp │ │ ├── nodePose2d.hpp │ │ └── nodePose3d.hpp │ └── node.hpp ├── node.cpp └── tests │ └── CMakeLists.txt ├── PCRegistration ├── CMakeLists.txt ├── README.md ├── arun.cpp ├── create_points.cpp ├── estimate_plane.cpp ├── examples │ ├── CMakeLists.txt │ ├── example_align_mth.cpp │ ├── example_fgraph_align.cpp │ └── example_synthetic_plane_traj.cpp ├── factors │ ├── factor1PosePoint2Plane.cpp │ └── factor1PosePoint2Point.cpp ├── gicp.cpp ├── mrob │ ├── create_points.hpp │ ├── estimate_plane.hpp │ ├── factors │ │ ├── factor1PosePoint2Plane.hpp │ │ └── factor1PosePoint2Point.hpp │ ├── loam.hpp │ ├── pc_registration.hpp │ ├── plane.hpp │ └── plane_registration.hpp ├── plane.cpp ├── plane_registration.cpp └── weight_point.cpp ├── README.md ├── VIO └── README.md ├── common ├── CMakeLists.txt ├── README.md ├── mrob │ ├── matrix_base.hpp │ ├── optimizer.hpp │ └── time_profiling.hpp ├── optimizer.cpp └── time_profiling.cpp └── geometry ├── CMakeLists.txt ├── README.md ├── SE3.cpp ├── SE3cov.cpp ├── SO3.cpp ├── examples ├── CMakeLists.txt ├── test_SE3.cpp └── test_SE3cov.cpp └── mrob ├── SE3.hpp ├── SE3cov.hpp └── SO3.hpp /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | \#*\# 3 | .\#* 4 | *.swp 5 | *.swo 6 | [#]*[#] 7 | .#* 8 | *.sublime-project 9 | *.sublime-workspace 10 | *.cproject 11 | *.project 12 | .settings/ 13 | build/ 14 | lib/ 15 | bin/ 16 | *.so 17 | *.idea 18 | *.png 19 | *.pdf 20 | *.pkl 21 | .vscode 22 | doc/* 23 | __pycache__ 24 | .* 25 | 26 | mrob 27 | dist/ 28 | mrob.egg-info/ 29 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "pybind11"] 2 | path = external/pybind11 3 | url = https://github.com/pybind/pybind11.git 4 | [submodule "external/Eigen"] 5 | path = external/Eigen 6 | url = https://gitlab.com/libeigen/eigen.git 7 | [submodule "external/Catch2"] 8 | path = external/Catch2 9 | url = https://github.com/catchorg/Catch2.git 10 | branch = v2.x 11 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Pre-requisites about cmake itself 2 | CMAKE_MINIMUM_REQUIRED(VERSION 3.5) 3 | 4 | if(COMMAND cmake_policy) 5 | cmake_policy(SET CMP0005 NEW) 6 | cmake_policy(SET CMP0003 NEW) 7 | endif(COMMAND cmake_policy) 8 | 9 | PROJECT(mrob) 10 | 11 | IF (NOT CMAKE_BUILD_TYPE) 12 | SET(CMAKE_BUILD_TYPE "Release") 13 | ENDIF (NOT CMAKE_BUILD_TYPE) 14 | 15 | MESSAGE(STATUS "Build type: ${CMAKE_BUILD_TYPE}") 16 | 17 | SET(CMAKE_CXX_STANDARD 14) 18 | SET(CMAKE_CXX_EXTENSIONS ON) 19 | 20 | IF(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") 21 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") #TODO: /Wall but disable noisy warnings 22 | ADD_COMPILE_DEFINITIONS(_USE_MATH_DEFINES) 23 | SET(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) 24 | ELSEIF(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") 25 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") 26 | ELSEIF(CMAKE_CXX_COMPILER_ID MATCHES "Clang") 27 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic -Wno-inconsistent-missing-override") 28 | ELSE() 29 | ENDIF(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") 30 | 31 | IF(ANDROID) 32 | SET(BUILD_TESTING OFF) 33 | ELSE(ANDROID) 34 | SET(BUILD_TESTING ON) 35 | enable_testing() 36 | ENDIF(ANDROID) 37 | 38 | MESSAGE(STATUS "Build Tests: ${BUILD_TESTING}") 39 | 40 | # =================================================================== 41 | INCLUDE_DIRECTORIES(SYSTEM ./external/Eigen) 42 | 43 | # =================================================================== 44 | # CODE: main modules and directories are included here 45 | 46 | # MROB modules. Remove those that are not necessary, by default all are active 47 | INCLUDE_DIRECTORIES(./src/common) 48 | ADD_SUBDIRECTORY(./src/common) 49 | 50 | INCLUDE_DIRECTORIES(./src/geometry) 51 | ADD_SUBDIRECTORY(./src/geometry) 52 | 53 | INCLUDE_DIRECTORIES(./src/FGraph) 54 | ADD_SUBDIRECTORY(./src/FGraph) 55 | 56 | INCLUDE_DIRECTORIES(./src/PCRegistration) 57 | ADD_SUBDIRECTORY(./src/PCRegistration) 58 | 59 | 60 | #INCLUDE_DIRECTORIES(./src/EigenFactors) 61 | #ADD_SUBDIRECTORY(./src/EigenFactors) 62 | 63 | 64 | # =================================================================== 65 | # New modules should be included here 66 | 67 | IF(NOT ANDROID) 68 | ADD_SUBDIRECTORY(./external/pybind11) 69 | ADD_SUBDIRECTORY(./mrobpy) 70 | ENDIF(NOT ANDROID) 71 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = source 9 | BUILDDIR = build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 21 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![PyPi version](https://img.shields.io/pypi/v/mrob.svg)](https://pypi.org/project/mrob/) 2 | [![PyPi downloads](https://img.shields.io/pypi/dm/mrob.svg)](https://pypi.org/project/mrob/) 3 | [![Documentation Status](https://readthedocs.org/projects/mrob/badge/?version=latest)](https://mrob.readthedocs.io/en/latest/?badge=latest) 4 | 5 |

6 | 7 |

8 | 9 | # MROB: Mobile Robotics library 10 | The Skoltech Mobile Robotics library (mrob) is our common framework for implementing our robotics research and projects. It includes a core set of functionalities such as geometric transformations (SE3), factor graphs for general state estimation, optimization, 3D point cloud registration and more to come. 11 | 12 | The general structure for the algorithms implemented: 13 | * [common](https://github.com/MobileRoboticsSkoltech/mrob/tree/master/src/common): common matrix definitions and typedefs. 14 | * [geometry](https://github.com/MobileRoboticsSkoltech/mrob/tree/master/src/geometry): Geometric transformations, mostly Rotations and Rigid Body Transformations in 3D. 15 | * [Fgraph](https://github.com/MobileRoboticsSkoltech/mrob/tree/master/src/FGraph): Factor Graphs for state estimation 16 | * [PCReg](https://github.com/MobileRoboticsSkoltech/mrob/tree/master/src/PCRegistration): Point Cloud Registration. 17 | * [mrobPy](https://github.com/MobileRoboticsSkoltech/mrob/tree/master/mrobpy) Python bindings (using pybind11) for the above methods. 18 | 19 | ## Python Examples 20 | The library is mainly designed to run in python, that is, algorithms are written in cpp and bind to python for general purpose use. 21 | We provide some examples in [python_examples](https://github.com/MobileRoboticsSkoltech/mrob/tree/master/python_examples) for more details. 22 | 23 | 24 | ## Dependencies 25 | The present library is meant to be a self-contained library. However, there are few dependencies: 26 | * C++'14 27 | * CMake 28 | * [Eigen](https://gitlab.com/libeigen/eigen) (included as a submodule) 29 | * [pybind11](https://github.com/pybind/pybind11) (included as a submodule) 30 | - python3-distutils 31 | - python3-dev 32 | * [Catch2 v2.x branch](https://github.com/catchorg/Catch2/tree/v2.x) (included as a submodule) 33 | 34 | This is the list of required packages to install: 35 | `sudo apt install build-essential cmake python3-distutils python3-dev` 36 | 37 | 38 | ## Repository 39 | Standard github cloning, adding the recursive term for submodules. 40 | 41 | `git clone --recursive git@github.com:MobileRoboticsSkoltech/mrob.git` 42 | 43 | If there was ever a submodule update (not frequently) the command to use: 44 | 45 | `git submodule update --recursive` 46 | 47 | ## Installation 48 | ``` 49 | cd mrob 50 | mkdir build 51 | cd build 52 | cmake .. 53 | make -j 54 | ``` 55 | 56 | If you need to use this library in Python code, you can install it using pip: 57 | `pip install mrob` 58 | 59 | **Note:** If your OS is Windows and you don't have Microsoft Visual C++ Redistributable package installed, 60 | then you need to [install it](https://docs.microsoft.com/en-us/cpp/windows/latest-supported-vc-redist?view=msvc-160#visual-studio-2015-2017-2019-and-2022) additionally. 61 | If you are using a 32-bit Python, then install the package for the X86 architecture. 62 | If you are using 64-bit Python, then install the package for the X64 architecture. 63 | Don't be afraid to install both packages. 64 | 65 | 66 | ## License 67 | Apache-2.0 License 68 | 69 | 70 | -------------------------------------------------------------------------------- /__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # 15 | # 16 | # __init__.py 17 | # 18 | # Created on: Nov 11, 2020 19 | # Author: Lyubov Miloserdova 20 | # miloslubov@gmail.com 21 | # 22 | 23 | try: 24 | from . import mrob 25 | 26 | from pkg_resources import get_distribution 27 | 28 | __version__ = get_distribution('mrob').version 29 | 30 | for module in dir(mrob): 31 | n = len(module) - 1 32 | if not (module[:2] == '__' and module[n:n-2:-1] == '__') and module.count('.') == 0: 33 | globals()[module] = getattr(mrob, module) 34 | 35 | del mrob 36 | except ImportError: 37 | import platform 38 | 39 | if platform.system() == "Windows": 40 | import sys 41 | 42 | sys.tracebacklimit = 0 43 | raise ImportError( 44 | "Maybe you don't have Microsoft Visual C++ Redistributable package installed. " + 45 | "Please follow the link and install redistributable " + 46 | "package: https://docs.microsoft.com/en-us/cpp/windows/latest-supported-vc-redist?view=msvc-160" + 47 | "#visual-studio-2015-2017-2019-and-2022") from None 48 | 49 | raise 50 | -------------------------------------------------------------------------------- /benchmarks/README.md: -------------------------------------------------------------------------------- 1 | # Datasets for benchmarking 2 | 3 | Here is a collection of benchmarks 4 | 5 | ## M3500 6 | 2D Pose SLAM from Olson and others. "Fast iterative alignment of pose graphs with poor initial estimates." ICRA 2006. 7 | 8 | ## Sphere 9 | 3D Pose SLAM. The examples were created with [g2o](https://github.com/RainerKuemmerle/g2o). There are 2 benchmarks: one with almost no noise (gt) and ** with some noise. 10 | -------------------------------------------------------------------------------- /doc/README.md: -------------------------------------------------------------------------------- 1 | 2 | ## How to run doxygen util 3 | * First, install the ``doxygen`` util: 4 | 5 | `sudo apt-get install doxygen` 6 | 7 | * Run ``doxygen`` util with config file from repository root folder: 8 | 9 | `doxygen ./doc/doxyfile.conf` 10 | 11 | * Generated html files will be added into the `./doc/html` folder; 12 | * Open `index.html` file in browser to start navigating the documentation. 13 | 14 | ## Useful links 15 | Documentation: 16 | * [C++ tags](https://www.doxygen.nl/manual/docblocks.html#cppblock); 17 | * [python tags](https://www.doxygen.nl/manual/docblocks.html#pythonblocks); 18 | * [config flags](https://www.doxygen.nl/manual/config.html#cfg_input); 19 | * [cheat sheet](https://www.mitk.org/w/images/1/1c/BugSquashingSeminars%242013-07-17-DoxyReference.pdf). -------------------------------------------------------------------------------- /make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | pushd %~dp0 4 | 5 | REM Command file for Sphinx documentation 6 | 7 | if "%SPHINXBUILD%" == "" ( 8 | set SPHINXBUILD=sphinx-build 9 | ) 10 | set SOURCEDIR=source 11 | set BUILDDIR=build 12 | 13 | if "%1" == "" goto help 14 | 15 | %SPHINXBUILD% >NUL 2>NUL 16 | if errorlevel 9009 ( 17 | echo. 18 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 19 | echo.installed, then set the SPHINXBUILD environment variable to point 20 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 21 | echo.may add the Sphinx directory to PATH. 22 | echo. 23 | echo.If you don't have Sphinx installed, grab it from 24 | echo.http://sphinx-doc.org/ 25 | exit /b 1 26 | ) 27 | 28 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 29 | goto end 30 | 31 | :help 32 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 33 | 34 | :end 35 | popd 36 | -------------------------------------------------------------------------------- /mrobpy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | pybind11_add_module(mrob mrobPy.cpp SE3py.cpp PCRegistrationPy.cpp PCPlanesPy.cpp FGraphPy.cpp) 2 | target_link_libraries(mrob PRIVATE SE3 PCRegistration FGraph ) 3 | -------------------------------------------------------------------------------- /mrobpy/PCRegistrationPy.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * PCRegistrationPy.cpp 17 | * 18 | * Created on: Jan 17, 2019 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | 25 | /** 26 | * This program generates the Python binding for the SE3 library 27 | * using the pybind11 library, here included as an external dependency, 28 | */ 29 | 30 | #include 31 | #include 32 | namespace py = pybind11; 33 | 34 | 35 | #include "mrob/SE3.hpp" 36 | #include "mrob/pc_registration.hpp" 37 | 38 | #include "mrob/estimate_plane.hpp" 39 | 40 | 41 | using namespace mrob; 42 | 43 | 44 | SE3 arun_solve(const py::EigenDRef X, const py::EigenDRef Y) 45 | { 46 | SE3 res; 47 | PCRegistration::arun(X,Y,res); 48 | return res; 49 | } 50 | 51 | 52 | SE3 gicp_solve(const py::EigenDRef X, const py::EigenDRef Y, 53 | const py::EigenDRef covX, const py::EigenDRef covY) 54 | { 55 | SE3 res; 56 | PCRegistration::gicp(X,Y,covX,covY,res); 57 | return res; 58 | } 59 | 60 | SE3 weighted_solve(const py::EigenDRef X, const py::EigenDRef Y, 61 | const py::EigenDRef weight) 62 | { 63 | SE3 res; 64 | PCRegistration::weighted_point(X,Y,weight,res); 65 | return res; 66 | } 67 | 68 | Mat41 estimate_plane_py(const py::EigenDRef X) 69 | { 70 | Mat41 plane = estimate_plane(X); 71 | return plane; 72 | } 73 | 74 | void init_PCRegistration(py::module &m) 75 | { 76 | m.def("arun", &arun_solve); 77 | m.def("gicp", &gicp_solve); 78 | m.def("weighted", &weighted_solve); 79 | m.def("estimate_plane", &estimate_plane_py); 80 | m.def("estimate_normal", &estimate_normal); 81 | m.def("estimate_centroid", &estimate_centroid); 82 | } 83 | 84 | 85 | -------------------------------------------------------------------------------- /mrobpy/README.md: -------------------------------------------------------------------------------- 1 | ## Python short instructions: 2 | 3 | Early implementation of tests (half tests). 4 | We run benchmarks and major issues, exceptions are triggered and test is not passed. 5 | 6 | On this stage we don't evaluate correctness (TODO). 7 | -------------------------------------------------------------------------------- /mrobpy/examples/SE3_covariance_visualization_examples.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import plotly.express as px 4 | import pandas as pd 5 | 6 | 7 | import mrob 8 | 9 | # import utils 10 | from vis_utils import sigma_visualize, sigma_visualize_3d 11 | from test_utils import get_mc, compound_mc 12 | 13 | # EXAMPLE #1 14 | #================================================================ 15 | plt.figure(figsize=(10,10)) 16 | plt.suptitle('Covariance ellipsoid visualisation v.s. \ncorresponding Monte Carlo direct sampling distribution') 17 | sigma = np.diag([0,0,0.1,0.001,0.01,0]) 18 | mean = np.array([0,0,0,0,0,0]) 19 | poses, xi = get_mc(mrob.geometry.SE3([0,0,0,1,0,0]), sigma, mean,N=1_000_0) 20 | sigma_visualize(mrob.geometry.SE3([0,0,0,1,0,0]), sigma=sigma,N = 100, K=[3], color='red') 21 | plt.scatter(poses[:,0],poses[:,1], label='Monte Carlo',s=2) 22 | plt.xlim([-1.5,1.5]) 23 | plt.ylim([-1.5,1.5]) 24 | plt.grid() 25 | 26 | plt.xlabel('X') 27 | plt.ylabel('Y') 28 | plt.show() 29 | 30 | #EXAMPLE #2 31 | #================================================================ 32 | # defining two poses with uncertainty to compound 33 | xi_1 = np.array([0,0,0,0.5,0,0]) 34 | T_1 = mrob.geometry.SE3(xi_1) 35 | sigma_1 = np.diag([0,0,0.01,0.01,0.01,0]) 36 | 37 | xi_2 = np.array([0,0,1.5,1.0,0,0]) 38 | T_2 = mrob.geometry.SE3(xi_2) 39 | sigma_2 = np.diag([0,0,0.1,0.01,0.01,0]) 40 | 41 | # performing monte carlo compounding of two poses 42 | T,sigma = compound_mc(T_1, sigma_1, T_2, sigma_2, M=100_000) 43 | 44 | plt.figure(figsize=(10,10)) 45 | plt.title('Monte Carlo pose compounding') 46 | sigma_visualize(T_1, sigma_1, label='sigma_1',color='r') 47 | sigma_visualize(T_2, sigma_2, label='sigma_2',color='g') 48 | sigma_visualize(T,sigma,label='compound',color='b') 49 | 50 | poses, xi = get_mc(T, sigma,N=20000) 51 | 52 | plt.scatter(poses[:,0],poses[:,1], label='Monte Carlo',s = 2) 53 | plt.grid() 54 | plt.axis('equal') 55 | plt.show() 56 | 57 | 58 | # EXAMPLE #3 59 | #================================================================ 60 | # defining two poses with uncertainty to compound 61 | xi_1 = np.array([0,0,0,0.5,0,0]) 62 | T_1 = mrob.geometry.SE3(xi_1) 63 | sigma_1 = np.diag([0,0,0.01,0.01,0.01,0]) 64 | 65 | cov = mrob.geometry.SE3Cov(T_1,sigma_1) 66 | 67 | xi_2 = np.array([0,0,1.5,1.0,0,0]) 68 | T_2 = mrob.geometry.SE3(xi_2) 69 | sigma_2 = np.diag([0,0,0.1,0.01,0.01,0]) 70 | 71 | # performing 2nd order compounding of two poses 72 | new_cov = cov.compound_2nd_order(T_2, sigma_2) 73 | T, sigma = mrob.geometry.SE3(new_cov.T()), new_cov.cov() 74 | 75 | plt.figure(figsize=(10,10)) 76 | plt.title('Second order pose compounding') 77 | sigma_visualize(T_1, sigma_1, label='sigma_1',color='r') 78 | sigma_visualize(T_2, sigma_2, label='sigma_2',color='g') 79 | sigma_visualize(T,sigma, label='2nd order \ncompound',color='b') 80 | 81 | plt.grid() 82 | plt.axis('equal') 83 | plt.show() 84 | 85 | # EXAMPLE #4 86 | #================================================================ 87 | plt.figure(figsize=(10,10)) 88 | plt.suptitle('Covariance ellipsoid visualisation v.s. \ncorresponding Monte Carlo direct sampling distribution') 89 | # sigma = np.diag([.3,.01,0.01,0.01,0.01,0.02]) 90 | mean = np.array([0,0,0,0,0,0]) 91 | 92 | T = mrob.geometry.SE3([0.1,-0.1,0.05,1,0.05,0.-1]) 93 | sigma = np.diag([0.01,0.01,0.1,0.001,0.001,0.001]) 94 | 95 | axes, circumferences = sigma_visualize_3d(T,sigma,N = 100,K = 3) 96 | 97 | df = pd.DataFrame(columns = ['x','y','z']) 98 | 99 | for key,val in axes.items(): 100 | tmp = pd.DataFrame(val,columns=['x','y','z']) 101 | tmp['label'] = key 102 | df = pd.concat([df,tmp]) 103 | 104 | for key,val in circumferences.items(): 105 | tmp = pd.DataFrame(val,columns=['x','y','z']) 106 | tmp['label'] = key 107 | df = pd.concat([df,tmp]) 108 | 109 | fig = px.line_3d(data_frame=df,x='x',y='y',z='z',color='label',hover_name='label') 110 | 111 | # Noise has zero mean values 112 | mean = np.zeros(6) 113 | poses, xi = get_mc(T, sigma, mean,N=1_000) 114 | particles = pd.DataFrame(poses, columns=['x','y','z']) 115 | fig.add_scatter3d(x=particles['x'],y=particles['y'],z=particles['z'],opacity=0.5,mode='markers',marker=dict(size=3)) 116 | fig.show() 117 | -------------------------------------------------------------------------------- /mrobpy/examples/SE3_second_vs_fourth_compounding_order.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from tqdm import tqdm 3 | 4 | import matplotlib 5 | from sys import platform 6 | if platform == "darwin": 7 | matplotlib.use('PS') 8 | import matplotlib.pyplot as plt 9 | 10 | import mrob 11 | 12 | from test_utils import compound_mc 13 | 14 | M = 100000 15 | err_1 = [] 16 | err_2 = [] 17 | 18 | alphas = np.linspace(0,1.0,20)#np.arange(0,1.01,0.1) 19 | 20 | for alpha in tqdm(alphas): 21 | xi_1 = np.array([np.pi/6,0,0,0,2,0]) 22 | T_1 = mrob.geometry.SE3(xi_1) 23 | sigma_1 = alpha*np.diag([1/2,1,1/2,10,5,5]) 24 | 25 | cov = mrob.geometry.SE3Cov(T_1, sigma_1) 26 | 27 | xi_2 = np.array([0,np.pi/4,0,0,0,1]) 28 | T_2 = mrob.geometry.SE3(xi_2) 29 | sigma_2 = alpha*np.diag([1/2,1/2,1,5,10,5]) 30 | 31 | T, sigma_mc = compound_mc(T_1, sigma_1, T_2, sigma_2, M=M) 32 | 33 | cov_2nd = cov.compound_2nd_order(T_2, sigma_2) 34 | cov_4th = cov.compound_4th_order(T_2, sigma_2) 35 | 36 | err_1.append(np.linalg.norm(sigma_mc - cov_2nd.cov())) 37 | err_2.append(np.linalg.norm(sigma_mc - cov_4th.cov())) 38 | 39 | plt.figure(figsize=(10,10)) 40 | plt.title('Error between 2nd and 4th order pose compounding and \nMonte Carlo pose compounding') 41 | plt.plot(alphas,err_1,'-x', label='2nd order') 42 | plt.plot(alphas,err_2,'-x', label='4th order') 43 | plt.xlabel('alpha') 44 | plt.ylabel('error') 45 | plt.grid() 46 | plt.legend() 47 | plt.show() 48 | -------------------------------------------------------------------------------- /mrobpy/examples/test_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | import mrob 4 | 5 | def get_mc(T, sigma, mean=[0,0,0,0,0,0], N = 100000): 6 | 7 | xi = np.random.multivariate_normal(mean,sigma,N,check_valid='ignore') 8 | 9 | propagated = [] 10 | for i in range(len(xi)): 11 | tmp = mrob.geometry.SE3(T) 12 | tmp.update_lhs(xi[i]) 13 | propagated.append(tmp) 14 | 15 | poses = np.array([x.t() for x in propagated]) 16 | poses = poses.reshape((-1,3)) 17 | 18 | xi = np.array([x.Ln() for x in propagated]) 19 | return poses, xi 20 | 21 | def compound_mc(T_1, sigma_1, T_2, sigma_2, M = 10000): 22 | 23 | # generating distributions 24 | p1, xi1 = get_mc(T_1, sigma_1, N=M) 25 | 26 | p2, xi2 = get_mc(T_2, sigma_2, N=M) 27 | 28 | # mean pose 29 | T = T_1.mul(T_2) 30 | 31 | sigma_mc = np.zeros_like(sigma_1) 32 | 33 | xi_m = [] 34 | 35 | for i in range(xi1.shape[0]): 36 | 37 | T_r = mrob.geometry.SE3(xi2[i]) 38 | T_l = mrob.geometry.SE3(xi1[i]) 39 | 40 | T_m = T_l.mul(T_r) 41 | 42 | xi_m.append(T_m.mul(T.inv()).Ln()) 43 | 44 | # calculating cavirance of xi points coordinates 45 | sigma_mc = np.cov(np.array(xi_m).transpose()) 46 | return T, sigma_mc 47 | 48 | 49 | # method to do block matrix permutations 50 | def sigma_permute(sigma): 51 | s = np.zeros_like(sigma) 52 | # top left block 53 | s[:3,:3] = sigma[3:,3:] 54 | 55 | # top right block 56 | s[:3,3:] = sigma[3:,:3] 57 | 58 | # bottom right block 59 | s[3:,3:] = sigma[:3,:3] 60 | 61 | # bottom left block 62 | s[3:,:3] = sigma[:3,3:] 63 | 64 | return s 65 | 66 | def compound_2nd(T_1, sigma_1, T_2, sigma_2): 67 | T = T_1.mul(T_2) 68 | T_1_adj = sigma_permute(T_1.adj()) 69 | 70 | sigma_2_ = T_1_adj@sigma_permute(sigma_2)@T_1_adj.transpose() 71 | 72 | sigma = sigma_permute(sigma_1) + sigma_2_ 73 | sigma = sigma_permute(sigma) 74 | 75 | return T, sigma 76 | 77 | def op1(A): 78 | return -np.eye(A.shape[0])*A.trace() + A 79 | 80 | def op2(A,B): 81 | return op1(A)@op1(B) + op1(B@A) 82 | 83 | def compound_4th(T_1, sigma_1, T_2, sigma_2): 84 | # applying 2nd order to get part of 4th order decomposition 85 | T, sigma = compound_2nd(T_1, sigma_1, T_2, sigma_2) 86 | 87 | T_1_adj = sigma_permute(T_1.adj()) 88 | 89 | _sigma_2 = T_1_adj @ sigma_permute(sigma_2) @ T_1_adj.transpose() 90 | 91 | # map all sigma into [rho, phi] order 92 | tmp = sigma_permute(sigma) 93 | s1 = sigma_permute(sigma_1) 94 | s2 = _sigma_2 95 | 96 | # using equations from reference paper 97 | sigma_1_rho_rho = s1[:3, :3] 98 | sigma_1_rho_phi = s1[:3, 3:] 99 | sigma_1_phi_phi = s1[3:, 3:] 100 | 101 | _sigma_2_rho_rho = s2[:3, :3] 102 | _sigma_2_rho_phi = s2[:3, 3:] 103 | _sigma_2_phi_phi = s2[3:, 3:] 104 | 105 | A_1 = np.zeros((6,6)) 106 | A_1[:3, :3] = op1(sigma_1_phi_phi) 107 | A_1[:3, 3:] = op1(sigma_1_rho_phi + sigma_1_rho_phi.transpose()) 108 | A_1[3:, 3:] = op1(sigma_1_phi_phi) 109 | 110 | _A_2 = np.zeros((6,6)) 111 | _A_2[:3, :3] = op1(_sigma_2_phi_phi) 112 | _A_2[:3, 3:] = op1(_sigma_2_rho_phi + _sigma_2_rho_phi.transpose()) 113 | _A_2[3:, 3:] = op1(_sigma_2_phi_phi) 114 | 115 | B_rho_rho = op2(sigma_1_phi_phi, _sigma_2_rho_rho) + \ 116 | op2(sigma_1_rho_phi.transpose(), _sigma_2_rho_phi) + \ 117 | op2(sigma_1_rho_phi,_sigma_2_rho_phi.transpose()) + \ 118 | op2(sigma_1_rho_rho, _sigma_2_phi_phi) 119 | 120 | B_rho_phi = op2(sigma_1_phi_phi,_sigma_2_rho_phi.transpose()) + \ 121 | op2(sigma_1_rho_phi,_sigma_2_phi_phi) # there was an error in Barfoot's paper and book 122 | 123 | B_phi_phi = op2(sigma_1_phi_phi, _sigma_2_phi_phi) 124 | 125 | B = np.zeros_like(sigma_1) 126 | B[:3,:3] = B_rho_rho 127 | B[:3,3:] = B_rho_phi 128 | B[3:,:3] = B_rho_phi.transpose() 129 | B[3:,3:] = B_phi_phi 130 | 131 | tmp += 1./12*(A_1@s2 + \ 132 | s2@A_1.transpose() + \ 133 | _A_2@s1 + \ 134 | s1@ _A_2.transpose()) + 0.25*B 135 | 136 | # mapping resulting sigma into [phi,rho] order 137 | res = sigma_permute(tmp) 138 | 139 | return T, res 140 | -------------------------------------------------------------------------------- /mrobpy/mrobPy.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * mrobPy.cpp 17 | * 18 | * Created on: Jan 17, 2019 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | 25 | #include 26 | #include 27 | 28 | 29 | #include "mrob/optimizer.hpp" 30 | 31 | namespace py = pybind11; 32 | 33 | 34 | 35 | void init_geometry(py::module &m); 36 | void init_FGraph(py::module &m); 37 | //void init_FGraphDense(py::module &m); 38 | void init_PCRegistration(py::module &m); 39 | void init_PCPlanes(py::module &m); 40 | 41 | 42 | 43 | PYBIND11_MODULE(mrob, m) { 44 | m.doc() = "pybind11 MROB library, now including \n-geometry: SE3, SO3 and other routines\n-registration: routines for PC aligment and others\n-fgrad: Factors Graphs "; 45 | // Later, in binding code: 46 | py::add_ostream_redirect(m, "ostream_redirect"); 47 | 48 | py::enum_(m, "optimMethod") 49 | .value("NEWTON_RAPHSON", mrob::Optimizer::optimMethod::NEWTON_RAPHSON) 50 | .value("LEVENBERG_MARQUARDT_SPHER", mrob::Optimizer::optimMethod::LEVENBERG_MARQUARDT_SPHER) 51 | .value("LEVENBERG_MARQUARDT_ELLIP", mrob::Optimizer::optimMethod::LEVENBERG_MARQUARDT_ELLIP) 52 | .export_values() 53 | ; 54 | 55 | // TODO to be deprecated this namespace 56 | py::module m_geom = m.def_submodule("geometry"); 57 | init_geometry(m_geom); 58 | 59 | // deprecated have removed this namespace 60 | //py::module m_fg = m.def_submodule("fgraph"); 61 | init_FGraph(m); 62 | 63 | py::module m_reg = m.def_submodule("registration"); 64 | init_PCRegistration(m_reg); 65 | init_PCPlanes(m_reg); 66 | } 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /mrobpy/tests/point_cloud_test.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import mrob 3 | 4 | import pytest 5 | 6 | class TestPointCloud: 7 | def test_point_cloud_registration(self): 8 | # example equal to ./PC_alignment/examples/example_align.cpp 9 | # generate random data 10 | N = 500 11 | X = np.random.rand(N,3) 12 | T = mrob.geometry.SE3(np.random.rand(6)) 13 | Y = T.transform_array(X) 14 | 15 | print('X = \n', X,'\n T = \n', T.T(),'\n Y =\n', Y) 16 | 17 | # solve the problem 18 | T_arun = mrob.registration.arun(X,Y) 19 | print('Arun solution =\n', T_arun.T()) 20 | 21 | assert(np.ndarray.all(np.isclose(T.T(), T_arun.T()))) 22 | 23 | W = np.ones(N) 24 | T_wp = mrob.registration.weighted(X,Y,W) 25 | print('Weighted point optimization solution =\n', T_wp.T()) 26 | 27 | assert(np.ndarray.all(np.isclose(T.T(),T_wp.T()))) 28 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # 15 | # 16 | # pyproject.toml 17 | # 18 | # Created on: Oct 31, 2020 19 | # Author: Lyubov Miloserdova 20 | # miloslubov@gmail.com 21 | # 22 | 23 | [build-system] 24 | requires = ["setuptools >= 40.8.0", "wheel", "setuptools-git-versioning >= 1.2.10"] 25 | build-backend = "setuptools.build_meta" 26 | -------------------------------------------------------------------------------- /python_examples/FGraph_2d.py: -------------------------------------------------------------------------------- 1 | # 2 | import mrob 3 | import numpy as np 4 | 5 | # example similar to ./FGrpah/examples/example_FGraph_solve.cpp 6 | 7 | # create graph, reserving for 10 nodes and 20 factors if indicated 8 | graph = mrob.FGraph() 9 | 10 | # TODO give more nodes, such as a rectangle. 11 | x = np.random.randn(3) 12 | n1 = graph.add_node_pose_2d(x) 13 | x = 1 + np.random.randn(3)*1e-1 14 | n2 = graph.add_node_pose_2d(x) 15 | print('node 1 id = ', n1, ' , node 2 id = ', n2) 16 | 17 | 18 | invCov = np.identity(3) 19 | graph.add_factor_1pose_2d(np.array([0,0,np.pi/4]),n1,1e6*invCov) 20 | graph.add_factor_2poses_2d(np.ones(3),n1,n2,invCov) 21 | 22 | graph.solve(mrob.LM) 23 | graph.print(True) 24 | 25 | 26 | -------------------------------------------------------------------------------- /python_examples/FGraph_2d_anchor.py: -------------------------------------------------------------------------------- 1 | # 2 | import mrob 3 | import numpy as np 4 | 5 | # this example is to show the effect of anchors: We fix some given nodes, that are excluded from optimization updates 6 | 7 | print('Anchor on first node:') 8 | graph = mrob.FGraph() 9 | 10 | x = np.random.randn(3) 11 | n1 = graph.add_node_pose_2d(x,mrob.NODE_ANCHOR) 12 | x = 1 + np.random.randn(3)*1e-1 13 | n2 = graph.add_node_pose_2d(x) 14 | print('node 1 id = ', n1, ' , node 2 id = ', n2) 15 | 16 | 17 | invCov = np.identity(3) 18 | graph.add_factor_2poses_2d(np.ones(3),n1,n2,invCov) 19 | 20 | graph.solve(mrob.LM) 21 | graph.print(True) 22 | 23 | A = graph.get_adjacency_matrix() 24 | A = A.todense() 25 | print(A) 26 | 27 | 28 | print('Anchor on second node:') 29 | graph = mrob.FGraph() 30 | 31 | x = np.random.randn(3) 32 | n1 = graph.add_node_pose_2d(x) 33 | x = 1 + np.random.randn(3)*1e-1 34 | n2 = graph.add_node_pose_2d(x,mrob.NODE_ANCHOR) 35 | print('node 1 id = ', n1, ' , node 2 id = ', n2) 36 | 37 | 38 | invCov = np.identity(3) 39 | graph.add_factor_2poses_2d(np.ones(3),n1,n2,invCov) 40 | 41 | graph.solve(mrob.LM) 42 | graph.print(True) 43 | 44 | A = graph.get_adjacency_matrix() 45 | A = A.todense() 46 | print(A) 47 | 48 | 49 | print('virtual anchor by a high cov:') 50 | graph = mrob.FGraph() 51 | 52 | # TODO give more nodes, such as a rectangle. 53 | x = np.random.randn(3) 54 | n1 = graph.add_node_pose_2d(x) 55 | invCov = np.identity(3) 56 | graph.add_factor_1pose_2d(x,n1,1e6*invCov) 57 | x = 1 + np.random.randn(3)*1e-1 58 | n2 = graph.add_node_pose_2d(x) 59 | print('node 1 id = ', n1, ' , node 2 id = ', n2) 60 | graph.add_factor_2poses_2d(np.ones(3),n1,n2,invCov) 61 | 62 | graph.solve(mrob.LM) 63 | graph.print(True) 64 | 65 | A = graph.get_adjacency_matrix() 66 | A = A.todense() 67 | print(A) 68 | -------------------------------------------------------------------------------- /python_examples/FGraph_M3500.py: -------------------------------------------------------------------------------- 1 | # 2 | import mrob 3 | import numpy as np 4 | import time 5 | 6 | import matplotlib.pyplot as plt 7 | 8 | 9 | 10 | def print_2d_graph(graph): 11 | '''This function draws the state variables for a 2D pose graph''' 12 | 13 | # read graph, returns a list (vector) of state (np arrays) 14 | x = graph.get_estimated_state() 15 | prev_p = np.zeros(3) 16 | plt.figure() 17 | p = x[0] 18 | plt.plot(p[0],p[0],'ob') 19 | for p in x: 20 | #plt.plot(p[0],p[1],'ob') 21 | plt.plot((prev_p[0],p[0]),(prev_p[1],p[1]) , '-b') 22 | prev_p = p 23 | plt.show() 24 | 25 | 26 | 27 | 28 | # Initialize data structures 29 | vertex_ini = {} 30 | factors = {} 31 | factors_dictionary = {} 32 | N = 3500 33 | 34 | # load file 35 | with open('../benchmarks/M3500.txt', 'r') as file: 36 | for line in file: 37 | d = line.split() 38 | # read edges and vertex, in TORO format 39 | if d[0] == 'EDGE2': 40 | # EDGE2 id_origin id_target dx dy dth I11 I12 I22 I33 I13 I23 41 | factors[int(d[1]),int(d[2])] = np.array([d[3], d[4], d[5], d[6],d[7],d[8],d[9],d[10],d[11]],dtype='float64') 42 | factors_dictionary[int(d[2])].append(int(d[1])) 43 | else: 44 | # VERTEX2 id x y theta 45 | # these are the initial guesses for node states 46 | vertex_ini[int(d[1])] = np.array([d[2], d[3], d[4]],dtype='float64') 47 | # create an empty list of pairs of nodes (factor) connected to each node 48 | factors_dictionary[int(d[1])] = [] 49 | 50 | #print(factors_dictionary) 51 | 52 | # Initialize FG 53 | graph = mrob.FGraph() 54 | x = np.zeros(3) 55 | n = graph.add_node_pose_2d(x,mrob.NODE_ANCHOR) 56 | print('node 0 id = ', n) # id starts at 1 57 | processing_time = [] 58 | 59 | # start events, we solve for each node, adding it and it corresponding factors 60 | # in total takes 0.3s to read all datastructure 61 | for t in range(1,N): 62 | x = vertex_ini[t] 63 | n = graph.add_node_pose_2d(x) 64 | assert t == n, 'index on node is different from counter' 65 | 66 | # find factors to add. there must be 1 odom and other observations 67 | connecting_nodes = factors_dictionary[n] 68 | 69 | for nodeOrigin in factors_dictionary[n]: 70 | # inputs: obs, idOrigin, idTarget, invCov 71 | obs = factors[nodeOrigin, t][:3] 72 | covInv = np.zeros((3,3)) 73 | # on M3500 always diagonal information matrices 74 | covInv[0,0] = factors[nodeOrigin, t][3] 75 | covInv[1,1] = factors[nodeOrigin, t][5] 76 | covInv[2,2] = factors[nodeOrigin, t][6] 77 | graph.add_factor_2poses_2d(obs, nodeOrigin,t,covInv) 78 | # for end. no more loop inside the factors 79 | 80 | 81 | # solve the problem 7s 2500nodes 82 | start = time.time() 83 | #graph.solve(mrob.GN) 84 | end = time.time() 85 | #print('Iteration = ', t, ', chi2 = ', graph.chi2() , ', time on calculation [ms] = ', 1e3*(end - start)) 86 | processing_time.append(1e3*(end - start)) 87 | 88 | # problem error or xi2 89 | #print('X2', t) 90 | 91 | # plot the current problem 92 | if (t+1) % 500 == 0: 93 | #print_2d_graph(graph) 94 | pass 95 | 96 | print('Initial problem drawn') 97 | print_2d_graph(graph) 98 | print('current initial chi2 = ', graph.chi2() ) 99 | start = time.time() 100 | graph.solve(mrob.LM, 50) 101 | end = time.time() 102 | print('\nLM chi2 = ', graph.chi2() , ', total time on calculation [s] = ', 1e0*(end - start)) 103 | print('solution drawn') 104 | print_2d_graph(graph) 105 | 106 | 107 | if 0: 108 | # Information matrix 109 | import matplotlib.pyplot as plt 110 | L = graph.get_information_matrix() 111 | plt.spy(L, marker='o', markersize=5) 112 | plt.title('Information matrix $\Lambda$') 113 | plt.show() 114 | 115 | # ch2 116 | chi2_array = graph.get_chi2_array() 117 | plt.plot(chi2_array) 118 | plt.show() 119 | 120 | # alternative use Gauss-Newton 121 | if 0: 122 | graph.solve(mrob.fgraph.GN) 123 | print('Iter 0 chi2 = ', graph.chi2() ) 124 | graph.solve(mrob.GN) 125 | print('Iter 1 chi2 = ', graph.chi2() ) 126 | graph.solve(mrob.GN) 127 | print('Iter 2 chi2 = ', graph.chi2() ) 128 | graph.solve(mrob.GN) 129 | print('Iter 3 chi2 = ', graph.chi2() ) 130 | print_2d_graph(graph) 131 | 132 | graph.solve(mrob.GN) 133 | print('Iter 4 chi2 = ', graph.chi2() ) 134 | graph.solve(mrob.GN) 135 | print('Iter 5 chi2 = ', graph.chi2() ) #already converges 136 | print_2d_graph(graph) 137 | 138 | 139 | 140 | -------------------------------------------------------------------------------- /python_examples/FGraph_landmark_2d_example.py: -------------------------------------------------------------------------------- 1 | # 2 | # add path to local library mrob on bash_aliases: "export PYTHONPATH=${PYTHONPATH}:${HOME}/mrob/mrob/lib" 3 | import mrob 4 | import numpy as np 5 | 6 | # create graph 7 | graph = mrob.FGraph() 8 | 9 | #initial point at [0,0,0] with some noise 10 | n1 = graph.add_node_pose_2d(np.random.randn(3)*0.0005) 11 | #n1 = graph.add_node_pose_2d(np.zeros(3)*0.0005)# hay un problem aqui cuando se inicia con el def constructor 12 | W_0 = np.identity(3) 13 | graph.add_factor_1pose_2d(np.zeros(3),n1,1e6*W_0) 14 | #n1 = graph.add_node_pose_2d(np.zeros(3), mrob.NODE_ANCHOR) 15 | # non-initialized landmarks, but they could be initialiazed when adding factors 16 | # In case of 2d landmarks, they might be sensitive to IC, around 0 of initiakl pose and landmark On those cases might (rearely) GN fail to converge. 17 | l1 = graph.add_node_landmark_2d(np.array([0,0])) 18 | l2 = graph.add_node_landmark_2d(np.array([0,0])) 19 | print('node pose id = ', n1, ' , node landmark 1 id = ', l1 , ' , node landmark 2 id = ', l2) 20 | 21 | 22 | # landmark factors 23 | obs = np.array([1,0]) 24 | W = np.identity(2) 25 | graph.add_factor_1pose_1landmark_2d(obs,n1,l1,W) 26 | obs = np.array([1,np.pi/2]) 27 | graph.add_factor_1pose_1landmark_2d(obs,n1,l2,W) 28 | 29 | 30 | 31 | print('\n\n\n Solving Fgraph:\n') 32 | graph.solve(mrob.GN) #1 iteration of Gauss-Newton 33 | print(graph.get_information_matrix()) 34 | #graph.print(True) 35 | #graph.solve(mrob.LM) 36 | #graph.print(True) 37 | print(graph.get_estimated_state()) 38 | print('Current chi2 = ', graph.chi2() ) # re-evaluates the error, on print it is only the error on evalation before update 39 | 40 | 41 | -------------------------------------------------------------------------------- /python_examples/FGraph_landmark_3d_example.py: -------------------------------------------------------------------------------- 1 | # 2 | # add path to local library mrob on bashr_rc: "export PYTHONPATH=${PYTHONPATH}:${HOME}/mrob/mrob/build/mrobpy" 3 | import mrob 4 | import numpy as np 5 | 6 | # example translated from FGrpah/examples/example_solver_3d_landmarks.cpp 7 | 8 | # Different ways to initialize robust factors 9 | #graph = mrob.FGraph(mrob.QUADRATIC) 10 | #graph = mrob.FGraph(mrob.HUBER) 11 | #graph = mrob.FGraph(mrob.MCCLURE) 12 | graph = mrob.FGraph(mrob.CAUCHY) 13 | 14 | xi = np.array([-0.0282668, -0.05882867, 0.0945925, -0.02430618, 0.01794402, -0.06549129]) 15 | n1 = graph.add_node_pose_3d(mrob.geometry.SE3(xi))# hay un problem aqui cuando se inicia con el def constructor 16 | # anchor factor 17 | W_0 = np.identity(6) 18 | graph.add_factor_1pose_3d(mrob.geometry.SE3(),n1,1e6*W_0) 19 | #n1 = graph.add_node_pose_3d(mrob.geometry.SE3(), mrob.NODE_ANCHOR) 20 | # non-initialized landmarks, nut they could be initialiazed 21 | l1 = graph.add_node_landmark_3d(np.zeros(3)) 22 | l2 = graph.add_node_landmark_3d(np.zeros(3)) 23 | l3 = graph.add_node_landmark_3d(np.zeros(3)) 24 | print('node pose id = ', n1, ' , node landmark 1 id = ', l1 , ' , node landmark 2 id = ', l2, ' , node landmark 3 id = ', l3) 25 | 26 | 27 | # landmark factors 28 | obs = np.array([1,0,0]) 29 | W = np.identity(3) 30 | graph.add_factor_1pose_1landmark_3d(obs,n1,l1,W) 31 | obs = np.array([1,1,0]) 32 | graph.add_factor_1pose_1landmark_3d(obs,n1,l2,W) 33 | obs = np.array([1,1,1]) 34 | graph.add_factor_1pose_1landmark_3d(obs,n1,l3,W) 35 | 36 | #graph.print(True) 37 | 38 | 39 | print('\n\n\n Solving Fgraph:\n') 40 | #graph.solve(mrob.fgraph.GN) #1 iteration of Gauss-Newton 41 | graph.solve(mrob.LM,10) #as many iterations until convergence for Levenberg Marquardt 42 | graph.print(True) 43 | 44 | 45 | 46 | 47 | # testing matrix 48 | if 0: 49 | import matplotlib.pyplot as plt 50 | L = graph.get_information_matrix() 51 | plt.spy(L, marker='o', markersize=5) 52 | plt.title('Information matrix $\Lambda$') 53 | plt.show() 54 | -------------------------------------------------------------------------------- /python_examples/FGraph_plane4d_example.py: -------------------------------------------------------------------------------- 1 | # 2 | # add path to local library mrob on bash_aliases: "export PYTHONPATH=${PYTHONPATH}:${HOME}/mrob/mrob/lib" 3 | import mrob 4 | import numpy as np 5 | 6 | # create graph 7 | graph = mrob.FGraph() 8 | 9 | #initial point at [0,0,0] with some noise 10 | #n1 = graph.add_node_pose_3d(mrob.geometry.SE3(np.random.randn(6)*0.05)) 11 | 12 | # non-initialized landmarks, but they could be initialiazed 13 | l1 = graph.add_node_plane_4d(np.array([0,1,0,0])) 14 | l2 = graph.add_node_plane_4d(np.array([1,0,0,0])) 15 | l3 = graph.add_node_plane_4d(np.array([1,0,0,0])) 16 | n1 = graph.add_node_pose_3d(mrob.geometry.SE3(np.array([0.0791869, 0.0458932,-0.0127219, -0.085251, 0.0202256, 0.0510333]))) 17 | print('node pose id = ', n1, ' , node landmark 1 id = ', l1 , ' , node landmark 2 id = ', l2, ' , node landmark 3 id = ', l3) 18 | 19 | 20 | # anchor factor 21 | W_0 = np.identity(6) 22 | T_0 = mrob.geometry.SE3(np.array([0,0,0,10,20,0])) 23 | #T_0 = mrob.geometry.SE3(np.random.randn(6)*0.05) 24 | n2 = graph.add_node_pose_3d(mrob.geometry.SE3()) 25 | graph.add_factor_1pose_3d(T_0,n1,1e6*W_0) 26 | W = np.identity(4) 27 | 28 | # plane factors 29 | # ground plane aligned with z 30 | obs = np.array([0,0,1,3]) 31 | graph.add_factor_1pose_1plane_4d(obs,n1,l1,W) 32 | 33 | #Planes should be a 4d vector normalized at |n|=1. Constructor takes care of normalizing. 34 | obs = np.array([1,0,0,1]) 35 | graph.add_factor_1pose_1plane_4d(obs,n1,l2,W) 36 | 37 | # PLane facing Y 38 | obs = np.array([0,-1,0.1,-15]) 39 | graph.add_factor_1pose_1plane_4d(obs,n1,l3,W) 40 | #graph.print(True) 41 | 42 | 43 | 44 | # plane factors at time 2 45 | # ground plane aligned with z, stays the same 46 | obs = np.array([0,0,1,3]) 47 | graph.add_factor_1pose_1plane_4d(obs,n2,l1,W) 48 | 49 | obs = np.array([1,0,0,1]) 50 | graph.add_factor_1pose_1plane_4d(obs,n2,l2,W) 51 | 52 | # PLane facing Y 53 | obs = np.array([0,-1,0.1,-17]) 54 | graph.add_factor_1pose_1plane_4d(obs,n2,l3,W) 55 | #graph.print(True) 56 | 57 | 58 | 59 | print('\n\n\n Solving Fgraph:\n') 60 | #graph.solve(mrob.GN) #1 iteration of Gauss-Newton 61 | graph.solve(mrob.LM) #as many iterations until convergence for Levenberg Marquardt 62 | graph.print(True) 63 | print('Current chi2 = ', graph.chi2() ) # re-evaluates the error, on print it is only the error on evalation before update 64 | 65 | 66 | -------------------------------------------------------------------------------- /python_examples/FGraph_point_plane_align.py: -------------------------------------------------------------------------------- 1 | # 2 | # add path to local library mrob on bashr_rc: "export PYTHONPATH=${PYTHONPATH}:${HOME}/mrob/mrob/lib" 3 | import mrob 4 | import numpy as np 5 | import open3d 6 | import time 7 | 8 | 9 | # generate random data 10 | N_points = 500 11 | N_planes = 3 12 | N_poses = 2 13 | point_noise = 0.05 14 | 15 | 16 | synthetic_points = mrob.registration.CreatePoints(N_points,N_planes,N_poses, point_noise,0.1) 17 | X = synthetic_points.get_point_cloud(1) 18 | ids = synthetic_points.get_point_plane_ids(1) 19 | 20 | T = mrob.geometry.SE3(np.random.randn(6)) 21 | T.print() 22 | Xa = np.array(X) 23 | Y = T.transform_array(X) + np.random.randn(N_points,3)*0.1 24 | 25 | def pcd_1(X, color, T = []): 26 | pcd = open3d.geometry.PointCloud() 27 | pcd.points = open3d.utility.Vector3dVector(X) 28 | if T!=[]: 29 | pcd.transform(T) 30 | pcd.paint_uniform_color(color) 31 | return pcd 32 | 33 | def vis_her(X, Y, T = []): 34 | blue = np.array([0,0,1], dtype='float64') 35 | red = np.array([1,0,0], dtype='float64') 36 | if T!=[]: 37 | open3d.visualization.draw_geometries([pcd_1(X,red,T), pcd_1(Y,blue)]) 38 | else: 39 | open3d.visualization.draw_geometries([pcd_1(X,red), pcd_1(Y,blue)]) 40 | 41 | def vis_arr(X): 42 | pcd = open3d.PointCloud() 43 | pcd.points =open3d.utility.Vector3dVector(X) 44 | pcd.paint_uniform_color(np.random.rand(3,).astype(np.float64)) 45 | open3d.visualization.draw_geometries([pcd]) 46 | 47 | 48 | # initial visualization 49 | vis_her(X,Y) 50 | 51 | # calculate planes 52 | pi = [] 53 | centroids = [] 54 | for p in range(N_planes): 55 | #indexes of the points corresponding to the plane 56 | Yi = [Y[i] for i in range(N_points) if ids[i] == p] 57 | Yi = np.array(Yi) 58 | pi.append( mrob.registration.estimate_normal(Yi)) 59 | centroids.append(mrob.registration.estimate_centroid(Yi)) 60 | print(centroids) 61 | # solve the problem. It finds transformation from the point reference (Y) frame to the plane reference frame (X) 62 | graph = mrob.FGraph() 63 | W = np.array([1]) 64 | print(W) 65 | n1 = graph.add_node_pose_3d(mrob.geometry.SE3()) 66 | for i in range(N_points): 67 | graph.add_factor_1pose_point2plane(z_point_x = X[i], 68 | z_point_y = centroids[ids[i]], #Y[i], 69 | z_normal_y = pi[ids[i]], 70 | nodePoseId = n1, 71 | obsInf = W) 72 | 73 | #graph.print(True) 74 | print('Current state of the graph: chi2 = ' , graph.chi2() ) 75 | start = time.time() 76 | #XXX initial ideal lambda is around 1e-1, vs the default 1e-5, so many more iterations are needed. 77 | graph.solve(mrob.LM, 30) 78 | end = time.time() 79 | print(', chi2 = ', graph.chi2() , ', time on calculation [s] = ', 1e0*(end - start)) 80 | results = graph.get_estimated_state() 81 | 82 | print(results) 83 | Testimated = mrob.geometry.SE3(results[0]) 84 | # initial visualization 85 | print('Error in poses rotation= ', T.distance_rotation(Testimated), ', distance trans = ', T.distance_trans(Testimated)) 86 | vis_her(X,Y,Testimated.T()) 87 | 88 | 89 | 90 | L = graph.get_information_matrix().todense() 91 | D, U = np.linalg.eig(L) 92 | print(D) 93 | print(U) 94 | 95 | 96 | -------------------------------------------------------------------------------- /python_examples/PC_align.py: -------------------------------------------------------------------------------- 1 | # 2 | # add path to local library mrob on bashr_rc: "export PYTHONPATH=${PYTHONPATH}:${HOME}/mrob/mrob/lib" 3 | import mrob 4 | import numpy as np 5 | import open3d 6 | # example equal to ./PC_alignment/examples/example_align.cpp 7 | # generate random data 8 | N = 500 9 | X = np.random.rand(N,3) 10 | T = mrob.geometry.SE3(np.random.rand(6)) 11 | Y = T.transform_array(X) 12 | 13 | print('X = \n', X,'\n T = \n', T.T(),'\n Y =\n', Y) 14 | 15 | 16 | 17 | 18 | def pcd_1(X, color, T = []): 19 | pcd = open3d.geometry.PointCloud() 20 | pcd.points = open3d.utility.Vector3dVector(X) 21 | if T!=[]: 22 | pcd.transform(T) 23 | pcd.paint_uniform_color(color) 24 | return pcd 25 | 26 | def vis_her(X, Y, T = []): 27 | blue = np.array([0,0,1], dtype='float64') 28 | red = np.array([1,0,0], dtype='float64') 29 | if T!=[]: 30 | open3d.visualization.draw_geometries([pcd_1(X,red), pcd_1(Y,blue, T)]) 31 | else: 32 | open3d.visualization.draw_geometries([pcd_1(X,red), pcd_1(Y,blue)]) 33 | 34 | def vis_arr(X): 35 | pcd = open3d.PointCloud() 36 | pcd.points =open3d.utility.Vector3dVector(X) 37 | pcd.paint_uniform_color(np.random.rand(3,).astype(np.float64)) 38 | open3d.visualization.draw_geometries([pcd]) 39 | 40 | 41 | 42 | # solve the problem 43 | vis_her(X,Y) 44 | T_arun = mrob.registration.arun(X,Y) 45 | print('Arun solution =\n', T_arun.T()) 46 | vis_her(Y,X,np.asarray(T_arun.T())) 47 | 48 | W = np.ones(N) 49 | T_wp = mrob.registration.weighted(X,Y,W) 50 | print('Weighted point optimization solution =\n', T_wp.T()) 51 | -------------------------------------------------------------------------------- /python_examples/README.md: -------------------------------------------------------------------------------- 1 | # Python examples using MROB 2 | 3 | First, compile the project as explained in the README. We recommend to use the path ~/soft. Then add the following line to .bashrc or .bash_aliases: 4 | 5 | `echo "export PYTHONPATH=${PYTHONPATH}:${HOME}/soft/mrob/lib" >> ~/.bash_aliases` 6 | 7 | Make sure that the path to lib is correct 8 | 9 | 10 | You can run any of the examples, for instance running the Manhattan 3500 benchmark for 2d Pose slam: 11 | `python3 FGrap_M3500` 12 | 13 | 14 | 15 | ## Other uses 16 | You can write your own code using mrob just following the examples. 17 | ``` 18 | python3 19 | import mrob 20 | T = mrob.geometry.SE3() 21 | ``` 22 | 23 | There are some namespaces dividing the library in utilities, such as *geometry* for transformations. 24 | 25 | You can also use a less verbose way of calling rmob library by specifying loading functions. We encourage the namespace-like style 26 | 27 | ``` 28 | python3 29 | from mrob.geometry import SE3 30 | T = SE3() 31 | ``` 32 | -------------------------------------------------------------------------------- /python_examples/SE3_examples.py: -------------------------------------------------------------------------------- 1 | # 2 | # add path to local library mrob on bashr_rc: "export PYTHONPATH=${PYTHONPATH}:${HOME}/mrob/mrob/lib" 3 | import mrob 4 | import numpy as np 5 | import matplotlib.pyplot as plt 6 | from mpl_toolkits.mplot3d import Axes3D 7 | 8 | 9 | # sets the default fonts on matplotlib to type2 postcript 10 | plt.rcParams['pdf.fonttype'] = 42 11 | plt.rcParams['ps.fonttype'] = 42 12 | 13 | 14 | def plotConfig(): 15 | "configfures the 3d plot structure for representing tranformations" 16 | fig = plt.figure() 17 | ax = fig.add_subplot(111, projection='3d') 18 | return ax 19 | 20 | def plotT(T, ax): 21 | "Plots a 3 axis frame in the origin given the mrob SE3 transformation, right-hand convention" 22 | # transform 3 axis to the coordinate system 23 | x = np.zeros((4,3)) 24 | x[0,:] = T.transform(np.array([0,0,0], dtype='float64')) 25 | x[1,:] = T.transform(np.array([1,0,0], dtype='float64')) 26 | ax.plot(x[[0,1],0],x[[0,1],1],x[[0,1],2],'r') # X axis 27 | x[2,:] = T.transform(np.array([0,1,0], dtype='float64')) 28 | ax.plot(x[[0,2],0],x[[0,2],1],x[[0,2],2],'g') # Y axis 29 | x[3,:] = T.transform(np.array([0,0,1], dtype='float64')) 30 | ax.plot(x[[0,3],0],x[[0,3],1],x[[0,3],2],'b') # Z axis 31 | plt.xlabel('x') 32 | plt.ylabel('y') 33 | 34 | 35 | # Illustration of different orientations 36 | # -------------------------------------------------------- 37 | if 1: 38 | print('Printing orientations') 39 | w = np.zeros(3) 40 | R = mrob.geometry.SO3(w) 41 | R.print() 42 | ax = plotConfig() 43 | T = np.eye(4) 44 | T[:3,:3] = R.R() 45 | plotT(mrob.geometry.SE3(T),ax)# we use this function, but it is equivalent since the translation is set to zero 46 | plt.title('Rotation Identity') 47 | plt.savefig('SO3_1.pdf', bbox_inches='tight') 48 | plt.show() 49 | 50 | 51 | # Rotation on z axis 52 | w[2] = -np.pi/4 53 | R = mrob.geometry.SO3(w) 54 | R.print() 55 | T[:3,:3] = R.R() 56 | ax = plotConfig() 57 | plotT(mrob.geometry.SE3(T),ax) 58 | plt.title('Rotation over the z axis') 59 | plt.savefig('SO3_2.pdf', bbox_inches='tight') 60 | plt.show() 61 | 62 | 63 | 64 | # Rotation on z axis 65 | w = np.random.randn(3) 66 | R = mrob.geometry.SO3(w) 67 | R.print() 68 | T[:3,:3] = R.R() 69 | ax = plotConfig() 70 | plotT(mrob.geometry.SE3(T),ax) 71 | plt.title('Rotation') 72 | plt.savefig('SO3_3.pdf', bbox_inches='tight') 73 | plt.show() 74 | 75 | 76 | # Interpolation TODO: clean code 77 | # -------------------------------------------------------- 78 | if 1: 79 | xi_ini = np.array([0,0,0,0,0,0], dtype='float64') 80 | #xi_fin = np.array([np.pi/3,1,0,0,0,0], dtype='float64') 81 | xi_fin = np.random.rand(6)*10 82 | if np.linalg.norm ( xi_fin[0:3] ) > np.pi: 83 | xi_fin[0:3] = xi_fin[0:3] / np.linalg.norm ( xi_fin[0:3] ) * (np.pi-1e-5) 84 | ax = plotConfig() 85 | N = 20 86 | xi = np.zeros((N,6)) 87 | t = np.zeros(N) 88 | for i in range(6): 89 | xi[:,i] = np.linspace(xi_ini[i],xi_fin[i],N, dtype='float64') 90 | t = np.linspace(0,1,N, dtype='float64') 91 | 92 | 93 | # interpolation in the manifold of se(3)^vee 94 | # and proper interpolation in SE(3) 95 | T_0 = mrob.geometry.SE3(xi_ini) 96 | T_0_inv = T_0.inv() 97 | T_1 = mrob.geometry.SE3(xi_fin) 98 | T_1.print() 99 | print('direct T1\n',T_1.T()) 100 | mrob.geometry.SE3(T_1.T()).print() 101 | #dxi = mrob.geometry.SE3( (T_1.T() @ T_0_inv.T()) ).Ln() 102 | dxi = T_1.mul(T_0_inv).Ln() 103 | for i in range(N): 104 | Ti = mrob.geometry.SE3(xi[i,:]) 105 | plotT(Ti,ax) 106 | #print(Ti.T()) 107 | 108 | Ts = mrob.geometry.SE3(t[i]*dxi).mul(T_0) 109 | #print(Ts.T()) 110 | plotT(Ts,ax) 111 | 112 | # ploting and visualizing error 113 | #print(Ts.T()) 114 | print(np.linalg.norm(Ti.Ln() - Ts.Ln())) 115 | #T.transform(np.array([0,0,0], dtype='float64')) 116 | plt.show() 117 | 118 | # inverse as the negate in the manifold -xi 119 | if 0: 120 | e = np.zeros(1000) 121 | for i in range(1000): 122 | xi = np.random.rand(6)*3 123 | Tr = mrob.geometry.SE3(xi) 124 | T = Tr.inv() 125 | Ti = mrob.geometry.SE3(-xi) 126 | e[i] = np.linalg.norm(T.T()- Ti.T()) 127 | #print( np.linalg.norm(T.T()- Ti.T())) 128 | 129 | print(np.max(e), np.min(e)) 130 | 131 | if 0: 132 | w = np.random.rand(3) 133 | R = mrob.geometry.SO3(w) 134 | print('SO3 matrix: \n', R.R() ) 135 | print('SO3 in the manifold: \n', R.Ln()) 136 | 137 | -------------------------------------------------------------------------------- /scripts/build-wheel-linux.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # 17 | # build-wheel-linux.sh 18 | # 19 | # Created on: Nov 28, 2020 20 | # Author: Lyubov Miloserdova 21 | # miloslubov@gmail.com 22 | # 23 | 24 | set -euo pipefail 25 | export LC_ALL=C 26 | 27 | #Early check for build tools 28 | cmake --version 29 | 30 | cd $(dirname $(readlink -f "${BASH_SOURCE[0]}"))/.. 31 | 32 | mkdir -p ./build ./dist ./mrob 33 | cp ./__init__.py ./mrob/__init__.py 34 | 35 | NUMPROC=$(nproc) 36 | echo "Running $NUMPROC parallel jobs" 37 | 38 | LATEST="" 39 | cd ./build 40 | for PYBIN in /opt/python/cp3*/bin/python 41 | do 42 | LATEST=${PYBIN} 43 | cmake -S .. -B . \ 44 | -DPYTHON_EXECUTABLE:FILEPATH=${PYBIN} \ 45 | -DCMAKE_BUILD_WITH_INSTALL_RPATH=TRUE \ 46 | -DCMAKE_INSTALL_RPATH='$ORIGIN' \ 47 | -DCMAKE_RUNTIME_OUTPUT_DIRECTORY=$PWD/../bin \ 48 | -DCMAKE_LIBRARY_OUTPUT_DIRECTORY=$PWD/../mrob \ 49 | && cmake --build . -j $(nproc) 50 | done 51 | cd ../ 52 | 53 | ${LATEST} -m pip install $([[ -n "$VIRTUAL_ENV" ]] || echo "--user") -q build auditwheel 54 | ${LATEST} -m build --wheel --outdir ./dist/ . 55 | auditwheel repair ./dist/*.whl 56 | 57 | -------------------------------------------------------------------------------- /scripts/build-wheel-macOS.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # 17 | # build-wheel-macOS.sh 18 | # 19 | # Created on: Mar 22, 2021 20 | # Author: Lyubov Miloserdova 21 | # miloslubov@gmail.com 22 | # 23 | 24 | set -euo pipefail 25 | export LC_ALL=C 26 | export MACOSX_DEPLOYMENT_TARGET=10.9 27 | 28 | cd $(dirname $(greadlink -f "${BASH_SOURCE[0]}"))/.. 29 | mkdir -p ./build ./dist ./mrob 30 | 31 | cp ./__init__.py ./mrob/__init__.py 32 | 33 | cd ./build 34 | 35 | NUMPROC=$(sysctl -n hw.ncpu) 36 | echo "Running $NUMPROC parallel jobs" 37 | 38 | for PYBIN in /Users/runner/hostedtoolcache/Python/3.*/x64/bin/python3.* 39 | do 40 | cmake .. -DPYTHON_EXECUTABLE:FILEPATH=$PYBIN \ 41 | -DCMAKE_BUILD_WITH_INSTALL_RPATH=TRUE \ 42 | -DCMAKE_INSTALL_RPATH="@loader_path" \ 43 | -DCMAKE_RUNTIME_OUTPUT_DIRECTORY=$PWD/../bin \ 44 | -DCMAKE_LIBRARY_OUTPUT_DIRECTORY=$PWD/../mrob \ 45 | && cmake --build . -j $NUMPROC 46 | done 47 | 48 | cd ../ 49 | python3 -m pip install --user -q build 50 | python3 -m build --wheel --outdir dist/ . 51 | -------------------------------------------------------------------------------- /scripts/build-wheels-windows.cmd: -------------------------------------------------------------------------------- 1 | ::============================================================================ 2 | :: Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 3 | :: 4 | :: Licensed under the Apache License, Version 2.0 (the "License"); 5 | :: you may not use this file except in compliance with the License. 6 | :: You may obtain a copy of the License at 7 | :: 8 | :: http://www.apache.org/licenses/LICENSE-2.0 9 | :: 10 | :: Unless required by applicable law or agreed to in writing, software 11 | :: distributed under the License is distributed on an "AS IS" BASIS, 12 | :: WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | :: See the License for the specific language governing permissions and 14 | :: limitations under the License. 15 | :: 16 | :: 17 | :: build-wheels-windows.cmd 18 | :: 19 | :: Created on: Sep 17, 2021 20 | :: Author: Lyubov Miloserdova 21 | :: miloslubov@gmail.com 22 | :: 23 | ::============================================================================ 24 | 25 | 26 | SETLOCAL EnableDelayedExpansion 27 | 28 | If "%~1"=="x64" ( 29 | SET plat="x64" || EXIT /B !ERRORLEVEL! 30 | ) else ( 31 | If "%~1"=="x86" ( 32 | SET plat="Win32" || EXIT /B !ERRORLEVEL! 33 | ) else ( 34 | echo Illegal argument: the argument must be set to x64 or x86 35 | EXIT /B 1 36 | ) 37 | ) 38 | 39 | :: Early check for build tools 40 | cmake --version || EXIT /B !ERRORLEVEL! 41 | 42 | pushd %~dp0 43 | cd .. 44 | 45 | mkdir .\build || popd && EXIT /B !ERRORLEVEL! 46 | mkdir .\mrob || popd && EXIT /B !ERRORLEVEL! 47 | copy .\__init__.py .\mrob\__init__.py || popd && EXIT /B !ERRORLEVEL! 48 | 49 | 50 | for /D %%P in (C:\hostedtoolcache\windows\Python\3*) do CALL :build %plat% %%P\%~1\python.exe || popd && EXIT /B !ERRORLEVEL! 51 | 52 | python -m pip install --user -q build || popd && EXIT /B !ERRORLEVEL! 53 | python -m build --wheel --outdir dist . || popd && EXIT /B !ERRORLEVEL! 54 | 55 | popd 56 | EXIT /B 0 57 | 58 | :build 59 | cmake -S . -B build -G "Visual Studio 16 2019" -A "%~1" -DPYTHON_EXECUTABLE:FILEPATH=%~2 -DCMAKE_RUNTIME_OUTPUT_DIRECTORY_RELEASE=%cd%\mrob -DCMAKE_LIBRARY_OUTPUT_DIRECTORY_RELEASE=%cd%\mrob || EXIT /B !ERRORLEVEL! 60 | cmake --build build --config Release -j %NUMBER_OF_PROCESSORS% || EXIT /B !ERRORLEVEL! 61 | EXIT /B 0 62 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # 15 | # 16 | # setup.cfg 17 | # 18 | # Created on: Oct 31, 2020 19 | # Author: Lyubov Miloserdova 20 | # miloslubov@gmail.com 21 | # 22 | 23 | [metadata] 24 | name = mrob 25 | home-page = https://github.com/MobileRoboticsSkoltech/mrob 26 | author = Gonzalo Ferrer 27 | author_email = G.Ferrer@skoltech.ru 28 | url = https://github.com/MobileRoboticsSkoltech/mrob 29 | download_url = https://github.com/MobileRoboticsSkoltech/mrob 30 | description = The Skoltech Mobile Robotics library (mrob) is a framework for implementing robotics research and projects. 31 | platforms = any 32 | maintainer = Gonzalo Ferrer 33 | maintainer_email = G.Ferrer@skoltech.ru 34 | long-description = file: README.md 35 | long_description_content_type = text/markdown 36 | license = Apache-2.0 37 | license_files = LICENSE 38 | 39 | [options] 40 | zip_safe = false 41 | include_package_data = true 42 | python_requires = >= 3.5 43 | packages = find: 44 | 45 | [options.package_data] 46 | * = 47 | *.so 48 | *.dylib 49 | *.dll 50 | *.pyd 51 | 52 | [sdist] 53 | formats = zip, gztar 54 | 55 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # 15 | # 16 | # setup.py 17 | # 18 | # Created on: Jan 22, 2020 19 | # Author: Lyubov Miloserdova 20 | # miloslubov@gmail.com 21 | # 22 | 23 | 24 | import setuptools 25 | 26 | 27 | try: 28 | from wheel.bdist_wheel import bdist_wheel as _bdist_wheel 29 | import platform, os, ctypes 30 | 31 | class bdist_wheel(_bdist_wheel): 32 | 33 | def finalize_options(self): 34 | _bdist_wheel.finalize_options(self) 35 | if platform.system() == "Darwin": 36 | self.root_is_pure = False 37 | 38 | def get_tag(self): 39 | python, abi, plat = _bdist_wheel.get_tag(self) 40 | if platform.system() == "Darwin": 41 | python, abi = 'py3', 'none' 42 | name = plat[:plat.find("_")] 43 | for i in range(3): 44 | plat = plat[plat.find("_") + 1:] # skip name and version of OS 45 | arch = plat 46 | version = os.getenv('MACOSX_DEPLOYMENT_TARGET').replace('.', '_') 47 | plat = name + "_" + version + "_" + arch 48 | elif platform.system() == "Windows": 49 | if ctypes.sizeof(ctypes.c_voidp) * 8 > 32: 50 | plat = "win_" + platform.machine().lower() 51 | else: 52 | plat = "win32" 53 | return python, abi, plat 54 | 55 | except ImportError: 56 | bdist_wheel = None 57 | 58 | setuptools.setup( 59 | version_config={ 60 | "dev_template": "{tag}.{branch}.post{ccount}+git.{sha}", 61 | "dirty_template": "{tag}.{branch}.post{ccount}+git.{sha}.dirty", 62 | }, 63 | setup_requires=['setuptools-git-versioning'], 64 | cmdclass={'bdist_wheel': bdist_wheel} 65 | ) 66 | -------------------------------------------------------------------------------- /source/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # 3 | # This file only contains a selection of the most common options. For a full 4 | # list see the documentation: 5 | # https://www.sphinx-doc.org/en/master/usage/configuration.html 6 | 7 | # -- Path setup -------------------------------------------------------------- 8 | 9 | # If extensions (or modules to document with autodoc) are in another directory, 10 | # add these directories to sys.path here. If the directory is relative to the 11 | # documentation root, use os.path.abspath to make it absolute, like shown here. 12 | # 13 | # import os 14 | # import sys 15 | # sys.path.insert(0, os.path.abspath('.')) 16 | 17 | import subprocess 18 | subprocess.call('cd .. ; doxygen ./doc/doxyfile.conf', shell=True) 19 | 20 | # -- Project information ----------------------------------------------------- 21 | 22 | project = 'mrob' 23 | copyright = '2021, Gonzalo Ferrer' 24 | author = 'Gonzalo Ferrer' 25 | 26 | 27 | # -- General configuration --------------------------------------------------- 28 | 29 | # Add any Sphinx extension module names here, as strings. They can be 30 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 31 | # ones. 32 | extensions = [ 33 | ] 34 | 35 | # Add any paths that contain templates here, relative to this directory. 36 | templates_path = ['_templates'] 37 | 38 | # List of patterns, relative to source directory, that match files and 39 | # directories to ignore when looking for source files. 40 | # This pattern also affects html_static_path and html_extra_path. 41 | exclude_patterns = [] 42 | 43 | 44 | # -- Options for HTML output ------------------------------------------------- 45 | 46 | # The theme to use for HTML and HTML Help pages. See the documentation for 47 | # a list of builtin themes. 48 | # 49 | html_theme = 'alabaster' 50 | 51 | # Add any paths that contain custom static files (such as style sheets) here, 52 | # relative to this directory. They are copied after the builtin static files, 53 | # so a file named "default.css" will overwrite the builtin "default.css". 54 | html_static_path = ['_static'] 55 | 56 | # Where to find doxygen documentation 57 | html_extra_path = ['../doc/html'] 58 | -------------------------------------------------------------------------------- /source/index.rst: -------------------------------------------------------------------------------- 1 | .. mrob documentation master file, created by 2 | sphinx-quickstart on Thu Jul 15 20:29:57 2021. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Welcome to mrob's documentation! 7 | ================================ 8 | 9 | .. toctree:: 10 | :maxdepth: 2 11 | :caption: Contents: 12 | 13 | 14 | 15 | Indices and tables 16 | ================== 17 | 18 | * :ref:`genindex` 19 | * :ref:`modindex` 20 | * :ref:`search` 21 | -------------------------------------------------------------------------------- /src/EigenFactors/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # locate the necessary dependencies, if any 2 | 3 | # extra header files 4 | SET(headers 5 | mrob/plane_factor.hpp 6 | ) 7 | 8 | # extra source files 9 | SET(sources 10 | plane_factor.cpp 11 | ) 12 | 13 | # create the shared library 14 | ADD_LIBRARY(EigenFactors SHARED ${sources}) 15 | TARGET_LINK_LIBRARIES(EigenFactors FGraph SE3) 16 | 17 | 18 | ADD_SUBDIRECTORY(examples) 19 | #ADD_SUBDIRECTORY(tests) 20 | -------------------------------------------------------------------------------- /src/EigenFactors/README.md: -------------------------------------------------------------------------------- 1 | # Eigen-Factors 2 | Directory that *will* containt the EF implementation. Right now, the working version is part of the PCRegistration for limited trajectories. 3 | -------------------------------------------------------------------------------- /src/EigenFactors/examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_EXECUTABLE(test_EF_Hessian test_EF_Hessian.cpp) 2 | TARGET_LINK_LIBRARIES(test_EF_Hessian SE3) 3 | ADD_EXECUTABLE(example_EF_1_pose example_solve_1_poses.cpp) 4 | TARGET_LINK_LIBRARIES(example_EF_1_pose EigenFactors) -------------------------------------------------------------------------------- /src/EigenFactors/examples/example_solve_1_poses.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * example_solve_1_poses.cpp 17 | * 18 | * Created on: Oct 23, 2019 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab. 22 | */ 23 | 24 | 25 | /** This code is a toy example to show how to use the basics for plane alignment. 26 | * For a more realistic example take a look at the python example 27 | */ 28 | 29 | 30 | #include "mrob/factor_graph_ef.hpp" 31 | 32 | 33 | using namespace mrob; 34 | 35 | // This examples show how to optimize 1 pose of a stream of planes 36 | 37 | int main() 38 | { 39 | // 1) create mock scene with some planes and GT poses 40 | // 2) Add information to the EF Fgraph structure. For now only planes 41 | // 3) Optimize, Measure distance to GT 42 | } 43 | -------------------------------------------------------------------------------- /src/EigenFactors/examples/test_EF_Hessian.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * test_EF_Hessian.cpp 17 | * 18 | * Created on: Aug 15, 2019 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab. 22 | */ 23 | 24 | 25 | #include 26 | 27 | #include "mrob/SE3.hpp" 28 | 29 | 30 | using namespace mrob; 31 | 32 | // This test will output the numerical derivatives and the calculated derivative 33 | int main() 34 | { 35 | // create transformation 36 | Mat61 x = Mat61::Zero();//Approximations only work well around the Identity, thus x = 0 37 | SE3 T(x); 38 | //std::cout << "Initial random transformation = \n" << x << std::endl; 39 | //T.print(); 40 | 41 | 42 | // gnerative basis in the manifold 43 | Mat6 G = Mat6::Identity(); 44 | 45 | //std::cout << "Generative basis\n" << G1 << G2 << G3 << G4 << G5 << G6 << std::endl; 46 | 47 | // calculate numerical derivatives d Exp / Dxi = d/dxi (Exp(x+e*Gi)-Exp(x) )/ eps, 48 | // where Gi are the generative basis, and x usually is zero (transformation around the Identy) 49 | double eps = 1e-4; 50 | if (0) 51 | { 52 | T = SE3(x); 53 | Mat61 dx = x + eps*G.col(2); 54 | SE3 Teps = SE3(dx); 55 | std::cout << "Initial random transformation = \n" << std::endl; 56 | T.print_lie(); 57 | std::cout << "dt lie algebra params = \n" << std::endl; 58 | Teps.print_lie(); 59 | std::cout << (Teps.T() - T.T())/eps << std::endl; 60 | } 61 | 62 | 63 | // Numerical derivates not around the identity 64 | // dT/dx = Gi + 0.5 65 | if (1) 66 | { 67 | std::cout << "\n\n Derivatives around a random element \n" << std::endl; 68 | x = Mat61::Random() * 0.1; 69 | T = SE3(x); 70 | uint_t i = 1; 71 | Mat61 dx = x+eps*G.col(i); 72 | SE3 Teps = SE3(dx); 73 | //std::cout << "Gi hat x\n"<< hat6(G.col(i)) * hat6(x) << "\n and x^ Gi \n"<< hat6(x) * hat6(G.col(i)) << std::endl; 74 | Mat4 derivative = (Teps.T() - T.T())/eps; 75 | Mat4 Gi = hat6(G.col(i)) , xhat = hat6(x); 76 | Mat4 analytical = Gi + 0.5*Gi * xhat + 0.5 * xhat * Gi; 77 | std::cout << derivative << "\n vs analytical =\n" << analytical << std::endl; 78 | } 79 | 80 | 81 | 82 | // calculate numerical second order derivatives 83 | if (1) 84 | { 85 | std::cout << "\n\n Second order derivatives around a the identity\n" << std::endl; 86 | //x = Mat61::Random() * 0.1; 87 | x = Mat61::Zero(); 88 | T = SE3(x); 89 | uint_t i = 0, j = 2; 90 | Mat61 dxi = x + eps*G.col(i); 91 | Mat61 dxj = x + eps*G.col(j); 92 | Mat61 dxij = x + eps*G.col(i) + eps*G.col(j); 93 | SE3 Ti(dxi), Tj(dxj), Tij(dxij); 94 | Mat4 derivative = (Tij.T() - Ti.T() - Tj.T() + T.T() )/eps/eps; 95 | Mat4 Gi = hat6(G.col(i)), Gj = hat6(G.col(j)); 96 | Mat4 analytical = 0.5*Gi * Gj + 0.5 * Gj * Gi; 97 | std::cout << derivative << "\n vs analytical =\n" << analytical << std::endl; 98 | 99 | } 100 | 101 | } 102 | -------------------------------------------------------------------------------- /src/EigenFactors/mrob/factor_graph_ef.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * factor_graph_ef.hpp 17 | * 18 | * Created on: Sep 27, 2019 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab. 22 | */ 23 | 24 | #ifndef FACTOR_GRAPH_EF_HPP_ 25 | #define FACTOR_GRAPH_EF_HPP_ 26 | 27 | #include "mrob/factor_graph_solve.hpp" 28 | 29 | /** 30 | * This class, inherits from solve_factor_graph and aims to provide the 31 | * additional structure required to process Eigen Factors as plane factors 32 | * 33 | * TODO: merge this in solve 34 | */ 35 | 36 | 37 | namespace mrob { 38 | 39 | class EFSolve: public FGraphSolve 40 | { 41 | public: 42 | EFSolve(); 43 | ~EFSolve(); 44 | 45 | /** 46 | * Solve the alignment problem for a stream of observations 47 | * from pose x_origin = I to x_f 48 | * w.r.t the final pose 49 | */ 50 | void solve_1_pose(); 51 | /** 52 | * Solve the full problem involving multiple poses 53 | */ 54 | void solve_planes(); 55 | }; 56 | 57 | }//end namespace 58 | 59 | #endif /* FACTOR_GRAPH_EF_HPP_ */ 60 | -------------------------------------------------------------------------------- /src/FGraph/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # locate the necessary dependencies, if any 2 | 3 | # extra header files 4 | SET(headers 5 | mrob/node.hpp 6 | mrob/factor.hpp 7 | mrob/factor_graph.hpp 8 | mrob/factor_graph_solve.hpp 9 | mrob/factor_graph_solve_dense.hpp 10 | ) 11 | 12 | # extra source files 13 | SET(sources 14 | node.cpp 15 | factor.cpp 16 | factor_graph.cpp 17 | factor_graph_solve.cpp 18 | factor_graph_solve_dense.cpp 19 | ) 20 | 21 | SET(factors_headers 22 | mrob/factors/nodePose3d.hpp 23 | mrob/factors/factor2Poses3d.hpp 24 | mrob/factors/factor1Pose3d.hpp 25 | mrob/factors/nodePose2d.hpp 26 | mrob/factors/factor1Pose2d.hpp 27 | mrob/factors/factor2Poses2d.hpp 28 | mrob/factors/nodeLandmark2d.hpp 29 | mrob/factors/nodeLandmark3d.hpp 30 | mrob/factors/factor1Pose1Landmark2d.hpp 31 | mrob/factors/factor1Pose1Landmark3d.hpp 32 | mrob/factors/nodePlane4d.hpp 33 | mrob/factors/factor1Pose1plane4d.hpp 34 | ) 35 | 36 | SET(factors_sources 37 | factors/nodePose3d.cpp 38 | factors/factor2Poses3d.cpp 39 | factors/factor1Pose3d.cpp 40 | factors/nodePose2d.cpp 41 | factors/factor1Pose2d.cpp 42 | factors/factor2Poses2d.cpp 43 | factors/nodeLandmark2d.cpp 44 | factors/nodeLandmark3d.cpp 45 | factors/factor1Pose1Landmark2d.cpp 46 | factors/factor1Pose1Landmark3d.cpp 47 | factors/nodePlane4d.cpp 48 | factors/factor1Pose1Plane4d.cpp 49 | ) 50 | 51 | # create the shared library 52 | ADD_LIBRARY(FGraph SHARED ${sources} ${factors_sources}) 53 | TARGET_LINK_LIBRARIES(FGraph SE3 common) 54 | 55 | 56 | ADD_SUBDIRECTORY(examples) 57 | #ADD_SUBDIRECTORY(tests) 58 | -------------------------------------------------------------------------------- /src/FGraph/README.md: -------------------------------------------------------------------------------- 1 | # FGraph 2 | Factor Graph library is an implementation to solve the inference problem given by the joint probability P(x,u,z).The solution to this joint probability is equivalent to a Nonlinear Least Squares (NLSQ) problem. 3 | To address this inference problem, we will use a graphical model, Factor Graphs. 4 | Factor Graphs are bipartite graphs, meaning that we express the relations from a set of vertices "nodes" which include our state variables through a set of vertices "factors", capturing the inherent distribution of the nodes variables due to observations. 5 | Bipartite is in the sense that edges of the graph are always from nodes to factors or vice versa. We require two abstract classes, 6 | * Class Node 7 | * Class Factor. (see factor.hpp for the conventions on residuals, observations, etc.) 8 | 9 | In general, the problem we target is of the form: 10 | 11 | C = sum ||f(x) - z||^2_W, where f(x) is a non-linear observation function and z is an observation. 12 | After linearizontion and staking the output of the sum into a matrix, the problem becomes 13 | C = 0.5||J dx - (z - f(x)) ||^2 = 0.5||J dx - r ||^2 14 | 15 | This library has been designed to interface with python. Please see mrobpy/examples 16 | 17 | 18 | ## Dependencies 19 | C++'14, Eigen 20 | 21 | -------------------------------------------------------------------------------- /src/FGraph/examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #ADD_EXECUTABLE(example_FGraph_2d_example example_solver_2d.cpp) 2 | #TARGET_LINK_LIBRARIES(example_FGraph_2d_example FGraph) 3 | 4 | ADD_EXECUTABLE(example_FGraph_3d_example example_solver_3d.cpp) 5 | TARGET_LINK_LIBRARIES(example_FGraph_3d_example FGraph) 6 | 7 | ADD_EXECUTABLE(example_FGraph_3d_ladmark_example example_solver_3d_landmarks.cpp) 8 | TARGET_LINK_LIBRARIES(example_FGraph_3d_ladmark_example FGraph) 9 | 10 | ADD_EXECUTABLE(example_FGraph_dense_3d example_solver_dense_3d.cpp) 11 | TARGET_LINK_LIBRARIES(example_FGraph_dense_3d FGraph common) -------------------------------------------------------------------------------- /src/FGraph/examples/example_solver_2d.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * example_solver.cpp 17 | * 18 | * Created on: April 10, 2019 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | 25 | 26 | 27 | 28 | #include "mrob/factor_graph_solve.hpp" 29 | #include "mrob/factors/factor1Pose2d.hpp" 30 | #include "mrob/factors/factor2Poses2d.hpp" 31 | #include "mrob/factors/nodePose2d.hpp" 32 | 33 | 34 | #include 35 | 36 | int main () 37 | { 38 | 39 | // create a simple graph to solve: 40 | // anchor ------ X1 ------- obs ---------- X2 41 | mrob::FGraphSolve graph(mrob::FGraphSolve::ADJ); 42 | 43 | // Initial node is defined at 0,0,0, and anchor factor actually observing it at 0 44 | mrob::Mat31 x, obs; 45 | x = mrob::Mat31::Random()*0.1; 46 | obs = mrob::Mat31::Zero(); 47 | // Nodes and factors are added to the graph using polymorphism. That is why 48 | // we need to declare here what specific kind of nodes or factors we use 49 | // while the definition is an abstract class (Node or Factor) 50 | std::shared_ptr n1(new mrob::NodePose2d(x)); 51 | graph.add_node(n1); 52 | Mat3 obsInformation= Mat3::Identity(); 53 | std::shared_ptr f1(new mrob::Factor1Pose2d(obs,n1,obsInformation*1e6)); 54 | graph.add_factor(f1); 55 | 56 | 57 | 58 | // Node 2, initialized at 0,0,0 59 | if (0){ 60 | std::shared_ptr n2(new mrob::NodePose2d(x)); 61 | graph.add_node(n2); 62 | 63 | // Add odom factor = [drot1, dtrans, drot2] 64 | obs << 0, 1, 0; 65 | //obs << M_PI_2, 0.5, 0; 66 | // this factor assumes that the current value of n2 (node destination) is updated according to obs 67 | std::shared_ptr f2(new mrob::Factor2Poses2dOdom(obs,n1,n2,obsInformation)); 68 | graph.add_factor(f2); 69 | 70 | obs << -1 , -1 , 0; 71 | std::shared_ptr f3(new mrob::Factor2Poses2d(obs,n2,n1,obsInformation)); 72 | graph.add_factor(f3); 73 | } 74 | 75 | 76 | 77 | // solve the Gauss Newton optimization 78 | graph.print(true); 79 | graph.solve(mrob::FGraphSolve::LM); 80 | 81 | std::cout << "\nSolved, chi2 = " << graph.chi2() << std::endl; 82 | 83 | graph.print(true); 84 | return 0; 85 | } 86 | -------------------------------------------------------------------------------- /src/FGraph/examples/example_solver_3d.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * example_solver_3d.cpp 17 | * 18 | * Created on: May 20, 2019 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | 25 | 26 | 27 | 28 | #include "mrob/factor_graph_solve.hpp" 29 | #include "mrob/factors/nodePose3d.hpp" 30 | #include "mrob/factors/factor1Pose3d.hpp" 31 | #include "mrob/factors/factor2Poses3d.hpp" 32 | 33 | 34 | #include 35 | 36 | int main () 37 | { 38 | 39 | // create a simple graph to solve: 40 | // anchor ------ X1 ------- obs ---------- X2 41 | mrob::FGraphSolve graph(mrob::FGraphSolve::ADJ); 42 | 43 | // Initial node is defined at 0,0,0, 0,0,0 and anchor factor actually observing it at 0 44 | mrob::Mat61 obs; 45 | mrob::Mat6 obsInformation = mrob::Mat6::Identity()*1e6; 46 | mrob::Mat61 x = mrob::Mat61::Random()*0.05; 47 | mrob::SE3 Tobs; 48 | mrob::SE3 Tx(x); 49 | std::shared_ptr n0(new mrob::NodePose3d(Tx)); 50 | graph.add_node(n0); 51 | std::shared_ptr f0(new mrob::Factor1Pose3d(Tobs,n0,obsInformation)); 52 | graph.add_factor(f0); 53 | 54 | // Add a small chain 55 | std::shared_ptr n1(new mrob::NodePose3d(Tx));// it will be later updated by the factor 56 | graph.add_node(n1); 57 | obs << -0.1,0.2,0.5, 1,-2 ,3; 58 | Tobs = mrob::SE3(obs); 59 | obsInformation= mrob::Mat6::Identity(); 60 | std::shared_ptr f1(new mrob::Factor2Poses3d(Tobs,n0,n1,obsInformation, true)); 61 | graph.add_factor(f1); 62 | 63 | // Add a small chain 64 | std::shared_ptr n2(new mrob::NodePose3d(Tx));// it will be later updated by the factor 65 | graph.add_node(n2); 66 | std::shared_ptr f2(new mrob::Factor2Poses3d(Tobs,n1,n2,obsInformation, true)); 67 | graph.add_factor(f2); 68 | 69 | // Closing the loop 70 | obs *= 2.0; 71 | std::shared_ptr f3(new mrob::Factor2Poses3d(mrob::SE3(obs),n0,n2,obsInformation*1e2, true)); 72 | graph.add_factor(f3); 73 | 74 | 75 | // solve the Gauss Newton optimization 76 | graph.print(true); 77 | graph.solve(); 78 | 79 | graph.print(true); 80 | std::cout << "\n\n\nSolved, chi2 = " << graph.chi2() << std::endl; 81 | return 0; 82 | } 83 | -------------------------------------------------------------------------------- /src/FGraph/examples/example_solver_3d_landmarks.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * example_solver_3d_landmarks.cpp 17 | * 18 | * Created on: March 21, 2020 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | 25 | 26 | 27 | 28 | #include "mrob/factors/nodePose3d.hpp" 29 | #include "mrob/factors/nodeLandmark3d.hpp" 30 | #include "mrob/factors/factor1Pose3d.hpp" 31 | #include "mrob/factors/factor1Pose1Landmark3d.hpp" 32 | #include "mrob/factor_graph_solve.hpp" 33 | 34 | 35 | #include 36 | 37 | int main () 38 | { 39 | 40 | // create a simple graph to solve: 41 | // anchor ---- X1 ------- obs ---------- L1,2,3 42 | mrob::FGraphSolve graph(mrob::FGraphSolve::ADJ); 43 | 44 | // Initial node is defined at 0,0,0, 0,0,0 and anchor factor actually observing it at 0 45 | mrob::Mat61 x, obs; 46 | x = mrob::Mat61::Random()*0.05; 47 | mrob::SE3 Tx(x); 48 | std::shared_ptr n0(new mrob::NodePose3d(Tx)); 49 | graph.add_node(n0); 50 | mrob::Mat4 Tobs = mrob::Mat4::Identity(); 51 | mrob::Mat6 obsInformation = mrob::Mat6::Identity(); 52 | std::shared_ptr f0(new mrob::Factor1Pose3d(Tobs,n0,obsInformation*1e6)); 53 | graph.add_factor(f0); 54 | 55 | // Add Ladmarks: uninitialized 56 | mrob::Mat31 land; 57 | land << 0,0,0; 58 | std::shared_ptr l1(new mrob::NodeLandmark3d(land));// it will be later updated by the factor 59 | graph.add_node(l1); 60 | std::shared_ptr l2(new mrob::NodeLandmark3d(land));// it will be later updated by the factor 61 | graph.add_node(l2); 62 | std::shared_ptr l3(new mrob::NodeLandmark3d(land));// it will be later updated by the factor 63 | graph.add_node(l3); 64 | 65 | // reuse of variable, now this is the observation of the landmark 66 | mrob::Mat3 landInf = mrob::Mat3::Identity(); 67 | land << 1,0,0; 68 | std::shared_ptr f1(new mrob::Factor1Pose1Landmark3d(land,n0,l1,landInf)); 69 | graph.add_factor(f1); 70 | 71 | land << 1,1,0; 72 | std::shared_ptr f2(new mrob::Factor1Pose1Landmark3d(land,n0,l2,landInf)); 73 | graph.add_factor(f2); 74 | 75 | land << 1,1,1; 76 | std::shared_ptr f3(new mrob::Factor1Pose1Landmark3d(land,n0,l3,landInf)); 77 | graph.add_factor(f3); 78 | 79 | // solve the Gauss Newton optimization 80 | graph.print(true); 81 | graph.solve(mrob::FGraphSolve::LM); 82 | 83 | std::cout << "\n\n\nSolved, chi2 = " << graph.chi2() << std::endl; 84 | graph.print(true); 85 | return 0; 86 | } 87 | -------------------------------------------------------------------------------- /src/FGraph/examples/example_solver_dense_3d.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * example_solver_3d.cpp 17 | * 18 | * Created on: Nov 10, 2020 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | 25 | 26 | 27 | 28 | #include "mrob/factor_graph_solve_dense.hpp" 29 | #include "mrob/factors/nodePose3d.hpp" 30 | #include "mrob/factors/factor1Pose3d.hpp" 31 | #include "mrob/factors/factor2Poses3d.hpp" 32 | 33 | 34 | #include 35 | 36 | int main () 37 | { 38 | 39 | // create a simple graph to solve: 40 | // anchor ------ X1 ------- obs ---------- X2 41 | mrob::FGraphSolveDense graph; 42 | 43 | // Initial node is defined at 0,0,0, 0,0,0 and anchor factor actually observing it at 0 44 | mrob::Mat61 obs; 45 | mrob::Mat6 obsInformation= mrob::Mat6::Identity()*1e6; 46 | mrob::Mat61 x = mrob::Mat61::Random()*0.05; 47 | mrob::SE3 Tobs; 48 | mrob::SE3 Tx(x); 49 | std::shared_ptr n0(new mrob::NodePose3d(Tx)); 50 | graph.add_node(n0); 51 | std::shared_ptr f0(new mrob::Factor1Pose3d(Tobs,n0,obsInformation)); 52 | graph.add_factor(f0); 53 | 54 | // Add a small chain 55 | std::shared_ptr n1(new mrob::NodePose3d(Tx));// it will be later updated by the factor 56 | graph.add_node(n1); 57 | obs << -0.1,0.2,0.5, 1,-2 ,3; 58 | Tobs = mrob::SE3(obs); 59 | obsInformation= mrob::Mat6::Identity(); 60 | std::shared_ptr f1(new mrob::Factor2Poses3d(Tobs,n0,n1,obsInformation, true)); 61 | graph.add_factor(f1); 62 | 63 | // Add a small chain 64 | std::shared_ptr n2(new mrob::NodePose3d(Tx));// it will be later updated by the factor 65 | graph.add_node(n2); 66 | std::shared_ptr f2(new mrob::Factor2Poses3d(Tobs,n1,n2,obsInformation, true)); 67 | graph.add_factor(f2); 68 | 69 | // Closing the loop 70 | obs *= 2.0; 71 | std::shared_ptr f3(new mrob::Factor2Poses3d(mrob::SE3(obs),n0,n2,obsInformation*1e2, true)); 72 | graph.add_factor(f3); 73 | 74 | 75 | // solve the Gauss Newton optimization 76 | graph.print(true); 77 | graph.solve(mrob::Optimizer::NEWTON_RAPHSON); 78 | 79 | graph.print(true); 80 | std::cout << "\n\n\nSolved, chi2 = " << graph.calculate_error() << std::endl; 81 | return 0; 82 | } 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /src/FGraph/factor.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * factor.cpp 17 | * 18 | * Created on: Feb 27, 2018 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | 25 | #include "mrob/factor.hpp" 26 | 27 | using namespace mrob; 28 | 29 | Factor::Factor(uint_t dim, uint_t allNodesDim, robustFactorType factor_type, uint_t potNumberNodes): 30 | id_(0), dim_(dim), allNodesDim_(allNodesDim), chi2_(0), robust_type_(factor_type), robust_weight_(0.0) 31 | { 32 | // Child factor must specify dimensions 33 | neighbourNodes_.reserve( potNumberNodes ); 34 | } 35 | 36 | Factor::~Factor() 37 | { 38 | neighbourNodes_.clear(); 39 | } 40 | 41 | 42 | matData_t Factor::evaluate_robust_weight(matData_t u, matData_t params) 43 | { 44 | switch(robust_type_) 45 | { 46 | case HUBER: 47 | // p(u) = 1/2u^2 if u < d 48 | // d(u-1/2d) 49 | if (u < 1.0) //XXX should this be set param? 50 | { 51 | robust_weight_ = 1.0; 52 | } 53 | else 54 | { 55 | robust_weight_ = 1.0/u; 56 | } 57 | break; 58 | case CAUCHY: 59 | robust_weight_ = 1.0/(1+u*u); 60 | break; 61 | case MCCLURE: 62 | params = 1+u*u; 63 | robust_weight_ = 1.0/params/params; 64 | break; 65 | case RANSAC: 66 | break; 67 | case QUADRATIC: 68 | default: 69 | robust_weight_ = 1.0; 70 | break; 71 | } 72 | return robust_weight_; 73 | } 74 | -------------------------------------------------------------------------------- /src/FGraph/factor_graph.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * factor_graph.cpp 17 | * 18 | * Created on: Feb 12, 2018 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #include 25 | #include 26 | 27 | 28 | using namespace mrob; 29 | 30 | FGraph::FGraph() : 31 | stateDim_(0),obsDim_(0) 32 | { 33 | } 34 | FGraph::~FGraph() 35 | { 36 | // clear every node's list of neigbours factors 37 | for (auto &&n: nodes_) 38 | n->clear(); 39 | factors_.clear(); 40 | nodes_.clear(); 41 | eigen_factors_.clear(); 42 | } 43 | 44 | factor_id_t FGraph::add_factor(std::shared_ptr &factor) 45 | { 46 | factor->set_id(factors_.size()); 47 | factors_.push_back(factor); 48 | auto *list = factor->get_neighbour_nodes(); 49 | for( auto &&n: *list) 50 | { 51 | n->add_factor(factor); 52 | } 53 | obsDim_ += factor->get_dim(); 54 | return factor->get_id(); 55 | } 56 | 57 | factor_id_t FGraph::add_eigen_factor(std::shared_ptr &factor) 58 | { 59 | factor->set_id(eigen_factors_.size()); 60 | eigen_factors_.push_back(factor); 61 | auto *list = factor->get_neighbour_nodes(); 62 | for( auto &&n: *list) 63 | { 64 | n->add_factor(factor); 65 | } 66 | obsDim_ += factor->get_dim(); 67 | return factor->get_id(); 68 | } 69 | 70 | 71 | factor_id_t FGraph::add_node(std::shared_ptr &node) 72 | { 73 | node->set_id(nodes_.size()); 74 | nodes_.push_back(node); 75 | switch(node->get_node_mode()) 76 | { 77 | case Node::nodeMode::STANDARD: 78 | active_nodes_.push_back(node); 79 | stateDim_ += node->get_dim(); 80 | break; 81 | case Node::nodeMode::ANCHOR: 82 | break; 83 | case Node::nodeMode::SCHUR_MARGI: 84 | assert(0 && "add_node::SCHUR_MARGI: Functionality not programmed yey"); 85 | break; 86 | } 87 | return node->get_id(); 88 | } 89 | 90 | std::shared_ptr& FGraph::get_node(factor_id_t key) 91 | { 92 | // TODO key on a set or map? 93 | assert(key < nodes_.size() && "FGraph::get_node: incorrect key"); 94 | return nodes_[key];// XXX test key again 95 | } 96 | 97 | std::shared_ptr& FGraph::get_factor(factor_id_t key) 98 | { 99 | // TODO key on a set or map? 100 | assert(key < factors_.size() && "FGraph::get_node: incorrect key"); 101 | return factors_[key]; 102 | } 103 | 104 | 105 | void FGraph::print(bool completePrint) const 106 | { 107 | std::cout << "Status of graph: " << 108 | nodes_.size() << "Nodes and " << 109 | factors_.size() << "Factors." << std::endl; 110 | 111 | if(completePrint) 112 | { 113 | for (auto &&n : nodes_) 114 | n->print(); 115 | for (auto &&f : factors_) 116 | f->print(); 117 | } 118 | } 119 | -------------------------------------------------------------------------------- /src/FGraph/factors/factor1Pose1Landmark2d.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * factor1Pose1Landmark2d.cpp 17 | * 18 | * Created on: Jul 27, 2020 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab. 22 | */ 23 | 24 | 25 | #include "mrob/factors/factor1Pose1Landmark2d.hpp" 26 | 27 | #include 28 | 29 | using namespace mrob; 30 | 31 | 32 | 33 | Factor1Pose1Landmark2d::Factor1Pose1Landmark2d(const Mat21 &observation, std::shared_ptr &nodePose, 34 | std::shared_ptr &nodeLandmark, const Mat2 &obsInf, bool initializeLandmark, 35 | Factor::robustFactorType robust_type): 36 | Factor(2,5, robust_type), obs_(observation), r_(Mat21::Zero()),landmark_(Mat21::Zero()), 37 | state_(Mat31::Zero()),dx_(0.0), dy_(0.0), q_(0.0), 38 | W_(obsInf), reversedNodeOrder_(false) 39 | { 40 | // chek for order, we need to ensure id_0 < id_1 41 | if (nodePose->get_id() < nodeLandmark->get_id()) 42 | { 43 | neighbourNodes_.push_back(nodePose); 44 | neighbourNodes_.push_back(nodeLandmark); 45 | } 46 | else 47 | { 48 | neighbourNodes_.push_back(nodeLandmark); 49 | neighbourNodes_.push_back(nodePose); 50 | // set reverse mode 51 | reversedNodeOrder_ = true; 52 | } 53 | 54 | if (initializeLandmark) 55 | { 56 | Mat31 x = nodePose->get_state(); 57 | Mat21 dl; 58 | dl << std::cos(obs_(1)+x(2)), std::sin(obs_(1)+x(2)); 59 | Mat21 land = x.head(2) + obs_(0)*dl; 60 | nodeLandmark->set_state(land); 61 | } 62 | } 63 | 64 | 65 | void Factor1Pose1Landmark2d::evaluate_residuals() 66 | { 67 | // check for order of pose landmark, it matter for Jacobian calculation 68 | uint_t poseIndex = 0; 69 | uint_t landmarkIndex = 1; 70 | if (reversedNodeOrder_) 71 | { 72 | landmarkIndex = 0; 73 | poseIndex = 1; 74 | } 75 | // From the local frame we observe the landmark 76 | state_ = get_neighbour_nodes()->at(poseIndex)->get_state(); 77 | landmark_ = get_neighbour_nodes()->at(landmarkIndex)->get_state(); 78 | dx_ = landmark_(0) - state_(0); 79 | dy_ = landmark_(1) - state_(1); 80 | q_ = dx_*dx_ + dy_*dy_; 81 | 82 | if (q_ < 1e-6) 83 | { 84 | r_.setZero(); 85 | return; 86 | } 87 | // r = [range, bearing] 88 | r_ << std::sqrt(q_), 89 | std::atan2(dy_,dx_) - state_(2); 90 | r_ = r_-obs_; 91 | r_(1) = wrap_angle(r_(1)); 92 | } 93 | void Factor1Pose1Landmark2d::evaluate_jacobians() 94 | { 95 | // This function requries to FIRST evaluate residuals (which is always done) 96 | Mat<2,3> Jx = Mat<2,3>::Zero(); 97 | matData_t sqrt_q = std::sqrt(q_); 98 | Jx << -dx_/sqrt_q, -dy_/sqrt_q, 0, 99 | dy_/q_, -dx_/q_, -1; 100 | Mat<2,2> Jl = Mat<2,2>::Zero(); 101 | Jl << dx_/sqrt_q, dy_/sqrt_q, 102 | -dy_/q_, dx_/q_; 103 | // If that happens, the configuration is singular and no valid angle can be calcualted 104 | if (q_ < 1e-6) 105 | { 106 | Jx.setIdentity(); 107 | Jl.setIdentity(); 108 | } 109 | if (!reversedNodeOrder_) 110 | { 111 | J_.topLeftCorner<2,3>() = Jx; 112 | J_.topRightCorner<2,2>() = Jl; 113 | } 114 | else 115 | { 116 | J_.topLeftCorner<2,2>() = Jl; 117 | J_.topRightCorner<2,3>() = Jx; 118 | } 119 | } 120 | 121 | void Factor1Pose1Landmark2d::evaluate_chi2() 122 | { 123 | chi2_ = 0.5 * r_.dot(W_ * r_); 124 | } 125 | void Factor1Pose1Landmark2d::print() const 126 | { 127 | std::cout << "Printing Factor: " << id_ << ", obs= \n" << obs_ 128 | << "\n Residuals= \n" << r_ 129 | << " \nand Information matrix\n" << W_ 130 | << "\n Calculated Jacobian = \n" << J_ 131 | << "\n Chi2 error = " << chi2_ 132 | << " and neighbour Nodes " << neighbourNodes_.size() 133 | << std::endl; 134 | } 135 | 136 | -------------------------------------------------------------------------------- /src/FGraph/factors/factor1Pose1Landmark3d.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * factor1Pose1Landmark3d.cpp 17 | * 18 | * Created on: March 17, 2020 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | 25 | #include "mrob/factors/factor1Pose1Landmark3d.hpp" 26 | 27 | #include 28 | 29 | using namespace mrob; 30 | 31 | 32 | Factor1Pose1Landmark3d::Factor1Pose1Landmark3d(const Mat31 &observation, std::shared_ptr &nodePose, 33 | std::shared_ptr &nodeLandmark, const Mat3 &obsInf, bool initializeLandmark, 34 | Factor::robustFactorType robust_type): 35 | Factor(3,9, robust_type), obs_(observation), W_(obsInf), reversedNodeOrder_(false) 36 | { 37 | // chek for order, we need to ensure id_0 < id_1 38 | if (nodePose->get_id() < nodeLandmark->get_id()) 39 | { 40 | neighbourNodes_.push_back(nodePose); 41 | neighbourNodes_.push_back(nodeLandmark); 42 | } 43 | else 44 | { 45 | neighbourNodes_.push_back(nodeLandmark); 46 | neighbourNodes_.push_back(nodePose); 47 | // set reverse mode 48 | reversedNodeOrder_ = true; 49 | } 50 | 51 | if (initializeLandmark) 52 | { 53 | // TODO Initialize landmark value to whatever observation we see from current pose 54 | } 55 | } 56 | 57 | 58 | 59 | void Factor1Pose1Landmark3d::evaluate_residuals() 60 | { 61 | // From T we observe z, and the residual is r = T^{-1} landm - z 62 | uint_t poseIndex = 0; 63 | uint_t landmarkIndex = 1; 64 | if (reversedNodeOrder_) 65 | { 66 | landmarkIndex = 0; 67 | poseIndex = 1; 68 | } 69 | Mat4 Tx = get_neighbour_nodes()->at(poseIndex)->get_state(); 70 | Tinv_ = SE3(Tx).inv(); 71 | landmark_ = get_neighbour_nodes()->at(landmarkIndex)->get_state(); 72 | r_ = Tinv_.transform(landmark_) - obs_; 73 | 74 | } 75 | void Factor1Pose1Landmark3d::evaluate_jacobians() 76 | { 77 | // This function requries to FIRST evaluate residuals (which is always done) 78 | // dr / dT = d / dT ( T-1 Exp(-dx) l ) = T-1 [l^ -I] 79 | Mat<4,6> Jr = Mat<4,6>::Zero(); 80 | Jr.topLeftCorner<3,3>() = hat3(landmark_); 81 | Jr.topRightCorner<3,3>() = -Mat3::Identity(); 82 | if (reversedNodeOrder_) 83 | { 84 | J_.topLeftCorner<3,3>() = Tinv_.R(); 85 | J_.topRightCorner<3,6>() = ( Tinv_.T()* Jr).topLeftCorner<3,6>(); 86 | } 87 | else 88 | { 89 | J_.topLeftCorner<3,6>() = ( Tinv_.T()* Jr).topLeftCorner<3,6>(); 90 | J_.topRightCorner<3,3>() = Tinv_.R(); 91 | } 92 | } 93 | 94 | void Factor1Pose1Landmark3d::evaluate_chi2() 95 | { 96 | chi2_ = 0.5 * r_.dot(W_ * r_); 97 | } 98 | void Factor1Pose1Landmark3d::print() const 99 | { 100 | std::cout << "Printing Factor: " << id_ << ", obs= \n" << obs_ 101 | << "\n Residuals= \n" << r_ 102 | << " \nand Information matrix\n" << W_ 103 | << "\n Calculated Jacobian = \n" << J_ 104 | << "\n Chi2 error = " << chi2_ 105 | << " and neighbour Nodes " << neighbourNodes_.size() 106 | << std::endl; 107 | } 108 | 109 | -------------------------------------------------------------------------------- /src/FGraph/factors/factor1Pose1Plane4d.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * factor1Pose1Plane4d.cpp 17 | * 18 | * Created on: Oct 10, 2020 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #include "mrob/factors/factor1Pose1Plane4d.hpp" 25 | #include "mrob/SE3.hpp" 26 | 27 | #include 28 | 29 | using namespace mrob; 30 | 31 | Factor1Pose1Plane4d::Factor1Pose1Plane4d(const Mat41 &observation, std::shared_ptr &nodePose, 32 | std::shared_ptr &nodePlane, const Mat4 &obsInf, 33 | Factor::robustFactorType robust_type): 34 | Factor(4,10, robust_type), obs_(observation), W_(obsInf), reversedNodeOrder_(false) 35 | { 36 | // ensure that plane 4d, normal \in P2 and distance \in R 37 | obs_.head(3).normalize(); 38 | // To preserve the order when building the Adjacency matrix 39 | if (nodePose->get_id() < nodePlane->get_id()) 40 | { 41 | neighbourNodes_.push_back(nodePose); 42 | neighbourNodes_.push_back(nodePlane); 43 | } 44 | else 45 | { 46 | neighbourNodes_.push_back(nodePlane); 47 | neighbourNodes_.push_back(nodePose); 48 | // set reverse mode 49 | reversedNodeOrder_ = true; 50 | } 51 | } 52 | 53 | void Factor1Pose1Plane4d::evaluate_residuals() 54 | { 55 | // From nosePose we observe a plane, in local coordinates 56 | // which then it is related to the landmark plane, in global coordinates. Thus, 57 | // the residual is formulated, according to our convention in factor.hpp: 58 | // 59 | // r = h(T,pi) - z_pi = T^{-T}pi - z_pi 60 | // 61 | // Points and planes are duals in 3D, as explained in Multiview-geometry by hartley and zisserman 62 | uint_t poseIndex = 0; 63 | uint_t landmarkIndex = 1; 64 | if (reversedNodeOrder_) 65 | { 66 | landmarkIndex = 0; 67 | poseIndex = 1; 68 | } 69 | Mat4 Tx = get_neighbour_nodes()->at(poseIndex)->get_state(); 70 | // The transformation we are looking for here is Txw, from world to local x. 71 | // which is the inverse of the current pose Tx in the state vector 72 | Tinv_transp_ = SE3(Tx).T().transpose(); 73 | plane_ = get_neighbour_nodes()->at(landmarkIndex)->get_state(); 74 | Mat41 pi_local = Tinv_transp_*plane_; 75 | r_ = pi_local - obs_; 76 | // Normals must have the same direction, so we ensure this here. XXX: Temporary solution 77 | //if (pi_local.head(3).dot(obs_.head(3)) < 0.0) 78 | // temporary solution 2: all observation have d >0, so we enforce this condition here. 79 | // If d~0 it should not work properly, but it works better than comparing normals 80 | if (pi_local(3) < 0.0) 81 | { 82 | std::cout << "correction" << std::endl; 83 | r_ = pi_local + obs_; 84 | } 85 | } 86 | void Factor1Pose1Plane4d::evaluate_jacobians() 87 | { 88 | Mat<4,6> Jx = Mat<4,6>::Zero(); 89 | Mat31 normal = plane_.head(3); 90 | Jx.topLeftCorner<3,3>() = hat3(normal); 91 | Jx.bottomRightCorner<1,3>() = normal; 92 | if (!reversedNodeOrder_) 93 | { 94 | J_.topLeftCorner<4,6>() = Tinv_transp_ * Jx; 95 | J_.topRightCorner<4,4>() = Tinv_transp_; 96 | } 97 | else 98 | { 99 | J_.topLeftCorner<4,4>() = Tinv_transp_; 100 | J_.topRightCorner<4,6>() = Tinv_transp_ * Jx; 101 | } 102 | } 103 | void Factor1Pose1Plane4d::evaluate_chi2() 104 | { 105 | chi2_ = 0.5 * r_.dot(W_ * r_); 106 | } 107 | 108 | void Factor1Pose1Plane4d::print() const 109 | { 110 | std::cout << "Printing Plane Factor: " << id_ << ", obs= \n" << obs_ 111 | << "\n Residuals= \n" << r_ 112 | << " \nand Information matrix\n" << W_ 113 | << "\n Calculated Jacobian = \n" << J_ 114 | << "\n Chi2 error = " << chi2_ 115 | << " and neighbour Nodes " << neighbourNodes_.size() 116 | << std::endl; 117 | } 118 | -------------------------------------------------------------------------------- /src/FGraph/factors/factor1Pose2d.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * Created on: Jan 14, 2019 17 | * Author: Konstantin Pakulev 18 | * konstantin.pakulev@skoltech.ru 19 | * Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #include "mrob/factors/factor1Pose2d.hpp" 25 | 26 | #include 27 | 28 | using namespace mrob; 29 | 30 | Factor1Pose2d::Factor1Pose2d(const Mat31 &observation, std::shared_ptr &n1, 31 | const Mat3 &obsInf, Factor::robustFactorType robust_type) : 32 | Factor(3, 3, robust_type), obs_(observation), W_(obsInf), J_(Mat3::Zero()) 33 | { 34 | neighbourNodes_.push_back(n1); 35 | } 36 | 37 | void Factor1Pose2d::evaluate_jacobians() 38 | { 39 | // Evaluate Jacobian 40 | J_ = Mat3::Identity(); 41 | } 42 | 43 | void Factor1Pose2d::evaluate_residuals() 44 | { 45 | r_ = get_neighbour_nodes()->at(0).get()->get_state() - obs_; 46 | r_(2) = wrap_angle(r_(2)); 47 | } 48 | 49 | void Factor1Pose2d::evaluate_chi2() 50 | { 51 | chi2_ = 0.5 * r_.dot(W_ * r_); 52 | } 53 | 54 | void Factor1Pose2d::print() const 55 | { 56 | std::cout << "Printing Factor: " << id_ << ", obs= \n" << obs_ 57 | << "\n Residuals= \n" << r_ 58 | << " \nand Information matrix\n" << W_ 59 | << "\n Calculated Jacobian = \n" << J_ 60 | << "\n Chi2 error = " << chi2_ 61 | << " and neighbour Nodes " << neighbourNodes_.size() 62 | << std::endl; 63 | } 64 | 65 | 66 | -------------------------------------------------------------------------------- /src/FGraph/factors/factor1Pose3d.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * factor1Pose3d.cpp 17 | * 18 | * Created on: Mar 5, 2018 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | 25 | #include "mrob/factors/factor1Pose3d.hpp" 26 | 27 | #include 28 | 29 | using namespace mrob; 30 | 31 | 32 | Factor1Pose3d::Factor1Pose3d(const Mat4 &observation, std::shared_ptr &n1, 33 | const Mat6 &obsInf, Factor::robustFactorType robust_type): 34 | Factor(6,6, robust_type), Tobs_(observation), W_(obsInf), J_(Mat6::Zero()) 35 | { 36 | // Ordering here is not a problem, the node is unique 37 | neighbourNodes_.push_back(n1); 38 | } 39 | 40 | Factor1Pose3d::Factor1Pose3d(const SE3 &observation, std::shared_ptr &n1, 41 | const Mat6 &obsInf, Factor::robustFactorType robust_type): 42 | Factor(6,6, robust_type), Tobs_(observation), W_(obsInf), J_(Mat6::Zero()) 43 | { 44 | // Ordering here is not a problem, the node is unique 45 | neighbourNodes_.push_back(n1); 46 | } 47 | 48 | void Factor1Pose3d::evaluate_residuals() 49 | { 50 | // Anchor residuals as r = x - obs 51 | // r = ln(X * Tobs^-1) = - ln(Tobs * x^-1) 52 | // NOTE Tobs is a global observation (reference identity) 53 | Mat4 x = get_neighbour_nodes()->at(0).get()->get_state(); 54 | Tr_ = SE3(x) * Tobs_.inv(); 55 | r_ = Tr_.ln_vee(); 56 | } 57 | 58 | void Factor1Pose3d::evaluate_jacobians() 59 | { 60 | // Evaluate Jacobian (see document on SE3 and small perturbations) 61 | // J = d/dxi ln(T X-1 exp(-xi) (T X-1)-1)= - Adj_{T X-1} = - Adj(Tr) 62 | // J = d/dxi ln(exp(xi)X T-1 (T X-1)-1)= I 63 | J_ = Mat6::Identity(); 64 | } 65 | 66 | void Factor1Pose3d::evaluate_chi2() 67 | { 68 | chi2_ = 0.5 * r_.dot(W_ * r_); 69 | } 70 | 71 | void Factor1Pose3d::print() const 72 | { 73 | std::cout << "Printing Factor: " << id_ << ", obs= \n" << Tobs_.T() 74 | << "\n Residuals= \n" << r_ 75 | << " \nand Information matrix\n" << W_ 76 | << "\n Calculated Jacobian = \n" << J_ 77 | << "\n Chi2 error = " << chi2_ 78 | << " and neighbour Nodes " << neighbourNodes_.size() 79 | << std::endl; 80 | } 81 | -------------------------------------------------------------------------------- /src/FGraph/factors/factor2Poses3d.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * factor2Poses3d.cpp 17 | * 18 | * Created on: Feb 28, 2018 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | 25 | #include "mrob/factors/factor2Poses3d.hpp" 26 | 27 | #include 28 | 29 | using namespace mrob; 30 | 31 | 32 | Factor2Poses3d::Factor2Poses3d(const Mat4 &observation, std::shared_ptr &nodeOrigin, 33 | std::shared_ptr &nodeTarget, const Mat6 &obsInf, bool updateNodeTarget, 34 | Factor::robustFactorType robust_type): 35 | Factor(6,12,robust_type), Tobs_(observation), W_(obsInf) 36 | { 37 | if (nodeOrigin->get_id() < nodeTarget->get_id()) 38 | { 39 | neighbourNodes_.push_back(nodeOrigin); 40 | neighbourNodes_.push_back(nodeTarget); 41 | } 42 | else 43 | { 44 | neighbourNodes_.push_back(nodeTarget); 45 | neighbourNodes_.push_back(nodeOrigin); 46 | 47 | // inverse observations to correctly modify this 48 | Tobs_.inv(); 49 | } 50 | if (updateNodeTarget) 51 | { 52 | // Updates the child node such that it matches the odometry observation 53 | // carefull on the reference frame that Tobs is expressed at the X_origin frame, hence this change: 54 | Mat4 TxOrigin = nodeOrigin->get_state(); 55 | nodeTarget->set_state( TxOrigin * Tobs_.T() ); 56 | } 57 | } 58 | 59 | Factor2Poses3d::Factor2Poses3d(const SE3 &observation, std::shared_ptr &nodeOrigin, 60 | std::shared_ptr &nodeTarget, const Mat6 &obsInf, bool updateNodeTarget, 61 | Factor::robustFactorType robust_type): 62 | Factor(6,12, robust_type), Tobs_(observation), W_(obsInf) 63 | { 64 | if (nodeOrigin->get_id() < nodeTarget->get_id()) 65 | { 66 | neighbourNodes_.push_back(nodeOrigin); 67 | neighbourNodes_.push_back(nodeTarget); 68 | } 69 | else 70 | { 71 | neighbourNodes_.push_back(nodeTarget); 72 | neighbourNodes_.push_back(nodeOrigin); 73 | 74 | // inverse observations to correctly modify this 75 | Tobs_.inv(); 76 | } 77 | if (updateNodeTarget) 78 | { 79 | // Updates the child node such that it matches the odometry observation 80 | // carefull on the reference frame that Tobs is expressed at the X_origin frame, hence this change: 81 | Mat4 TxOrigin = nodeOrigin->get_state(); 82 | nodeTarget->set_state( TxOrigin * Tobs_.T() ); 83 | } 84 | } 85 | 86 | 87 | void Factor2Poses3d::evaluate_residuals() 88 | { 89 | // From Origin we observe Target such that: T_o * T_obs = T_t 90 | // Tr = Txo * Tobs * Txt^-1 91 | // NOTE: We could also use the adjoint to refer the manifold coordinates obs w.r.t xo but in this case that 92 | // does not briung any advantage on the xo to the global frame (identity) 93 | // (xo reference)T_obs * T_xo = T_xo * (global)T_obs. (rhs is what we use here) 94 | Mat4 TxOrigin = get_neighbour_nodes()->at(0)->get_state(); 95 | Mat4 TxTarget = get_neighbour_nodes()->at(1)->get_state(); 96 | Tr_ = SE3(TxOrigin) * Tobs_ * SE3(TxTarget).inv(); 97 | r_ = Tr_.ln_vee(); 98 | 99 | } 100 | void Factor2Poses3d::evaluate_jacobians() 101 | { 102 | // it assumes you already have evaluated residuals 103 | J_.topLeftCorner<6,6>() = Mat6::Identity(); 104 | J_.topRightCorner<6,6>() = -Tr_.adj(); 105 | } 106 | 107 | void Factor2Poses3d::evaluate_chi2() 108 | { 109 | chi2_ = 0.5 * r_.dot(W_ * r_); 110 | } 111 | void Factor2Poses3d::print() const 112 | { 113 | std::cout << "Printing Factor: " << id_ << ", obs= \n" << Tobs_.T() 114 | << "\n Residuals= \n" << r_ 115 | << " \nand Information matrix\n" << W_ 116 | << "\n Calculated Jacobian = \n" << J_ 117 | << "\n Chi2 error = " << chi2_ 118 | << " and neighbour Node ids: " << neighbourNodes_[0]->get_id() 119 | << ", " << neighbourNodes_[1]->get_id() 120 | << std::endl; 121 | } 122 | 123 | -------------------------------------------------------------------------------- /src/FGraph/factors/nodeLandmark2d.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * nodeLandmark2d.cpp 17 | * 18 | * Created on: Jul 27, 2020 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab. 22 | */ 23 | 24 | #include "mrob/factors/nodeLandmark2d.hpp" 25 | 26 | #include 27 | #include 28 | 29 | 30 | using namespace mrob; 31 | 32 | NodeLandmark2d::NodeLandmark2d(const Mat21 &initial_x, Node::nodeMode mode) : 33 | Node(2, mode), state_(initial_x), auxiliaryState_(initial_x) 34 | { 35 | assert(initial_x.rows() == 2 && "NodeLandmark2d:: Incorrect dimension on initial state rows" ); 36 | assert(initial_x.cols() == 1 && "NodeLandmark2d:: Incorrect dimension on initial state cols" ); 37 | } 38 | 39 | 40 | void NodeLandmark2d::update(const Eigen::Ref &dx) 41 | { 42 | state_ += dx; 43 | } 44 | 45 | void NodeLandmark2d::update_from_auxiliary(const Eigen::Ref &dx) 46 | { 47 | state_ = auxiliaryState_ + dx; 48 | } 49 | 50 | void NodeLandmark2d::set_state(const Eigen::Ref &x) 51 | { 52 | state_ = x; 53 | } 54 | 55 | void NodeLandmark2d::set_auxiliary_state(const Eigen::Ref &x) 56 | { 57 | auxiliaryState_ = x; 58 | } 59 | 60 | void NodeLandmark2d::print() const 61 | { 62 | std::cout << "Printing NodeLandmark2d: " << id_ 63 | << ", state = \n" << state_; 64 | std::cout << "\nand neighbour factors " << neighbourFactors_.size() 65 | << std::endl; 66 | } 67 | -------------------------------------------------------------------------------- /src/FGraph/factors/nodeLandmark3d.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * nodeLandmark3d.cpp 17 | * 18 | * Created on: March 17, 2020 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #include "mrob/factors/nodeLandmark3d.hpp" 25 | 26 | #include 27 | #include 28 | 29 | using namespace mrob; 30 | 31 | NodeLandmark3d::NodeLandmark3d(const Mat31 &initial_x, Node::nodeMode mode) : 32 | Node(3, mode), state_(initial_x), auxiliaryState_(initial_x) 33 | { 34 | assert(initial_x.rows() == 3 && "NodeLandmark3d:: Incorrect dimension on initial state rows" ); 35 | assert(initial_x.cols() == 1 && "NodeLandmark3d:: Incorrect dimension on initial state cols" ); 36 | } 37 | 38 | 39 | void NodeLandmark3d::update(const Eigen::Ref &dx) 40 | { 41 | Mat31 dxf = dx;//XXX cast is necessary? 42 | state_ += dxf; 43 | } 44 | 45 | void NodeLandmark3d::update_from_auxiliary(const Eigen::Ref &dx) 46 | { 47 | Mat31 dxf = dx; 48 | state_ = auxiliaryState_ + dxf; 49 | } 50 | 51 | void NodeLandmark3d::set_state(const Eigen::Ref &x) 52 | { 53 | // cast is done by Eigen 54 | state_ = x; 55 | } 56 | 57 | void NodeLandmark3d::set_auxiliary_state(const Eigen::Ref &x) 58 | { 59 | auxiliaryState_ = x; 60 | } 61 | 62 | void NodeLandmark3d::print() const 63 | { 64 | std::cout << "Printing NodeLandmark3d: " << id_ 65 | << ", state = \n" << state_; 66 | std::cout << "\nand neighbour factors " << neighbourFactors_.size() 67 | << std::endl; 68 | } 69 | -------------------------------------------------------------------------------- /src/FGraph/factors/nodePlane4d.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * nodePlane4d.cpp 17 | * 18 | * Created on: Oct 10, 2020 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | 25 | #include "mrob/factors/nodePlane4d.hpp" 26 | #include 27 | #include 28 | 29 | using namespace mrob; 30 | 31 | NodePlane4d::NodePlane4d(const Mat41 &initial_x, Node::nodeMode mode): 32 | Node(4,mode), state_(initial_x), auxiliaryState_(initial_x) 33 | { 34 | assert(initial_x.rows() == 4 && "NodePlane4d:: Incorrect dimension on initial state rows" ); 35 | assert(initial_x.cols() == 1 && "NodePlane4d:: Incorrect dimension on initial state cols" ); 36 | // ensure that plane 4d |n|=1. Distance should be well defined and not scaled 37 | state_.head(3).normalize(); 38 | auxiliaryState_.head(3).normalize(); 39 | } 40 | 41 | 42 | 43 | void NodePlane4d::update(const Eigen::Ref &dx) 44 | { 45 | state_ += dx; 46 | state_.head(3).normalize(); 47 | } 48 | 49 | void NodePlane4d::update_from_auxiliary(const Eigen::Ref &dx) 50 | { 51 | state_ = auxiliaryState_ + dx; 52 | state_.head(3).normalize(); 53 | } 54 | 55 | void NodePlane4d::print() const 56 | { 57 | std::cout << "Printing NodePlane4d: " << id_ 58 | << ", state = \n" << state_ << ",\n"; 59 | std::cout << "and neighbour factors " << neighbourFactors_.size() 60 | << std::endl; 61 | } 62 | -------------------------------------------------------------------------------- /src/FGraph/factors/nodePose2d.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * Created on: Jan 14, 2019 17 | * Author: Konstantin Pakulev 18 | * konstantin.pakulev@skoltech.ru 19 | * Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #include "mrob/factors/nodePose2d.hpp" 25 | #include 26 | 27 | using namespace mrob; 28 | 29 | NodePose2d::NodePose2d(const Mat31 &initial_x, Node::nodeMode mode) : Node(3, mode), state_(initial_x), auxiliaryState_(initial_x) 30 | { 31 | assert(initial_x.rows() == 3 && "NodePose2d:: Incorrect dimension on initial state rows"); 32 | assert(initial_x.cols() == 1 && "NodePose2d:: Incorrect dimension on initial state cols"); 33 | } 34 | 35 | void NodePose2d::update(const Eigen::Ref &dx) 36 | { 37 | state_ += dx; 38 | state_(2) = wrap_angle(state_(2)); 39 | 40 | } 41 | 42 | void NodePose2d::update_from_auxiliary(const Eigen::Ref &dx) 43 | { 44 | state_ = auxiliaryState_ + dx; 45 | state_(2) = wrap_angle(state_(2)); 46 | 47 | } 48 | 49 | 50 | void NodePose2d::set_state(const Eigen::Ref &x) 51 | { 52 | state_ = x; 53 | state_(2) = wrap_angle(state_(2)); 54 | } 55 | 56 | void NodePose2d::set_auxiliary_state(const Eigen::Ref &x) 57 | { 58 | auxiliaryState_ = x; 59 | auxiliaryState_(2) = wrap_angle(auxiliaryState_(2)); 60 | } 61 | 62 | void NodePose2d::print() const 63 | { 64 | std::cout << "Printing NodePose2d: " << id_ 65 | << ", state = \n" << state_ 66 | << "\nand neighbour factors " << neighbourFactors_.size() 67 | << std::endl; 68 | } 69 | -------------------------------------------------------------------------------- /src/FGraph/factors/nodePose3d.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * nodePose3d.cpp 17 | * 18 | * Created on: Feb 28, 2018 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #include "mrob/factors/nodePose3d.hpp" 25 | 26 | #include 27 | #include 28 | 29 | using namespace mrob; 30 | 31 | NodePose3d::NodePose3d(const Mat4 &initial_x, Node::nodeMode mode) : 32 | Node(6,mode), state_(initial_x), auxiliaryState_(initial_x) 33 | { 34 | // TODO remove me 35 | //assert(initial_x.rows() == 6 && "NodePose3d:: Incorrect dimension on initial state rows" ); 36 | //assert(initial_x.cols() == 1 && "NodePose3d:: Incorrect dimension on initial state cols" ); 37 | assert(isSE3(initial_x) && "NodePose3d:: Incorrect initial state, not an element of SE3" ); 38 | } 39 | 40 | NodePose3d::NodePose3d(const SE3 &initial_x, Node::nodeMode mode) : 41 | Node(6, mode), state_(initial_x), auxiliaryState_(initial_x) 42 | { 43 | assert(isSE3(initial_x.T()) && "NodePose3d:: Incorrect initial state, not an element of SE3" ); 44 | } 45 | 46 | 47 | void NodePose3d::update(const Eigen::Ref &dx) 48 | { 49 | Mat61 dxf = dx; 50 | 51 | // Tx and x are always sync, i.e., Tx = exp(x^) 52 | state_.update_lhs(dxf); 53 | // XXX regeneration of state is required, for now we do it every time. random? count? 54 | // XXX is it necessary? 55 | state_.regenerate(); 56 | } 57 | 58 | void NodePose3d::update_from_auxiliary(const Eigen::Ref &dx) 59 | { 60 | Mat61 dxf = dx; 61 | state_ = auxiliaryState_;//we update from the auxiliary state 62 | state_.update_lhs(dxf); 63 | } 64 | 65 | void NodePose3d::set_state(const Eigen::Ref &x) 66 | { 67 | // casting is necessary for SE3 constructor, it does not handle a ref TODO 68 | Mat4 newState = x; 69 | state_ = SE3(newState); 70 | } 71 | 72 | void NodePose3d::set_auxiliary_state(const Eigen::Ref &x) 73 | { 74 | Mat4 newState = x; 75 | auxiliaryState_ = SE3(newState); 76 | } 77 | 78 | void NodePose3d::print() const 79 | { 80 | std::cout << "Printing NodePose3d: " << id_ 81 | << ", state = \n" << state_.ln_vee() << ",\n SE3 matrix: \n"; 82 | state_.print(); 83 | std::cout << "\nand neighbour factors " << neighbourFactors_.size() 84 | << std::endl; 85 | } 86 | -------------------------------------------------------------------------------- /src/FGraph/mrob/factor_graph_solve_dense.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * factor_graph_solve_dense.hpp 17 | * 18 | * Created on: Sep 3, 2020 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab. 22 | */ 23 | 24 | #ifndef FACTOR_GRAPH_SOLVE_DENSE_HPP_ 25 | #define FACTOR_GRAPH_SOLVE_DENSE_HPP_ 26 | 27 | #include "mrob/optimizer.hpp" 28 | #include "mrob/factor_graph.hpp" 29 | #include "mrob/time_profiling.hpp" 30 | 31 | namespace mrob { 32 | 33 | 34 | /** 35 | * Class FGraphSolveDense solve a factor graph problem assuming dense 36 | * precision matrix. 37 | * 38 | * It inherits from two classes: 39 | * - FGraph: structure for adding generic factors 40 | * - Optimizer: Optimization methods given some abstract routines 41 | */ 42 | class FGraphSolveDense: public FGraph, public OptimizerDense 43 | { 44 | public: 45 | FGraphSolveDense(); 46 | ~FGraphSolveDense(); 47 | 48 | // Function from the parent class Optimizer 49 | virtual matData_t calculate_error() override; 50 | virtual void calculate_gradient_hessian() override; 51 | virtual void update_state() override; 52 | virtual void bookkeep_state() override; 53 | virtual void update_state_from_bookkeep() override; 54 | 55 | }; 56 | 57 | 58 | }//namespace 59 | 60 | #endif /* FACTOR_GRAPH_SOLVE_DENSE_HPP_ */ 61 | -------------------------------------------------------------------------------- /src/FGraph/mrob/factors/factor1Pose1Landmark2d.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * factor1Pose1Landmark2d.hpp 17 | * 18 | * Created on: Jul 27, 2020 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab. 22 | */ 23 | 24 | #ifndef FACTOR1POSE1LANDMARK2D_HPP_ 25 | #define FACTOR1POSE1LANDMARK2D_HPP_ 26 | 27 | 28 | 29 | #include "mrob/matrix_base.hpp" 30 | #include "mrob/factor.hpp" 31 | 32 | namespace mrob{ 33 | 34 | /** 35 | * The factor1Pose1Landmark2d is a vertex representing the distribution between 36 | * a 2D pose and a Landmark, a 2D point. The observation consist of Range and Bearing. 37 | * 38 | * The observation is a 2D point, in the local frame of the current 2D pose. 39 | * The two Nodes that the factor is connecting, which are provided by their 40 | * shared_ptr's, are: 41 | * - 1 Pose2d 42 | * - 1 Landmark2d 43 | * We provide the node's Id to get the correspondent Jacobian 44 | * 45 | * 46 | * In particular, the relation between the transformation of poses is: 47 | * z = h(x) 48 | * 49 | * z = [range,bearing] is a 2d vector, composed of a range and bearing in the local frame. 50 | * l is a 2d point encoding the landmark position l = [x,y] 51 | * 52 | * 53 | * Constructor functions will be overloaded to include the pointers of the nodes, 54 | * The convention is 2d pose, we observe node destination, 55 | * such that: Factor1Pose1Landmark2d(nodePose, nodeLandmark, ... 56 | * 57 | * The observations relate a pair of nodes. The order matters, since this will 58 | * affect the order on the Jacobian block matrix 59 | */ 60 | 61 | class Factor1Pose1Landmark2d : public Factor 62 | { 63 | public: 64 | Factor1Pose1Landmark2d(const Mat21 &observation, std::shared_ptr &nodePose, 65 | std::shared_ptr &nodeLandmark, const Mat2 &obsInf, bool initializeLandmark=false, 66 | Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC); 67 | ~Factor1Pose1Landmark2d() override = default; 68 | /** 69 | * Jacobians are not evaluated, just the residuals 70 | */ 71 | void evaluate_residuals() override; 72 | /** 73 | * Evaluates residuals and Jacobians 74 | */ 75 | void evaluate_jacobians() override; 76 | void evaluate_chi2() override; 77 | 78 | void print() const; 79 | 80 | const Eigen::Ref get_obs() const {return obs_;}; 81 | const Eigen::Ref get_residual() const {return r_;}; 82 | const Eigen::Ref get_information_matrix() const {return W_;}; 83 | const Eigen::Ref get_jacobian() const {return J_;}; 84 | 85 | protected: 86 | Mat21 obs_, r_, landmark_; 87 | Mat31 state_; 88 | matData_t dx_, dy_, q_; // increments from pose to landmark (x,y) and q squared L2 distance 89 | Mat2 W_;//inverse of observation covariance (information matrix) 90 | Mat<2,5> J_;//Joint Jacobian, Block will depend on order 91 | bool reversedNodeOrder_;//flag to keep order when building the adjacency matrix. This should be transparent for the user 92 | 93 | public: 94 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // as proposed by Eigen 95 | 96 | }; 97 | 98 | 99 | } 100 | 101 | 102 | #endif /* FACTOR1POSE1LANDMARK2D_HPP_ */ 103 | -------------------------------------------------------------------------------- /src/FGraph/mrob/factors/factor1Pose1Landmark3d.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * factor1Pose1Landmark3d.hpp 17 | * 18 | * Created on: March 17, 2020 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #ifndef FACTOR1POSE1LANDMARK3D_HPP_ 25 | #define FACTOR1POSE1LANDMARK3D_HPP_ 26 | 27 | 28 | #include "mrob/matrix_base.hpp" 29 | #include "mrob/SE3.hpp" //requires including and linking SE3 library 30 | #include "mrob/factor.hpp" 31 | 32 | namespace mrob{ 33 | 34 | /** 35 | * The factor1Pose1Landmark3d is a vertex representing the distribution between 36 | * a Rigid Body Transformation encoding a 3D pose and a Landmark, a 3D point 37 | * 38 | * The observation is a 3D point, in the local frame of the current 3D pose. 39 | * The two Nodes that the factor is connecting, which are provided by their 40 | * shared_ptr's, are: 41 | * - 1 Pose3d 42 | * - 1 Landmark3d 43 | * We provide the node's Id to get the correspondent Jacobian 44 | * 45 | * 46 | * In particular, the relation between the transformation of poses is: 47 | * z = T^{-1}*l 48 | * 49 | * z is a 3d point with the observations in the local frame T 50 | * T is the transformation encoded by the 3D pose, the local frame. 51 | * l is a 3d point encoding the landmark position 52 | * 53 | * and the residual is thus: 54 | * r = T^{-1}l - z 55 | * 56 | * 57 | * Constructor functions will be overloaded to include the pointers of the nodes, 58 | * The convention is 3d pose, we observe node destination, 59 | * such that: Factor1Pose1Landmark3d(nodePose, nodeLandmark, ... 60 | * 61 | * The observations relate a pair of nodes. The order matters, since this will 62 | * affect the order on the Jacobian block matrix 63 | */ 64 | 65 | class Factor1Pose1Landmark3d : public Factor 66 | { 67 | public: 68 | Factor1Pose1Landmark3d(const Mat31 &observation, std::shared_ptr &nodePose, 69 | std::shared_ptr &nodeLandmark, const Mat3 &obsInf, bool initializeLandmark=false, 70 | Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC); 71 | ~Factor1Pose1Landmark3d() override = default; 72 | /** 73 | * Jacobians are not evaluated, just the residuals 74 | */ 75 | void evaluate_residuals() override; 76 | /** 77 | * Evaluates residuals and Jacobians 78 | */ 79 | void evaluate_jacobians() override; 80 | void evaluate_chi2() override; 81 | 82 | void print() const; 83 | 84 | const Eigen::Ref get_obs() const {return obs_;}; 85 | const Eigen::Ref get_residual() const {return r_;}; 86 | const Eigen::Ref get_information_matrix() const {return W_;}; 87 | const Eigen::Ref get_jacobian() const {return J_;}; 88 | 89 | protected: 90 | Mat31 obs_, r_, landmark_; 91 | SE3 Tinv_; 92 | Mat3 W_;//inverse of observation covariance (information matrix) 93 | Mat<3,9> J_;//Joint Jacobian 94 | bool reversedNodeOrder_;//flag to keep order when building the adjacency matrix. This should be transparent for the user 95 | 96 | public: 97 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // as proposed by Eigen 98 | 99 | }; 100 | 101 | 102 | } 103 | 104 | 105 | 106 | 107 | #endif /* Factor1Pose1Landmark3d_HPP_ */ 108 | -------------------------------------------------------------------------------- /src/FGraph/mrob/factors/factor1Pose1Plane4d.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * factor1Pose1Plane4d.hpp 17 | * 18 | * Created on: Oct 10, 2020 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #ifndef FACTOR1POSE1PLANE4D_HPP_ 25 | #define FACTOR1POSE1PLANE4D_HPP_ 26 | 27 | 28 | #include "mrob/matrix_base.hpp" 29 | #include "mrob/factor.hpp" 30 | 31 | namespace mrob{ 32 | 33 | /** 34 | * Factor 1 Pose (3D) and plane 4d relates a pose with a plane observation, extracted from 35 | * segmented points in a point cloud. 36 | * 37 | * 38 | */ 39 | 40 | class Factor1Pose1Plane4d : public Factor 41 | { 42 | public: 43 | Factor1Pose1Plane4d(const Mat41 &observation, std::shared_ptr &nodePose, 44 | std::shared_ptr &nodePlane, const Mat4 &obsInf, 45 | Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC); 46 | ~Factor1Pose1Plane4d() override = default; 47 | 48 | /** 49 | * Jacobians are not evaluated, just the residuals 50 | */ 51 | virtual void evaluate_residuals() override; 52 | /** 53 | * Evaluates residuals and Jacobians 54 | */ 55 | virtual void evaluate_jacobians() override; 56 | virtual void evaluate_chi2() override; 57 | 58 | virtual void print() const; 59 | 60 | virtual const Eigen::Ref get_obs() const override {return obs_;}; 61 | virtual const Eigen::Ref get_residual() const override {return r_;}; 62 | virtual const Eigen::Ref get_information_matrix() const override {return W_;}; 63 | virtual const Eigen::Ref get_jacobian() const override {return J_;}; 64 | 65 | 66 | private: 67 | Mat41 obs_, r_; //residuals 68 | Mat<4,10> J_;//Jacobians dimensions obs x [plane(4) + pose(6)] 69 | Mat4 W_;//inverse of observation covariance (information matrix) 70 | bool reversedNodeOrder_; 71 | // intermediate variables to keep 72 | Mat41 plane_; 73 | Mat4 Tinv_transp_; 74 | 75 | public: 76 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 77 | 78 | }; 79 | 80 | } 81 | 82 | #endif /* FACTOR1POSE1PLANE4D_HPP_ */ 83 | -------------------------------------------------------------------------------- /src/FGraph/mrob/factors/factor1Pose2d.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * Created on: Jan 14, 2019 17 | * Author: Konstantin Pakulev 18 | * konstantin.pakulev@skoltech.ru 19 | * Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #ifndef MROB_FACTOR1POSE2D_H 25 | #define MROB_FACTOR1POSE2D_H 26 | 27 | #include "mrob/matrix_base.hpp" 28 | #include "mrob/factor.hpp" 29 | 30 | using namespace mrob; 31 | 32 | 33 | /** 34 | * The Factor1Poses2d is a vertex representing the distribution 35 | * of a nodePose2d, pretty much like an anchoring factor. 36 | * 37 | * The state is an observed RBT, coincident with the node state it is connected to. 38 | * 39 | * In particular, the residual of this factor is: 40 | * r = obs-x 41 | */ 42 | 43 | 44 | namespace mrob{ 45 | class Factor1Pose2d : public Factor 46 | { 47 | public: 48 | Factor1Pose2d(const Mat31 &observation, std::shared_ptr &n1, 49 | const Mat3 &obsInf, Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC); 50 | ~Factor1Pose2d() override = default; 51 | 52 | void evaluate_residuals() override; 53 | void evaluate_jacobians() override; 54 | void evaluate_chi2() override; 55 | 56 | void print() const; 57 | 58 | const Eigen::Ref get_obs() const {return obs_;}; 59 | const Eigen::Ref get_residual() const {return r_;}; 60 | const Eigen::Ref get_information_matrix() const {return W_;}; 61 | const Eigen::Ref get_jacobian() const {return J_;}; 62 | 63 | protected: 64 | Mat31 obs_, r_; //and residuals 65 | Mat3 W_;//inverse of observation covariance (information matrix) 66 | Mat3 J_;//Jacobian 67 | }; 68 | } 69 | 70 | #endif //MROB_FACTOR1POSE2D_H 71 | -------------------------------------------------------------------------------- /src/FGraph/mrob/factors/factor1Pose3d.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * factor1Pose3d.hpp 17 | * 18 | * Created on: Mar 5, 2018 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #ifndef FACTOR1POSE3D_HPP_ 25 | #define FACTOR1POSE3D_HPP_ 26 | 27 | 28 | #include "mrob/matrix_base.hpp" 29 | #include "mrob/SE3.hpp" //requires including and linking SE3 library 30 | #include "mrob/factor.hpp" 31 | 32 | namespace mrob{ 33 | 34 | /** 35 | * The Factor1Poses3d is a vertex representing the distribution 36 | * of a nodePose3d, pretty much like an anchoring factor. 37 | * 38 | * The state is an observed RBT, coincident with the node state it is connected to. 39 | * 40 | * In particular, the residual of this factor is: 41 | * r = (x - obs) = Tx * Tobs^{-1} 42 | */ 43 | 44 | class Factor1Pose3d : public Factor 45 | { 46 | public: 47 | Factor1Pose3d(const Mat4 &observation, std::shared_ptr &n1, const Mat6 &obsInf, 48 | Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC); 49 | Factor1Pose3d(const SE3 &observation, std::shared_ptr &n1, const Mat6 &obsInf, 50 | Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC); 51 | ~Factor1Pose3d() override = default; 52 | /** 53 | * Returns the chi2 error and fills the residual vector 54 | */ 55 | void evaluate_residuals() override; 56 | void evaluate_jacobians() override; 57 | void evaluate_chi2() override; 58 | 59 | void print() const; 60 | 61 | const Eigen::Ref get_obs() const {return Tobs_.T();}; 62 | const Eigen::Ref get_residual() const {return r_;}; 63 | const Eigen::Ref get_information_matrix() const {return W_;}; 64 | const Eigen::Ref get_jacobian() const {return J_;}; 65 | 66 | 67 | protected: 68 | Mat61 r_; //and residuals 69 | SE3 Tobs_, Tr_;//Transformation for the observation and the residual 70 | Mat6 W_;//inverse of observation covariance (information matrix) 71 | Mat6 J_;//Jacobian 72 | 73 | 74 | }; 75 | 76 | 77 | } 78 | 79 | 80 | 81 | #endif /* FACTOR1POSE3D_HPP_ */ 82 | -------------------------------------------------------------------------------- /src/FGraph/mrob/factors/factor2Poses2d.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * Created on: Jan 14, 2019 17 | * Author: Gonzalo Ferrer 18 | * g.ferrer@skoltech.ru 19 | * konstantin.pakulev@skoltech.ru 20 | * Konstantin Pakulev 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | #ifndef MROB_FACTOR2POSES2D_H 24 | #define MROB_FACTOR2POSES2D_H 25 | 26 | #include "mrob/matrix_base.hpp" 27 | #include "mrob/factor.hpp" 28 | 29 | 30 | namespace mrob{ 31 | 32 | /** 33 | * Factor2Poses2d if a factor relating a 2 2dimensional poses 34 | * through a direct observation such that: 35 | * observation = h(nodeOrigin,nodeTarget) = x_target - x_origin 36 | * or 37 | * residual = x_origin + observation - x_target 38 | * 39 | * With this arrangement, the linearized factor substracts the residual (r) 40 | * to the first order term of the nonlinear observation function: 41 | * || J dx - r || 42 | * 43 | * This convention will be followed by all factors in this library, otherwise the optimization 44 | * will not work properly. 45 | * 46 | * 47 | * It is possible to update the target node if it has not been initialized. 48 | * By default is set to FALSE, so we must specify it to true in case we want 49 | * to update (for instance, for displacement odometry factors) 50 | * 51 | */ 52 | class Factor2Poses2d : public Factor 53 | { 54 | public: 55 | Factor2Poses2d(const Mat31 &observation, std::shared_ptr &nodeOrigin, 56 | std::shared_ptr &nodeTarget, const Mat3 &obsInf, bool updateNodeTarget=false, 57 | Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC); 58 | ~Factor2Poses2d() override = default; 59 | 60 | void evaluate_residuals() override; 61 | void evaluate_jacobians() override; 62 | void evaluate_chi2() override; 63 | 64 | const Eigen::Ref get_obs() const override {return obs_;}; 65 | const Eigen::Ref get_residual() const override {return r_;}; 66 | const Eigen::Ref get_information_matrix() const override {return W_;}; 67 | const Eigen::Ref get_jacobian() const override {return J_;}; 68 | void print() const override; 69 | 70 | protected: 71 | // The Jacobian's correspondent nodes are ordered on the vector 72 | // being [0]->J1 and [1]->J2 73 | // declared here but initialized on child classes 74 | Mat31 obs_, r_; //and residuals 75 | Mat3 W_;//inverse of observation covariance (information matrix) 76 | Mat<3,6> J_;//Joint Jacobian 77 | 78 | public: 79 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // as proposed by Eigen 80 | }; 81 | 82 | /** 83 | * Factor2Poses2dOdom if a factor expressing the relation between consecutive 84 | * 2d nodes with odometry observations such that; 85 | * residual = g (x_origin, observation) - x_dest 86 | * 87 | * Observation = [drot1, dtrans, drot2] 88 | * 89 | * We assume that this factor also updates the value of node destination 90 | * unless explicitly written 91 | * 92 | */ 93 | class Factor2Poses2dOdom : public Factor2Poses2d 94 | { 95 | public: 96 | /** 97 | * Constructor of factor Odom. Conventions are: 98 | * 1) obs = [drot1, dtrans, drot2] 99 | * 2) This factor also updates the value of node destination according to obs 100 | */ 101 | Factor2Poses2dOdom(const Mat31 &observation, std::shared_ptr &nodeOrigin, 102 | std::shared_ptr &nodeTarget, const Mat3 &obsInf, bool updateNodeTarget=true, 103 | Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC); 104 | ~Factor2Poses2dOdom() override = default; 105 | 106 | void evaluate_residuals() override; 107 | void evaluate_jacobians() override; 108 | 109 | private: 110 | Mat31 get_odometry_prediction(Mat31 state, Mat31 motion); 111 | 112 | }; 113 | 114 | } 115 | 116 | #endif //MROB_FACTOR2POSES2D_H 117 | -------------------------------------------------------------------------------- /src/FGraph/mrob/factors/nodeLandmark2d.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * nodeLandmark2d.hpp 17 | * 18 | * Created on: Jul 27, 2020 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab. 22 | */ 23 | 24 | #ifndef NODELANDMARK2D_HPP_ 25 | #define NODELANDMARK2D_HPP_ 26 | 27 | #include "mrob/matrix_base.hpp" 28 | #include "mrob/node.hpp" 29 | 30 | namespace mrob{ 31 | 32 | /** 33 | * Class Node Landmark 2d is a node whose state is a landmark position 34 | * l = [x,y]'. 35 | * Here we implement the necessary methods compliant with the node.hpp 36 | * interface 37 | */ 38 | 39 | class NodeLandmark2d : public Node 40 | { 41 | public: 42 | /** 43 | * For initialization, requires an initial estimation of the state. 44 | */ 45 | NodeLandmark2d(const Mat21 &initial_x, Node::nodeMode mode = STANDARD); 46 | virtual ~NodeLandmark2d() override = default; 47 | 48 | virtual void update(const Eigen::Ref &dx); 49 | virtual void update_from_auxiliary(const Eigen::Ref &dx); 50 | virtual void set_state(const Eigen::Ref &x); 51 | virtual void set_auxiliary_state(const Eigen::Ref &x); 52 | virtual const Eigen::Ref get_state() const {return state_;}; 53 | virtual const Eigen::Ref get_auxiliary_state() const {return auxiliaryState_;}; 54 | void print() const; 55 | 56 | protected: 57 | Mat21 state_; 58 | Mat21 auxiliaryState_; 59 | 60 | public: 61 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // as proposed by Eigen 62 | 63 | 64 | }; 65 | 66 | 67 | } 68 | 69 | 70 | #endif /* NODELANDMARK2D_HPP_ */ 71 | -------------------------------------------------------------------------------- /src/FGraph/mrob/factors/nodeLandmark3d.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * nodeLandmark3d.hpp 17 | * 18 | * Created on: March 17, 2020 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #ifndef NODELANDMARK3D_HPP_ 25 | #define NODELANDMARK3D_HPP_ 26 | 27 | #include "mrob/matrix_base.hpp" 28 | #include "mrob/SE3.hpp" //requires including and linking SE3 library 29 | #include "mrob/node.hpp" 30 | 31 | namespace mrob{ 32 | 33 | class NodeLandmark3d : public Node 34 | { 35 | public: 36 | /** 37 | * For initialization, requires an initial estimation of the state. 38 | */ 39 | NodeLandmark3d(const Mat31 &initial_x, Node::nodeMode mode = STANDARD); 40 | //NodePose3d(const SE3 &initial_T); 41 | virtual ~NodeLandmark3d() override = default; 42 | 43 | virtual void update(const Eigen::Ref &dx); 44 | virtual void update_from_auxiliary(const Eigen::Ref &dx); 45 | virtual void set_state(const Eigen::Ref &x); 46 | virtual void set_auxiliary_state(const Eigen::Ref &x); 47 | virtual const Eigen::Ref get_state() const {return state_;}; 48 | virtual const Eigen::Ref get_auxiliary_state() const {return auxiliaryState_;}; 49 | void print() const; 50 | 51 | protected: 52 | Mat31 state_; 53 | Mat31 auxiliaryState_; 54 | 55 | public: 56 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // as proposed by Eigen 57 | 58 | 59 | }; 60 | 61 | 62 | } 63 | 64 | 65 | #endif /* NODELANDMARK3D_HPP_ */ 66 | -------------------------------------------------------------------------------- /src/FGraph/mrob/factors/nodePlane4d.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * nodePlane4d.hpp 17 | * 18 | * Created on: Oct 10, 2020 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #ifndef NODEPLANE4D_HPP_ 25 | #define NODEPLANE4D_HPP_ 26 | 27 | #include "mrob/matrix_base.hpp" 28 | #include "mrob/node.hpp" 29 | 30 | namespace mrob{ 31 | 32 | class NodePlane4d : public Node 33 | { 34 | public: 35 | /** 36 | * Node Plane 4d stands for the representation of planes as 4x1 vectors: 37 | * pi = [n', d] = [nx, ny, nz, d], s.t. ||n|| =1 38 | * 39 | * For initialization, requires an initial estimation of the state in global coordinates. 40 | * TODO: relative formulation of planes might be more convinient. 41 | * 42 | * Note that the dimensionality of this node is 4, that is the DOF, 43 | * althought we know that the normal lies in S^2, so that shrinks the 44 | * DOF to 3 in total. 45 | */ 46 | NodePlane4d(const Mat41 &initial_x, Node::nodeMode mode = STANDARD); 47 | virtual ~NodePlane4d() override = default; 48 | /** 49 | * Update of 4d is simply addition and a clamping process to guarantee unit normal 50 | * pi'= clamped(pi + dx) 51 | * 52 | */ 53 | virtual void update(const Eigen::Ref &dx) override; 54 | virtual void update_from_auxiliary(const Eigen::Ref &dx) override; 55 | virtual void set_state(const Eigen::Ref &x) override {state_ = x;}; 56 | virtual void set_auxiliary_state(const Eigen::Ref &x) override {auxiliaryState_ = x;}; 57 | virtual const Eigen::Ref get_state() const override {return state_;}; 58 | virtual const Eigen::Ref get_auxiliary_state() const override {return auxiliaryState_;}; 59 | virtual void print() const override; 60 | 61 | protected: 62 | Mat41 state_; 63 | Mat41 auxiliaryState_; //an auxiliary vector for undoing updates 64 | 65 | public: 66 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // as proposed by the Eigen library 67 | 68 | }; 69 | } 70 | #endif /* NODEPLANE4D_HPP_ */ 71 | -------------------------------------------------------------------------------- /src/FGraph/mrob/factors/nodePose2d.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * Created on: Jan 14, 2019 17 | * Author: Konstantin Pakulev 18 | * konstantin.pakulev@skoltech.ru 19 | * Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #ifndef MROB_NODEPOSE2D_H 25 | #define MROB_NODEPOSE2D_H 26 | 27 | #include "mrob/matrix_base.hpp" 28 | #include "mrob/node.hpp" 29 | 30 | namespace mrob{ 31 | 32 | 33 | /** 34 | * This class NodePose2d for now is parametrized on XYT coordinates 35 | * TODO compare with SE(2) 36 | */ 37 | class NodePose2d : public Node { 38 | public: 39 | /** 40 | * For initialization, requires an initial estimation of the state. 41 | */ 42 | explicit NodePose2d(const Mat31 &initial_x, Node::nodeMode mode = STANDARD); 43 | virtual ~NodePose2d() override = default; 44 | 45 | virtual void update(const Eigen::Ref &dx); 46 | virtual void update_from_auxiliary(const Eigen::Ref &dx); 47 | virtual void set_state(const Eigen::Ref &x); 48 | virtual void set_auxiliary_state(const Eigen::Ref &x); 49 | virtual const Eigen::Ref get_state() const {return state_;}; 50 | virtual const Eigen::Ref get_auxiliary_state() const {return auxiliaryState_;}; 51 | void print() const; 52 | protected: 53 | Mat31 state_; 54 | Mat31 auxiliaryState_; 55 | public: 56 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 57 | }; 58 | 59 | } 60 | 61 | #endif //MROB_NODEPOSE2D_H 62 | -------------------------------------------------------------------------------- /src/FGraph/mrob/factors/nodePose3d.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * nodePose3d.hpp 17 | * 18 | * Created on: Feb 28, 2018 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #ifndef NODEPOSE3D_HPP_ 25 | #define NODEPOSE3D_HPP_ 26 | 27 | #include "mrob/matrix_base.hpp" 28 | #include "mrob/SE3.hpp" //requires including and linking SE3 library 29 | #include "mrob/node.hpp" 30 | 31 | namespace mrob{ 32 | 33 | class NodePose3d : public Node 34 | { 35 | public: 36 | /** 37 | * For initialization, requires an initial estimation of the state. 38 | * For 3D poses we use a transformation matrix 4x4 39 | * 40 | * Note that the dimensionality of this node is 6, that is the DOF 41 | */ 42 | NodePose3d(const Mat4 &initial_x, Node::nodeMode mode = STANDARD); 43 | /** 44 | * Initialization directly on SE3 a matrix 45 | */ 46 | NodePose3d(const SE3 &initial_x, Node::nodeMode mode = STANDARD); 47 | ~NodePose3d() override = default; 48 | /** 49 | * Left update operation corresponds to 50 | * T'=exp(dxi^)*T 51 | * x'=vee(ln(T')) 52 | */ 53 | virtual void update(const Eigen::Ref &dx); 54 | virtual void update_from_auxiliary(const Eigen::Ref &dx); 55 | virtual void set_state(const Eigen::Ref &x); 56 | virtual void set_auxiliary_state(const Eigen::Ref &x); 57 | virtual const Eigen::Ref get_state() const {return state_.T();}; 58 | virtual const Eigen::Ref get_auxiliary_state() const {return auxiliaryState_.T();}; 59 | void print() const; 60 | 61 | protected: 62 | SE3 state_; 63 | SE3 auxiliaryState_; //an auxiliary vector for undoing updates 64 | 65 | public: 66 | //EIGEN_MAKE_ALIGNED_OPERATOR_NEW // as proposed by Eigen 67 | 68 | 69 | }; 70 | 71 | 72 | } 73 | 74 | 75 | #endif /* NODEPOSE3D_HPP_ */ 76 | -------------------------------------------------------------------------------- /src/FGraph/node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * node.cpp 17 | * 18 | * Created on: Feb 27, 2018 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #include "mrob/node.hpp" 25 | 26 | using namespace mrob; 27 | 28 | Node::Node(uint_t dim, nodeMode mode): 29 | id_(0), dim_(dim), node_mode_(mode) 30 | { 31 | } 32 | 33 | Node::~Node() 34 | { 35 | neighbourFactors_.clear(); 36 | } 37 | bool Node::add_factor(std::shared_ptr &factor) 38 | { 39 | neighbourFactors_.push_back(factor); 40 | return true; 41 | } 42 | 43 | bool Node::rm_factor(std::shared_ptr &factor) 44 | { 45 | // exhaustive line search over the vector, this SHOULD be small, right? 46 | // still, it is very ineffcient O(n), but we have preferred using a vector 47 | // over a set or a map because we are gonna iterate over this container, 48 | // while it is not so clear that we would want to remove factors (although possible) 49 | std::vector >::iterator f; 50 | for (f = neighbourFactors_.begin(); f != neighbourFactors_.end(); ++f) 51 | { 52 | if (*f == factor) 53 | break; 54 | } 55 | neighbourFactors_.erase(f); 56 | return true; 57 | } 58 | 59 | // support function for 2D poses 60 | double mrob::wrap_angle(double angle) 61 | { 62 | double pi2 = 2 * M_PI; 63 | 64 | while (angle < -M_PI) angle += pi2; 65 | while (angle >= M_PI) angle -= pi2; 66 | 67 | return angle; 68 | } 69 | -------------------------------------------------------------------------------- /src/FGraph/tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #ADD_EXECUTABLE(test_FGraph test_FGraph.cpp) 2 | #TARGET_LINK_LIBRARIES(test_FGraph FGraph) 3 | -------------------------------------------------------------------------------- /src/PCRegistration/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # locate the necessary dependencies, if any 2 | 3 | 4 | # extra source files 5 | SET(sources 6 | arun.cpp 7 | gicp.cpp 8 | plane.cpp 9 | plane_registration.cpp 10 | create_points.cpp 11 | weight_point.cpp 12 | estimate_plane.cpp 13 | factors/factor1PosePoint2Plane.cpp 14 | ) 15 | 16 | # extra header files 17 | SET(headers 18 | mrob/pc_registration.hpp 19 | mrob/plane.hpp 20 | mrob/plane_registration.hpp 21 | mrob/create_points.hpp 22 | mrob/estimate_plane.hpp 23 | mrob/factors/factor1PosePoint2Plane.hpp 24 | ) 25 | 26 | # create the shared library 27 | ADD_LIBRARY(PCRegistration SHARED ${sources} ) 28 | TARGET_LINK_LIBRARIES(PCRegistration SE3 FGraph common) 29 | 30 | 31 | ADD_SUBDIRECTORY(examples) 32 | #ADD_SUBDIRECTORY(test) 33 | -------------------------------------------------------------------------------- /src/PCRegistration/README.md: -------------------------------------------------------------------------------- 1 | # PCRegistration 2 | Point Cloud Registration. Different methods implemente for point cloud registration: 3 | * Arun, and SVD-based method (Arun'1983) 4 | * GICP 5 | * NICP? 6 | 7 | 8 | ## Dependencies 9 | C++'14, Eigen 10 | -------------------------------------------------------------------------------- /src/PCRegistration/estimate_plane.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * estimate_plane.cpp 16 | * 17 | * Created on: Dec 28, 2020 18 | * Author: Gonzalo Ferrer 19 | * g.ferrer@skoltech.ru 20 | * Mobile Robotics Lab. 21 | */ 22 | 23 | 24 | #include "mrob/estimate_plane.hpp" 25 | #include 26 | 27 | using namespace mrob; 28 | 29 | Mat41 estimate_plane_centered(const Eigen::Ref X); 30 | 31 | Mat41 mrob::estimate_plane(const Eigen::Ref X) 32 | { 33 | // Initialization 34 | assert(X.cols() == 3 && "Estimate_plane: Incorrect sizing, we expect Nx3"); 35 | assert(X.rows() >= 3 && "Estimate_plane: Incorrect sizing, we expect at least 3 correspondences (not aligned)"); 36 | 37 | // Plane estimation, centered approach 38 | return estimate_plane_centered(X); 39 | } 40 | 41 | //local function, we also will test the homogeneous plane estimation 42 | Mat41 estimate_plane_centered(const Eigen::Ref X) 43 | { 44 | uint_t N = X.rows(); 45 | // Calculate center of points: 46 | Mat13 c = X.colwise().sum(); 47 | c /= (double)N; 48 | 49 | 50 | MatX qx = X.rowwise() - c; 51 | Mat3 C = qx.transpose() * qx; 52 | 53 | 54 | Eigen::SelfAdjointEigenSolver eigs; 55 | eigs.computeDirect(C); 56 | 57 | Mat31 normal = eigs.eigenvectors().col(0); 58 | 59 | matData_t d = - c*normal; 60 | 61 | Mat41 plane; 62 | plane << normal, d; 63 | // error = eigs.eigenvalues()(0); 64 | return plane; 65 | 66 | } 67 | 68 | 69 | 70 | Mat31 mrob::estimate_normal(const Eigen::Ref X) 71 | { 72 | Mat41 res = estimate_plane(X); 73 | return res.head(3); 74 | } 75 | 76 | 77 | Mat31 mrob::estimate_centroid(const Eigen::Ref X) 78 | { 79 | // Initialization 80 | assert(X.cols() == 3 && "Estimate_centroid: Incorrect sizing, we expect Nx3"); 81 | assert(X.rows() >= 3 && "Estimate_centroid: Incorrect sizing, we expect at least 3 correspondences (not aligned)"); 82 | 83 | uint_t N = X.rows(); 84 | Mat13 c = X.colwise().sum(); 85 | c /= (double)N; 86 | return c; 87 | } 88 | -------------------------------------------------------------------------------- /src/PCRegistration/examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_EXECUTABLE(example_align_mth example_align_mth.cpp) 2 | TARGET_LINK_LIBRARIES(example_align_mth PCRegistration) 3 | 4 | ADD_EXECUTABLE(example_synthetic_plane_traj example_synthetic_plane_traj.cpp) 5 | TARGET_LINK_LIBRARIES(example_synthetic_plane_traj PCRegistration) 6 | 7 | ADD_EXECUTABLE(example_fgraph_align example_fgraph_align.cpp) 8 | TARGET_LINK_LIBRARIES(example_fgraph_align FGraph PCRegistration) 9 | -------------------------------------------------------------------------------- /src/PCRegistration/examples/example_align_mth.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * example_align_mth.cpp 17 | * 18 | * Created on: Feb 2, 2018 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | 25 | #include 26 | #include "mrob/pc_registration.hpp" 27 | 28 | 29 | int main() 30 | { 31 | // test data structures 32 | int N = 4; 33 | mrob::MatX X = mrob::MatX::Random(N,3); 34 | std::cout << "X data: \n" << X << std::endl; 35 | 36 | mrob::Mat61 xi = mrob::Mat61::Random(); 37 | //Mat61 xi = Mat61::Zero(); 38 | mrob::SE3 T(xi); 39 | std::cout << "T random transformation: \n" << std::endl; 40 | T.print(); 41 | 42 | 43 | mrob::MatX Y = T.transform_array(X); 44 | std::cout << "Y data: \n" << Y << std::endl; 45 | 46 | mrob::SE3 T_arun; 47 | mrob::PCRegistration::arun(X,Y,T_arun); 48 | std::cout << "T solved by Arun method: \n" << std::endl; 49 | T_arun.print(); 50 | 51 | // Solve for GICP 52 | mrob::MatX covX(3*N,3); 53 | mrob::MatX covY(3*N,3); 54 | for (int i = 0; i < N ; ++i) 55 | { 56 | covX.block<3,3>(3*i,0) = mrob::Mat3::Identity(); 57 | covY.block<3,3>(3*i,0) = mrob::Mat3::Identity(); 58 | } 59 | mrob::SE3 T_gicp; 60 | int iters = mrob::PCRegistration::gicp(X,Y,covX,covY,T_gicp); 61 | std::cout << "T solved by GICP method on " << iters << " iters: \n" << std::endl; 62 | T_gicp.print(); 63 | 64 | 65 | // Solve for weighted point optimization 66 | mrob::MatX1 weight = mrob::MatX1::Ones(N); 67 | mrob::SE3 T_wp; 68 | iters = mrob::PCRegistration::weighted_point(X,Y,weight,T_wp);//TODO change to work with Nx3 convention 69 | std::cout << "T solved by Weight point Optimization method on " << iters << " iters: \n" << std::endl; 70 | T_wp.print(); 71 | 72 | 73 | //align_mth::Csample_uniform_SE3 s(0.3, 4); 74 | //std::cout << "Csample = " << s.sample() << std::endl; 75 | 76 | 77 | 78 | 79 | return 1; 80 | } 81 | 82 | -------------------------------------------------------------------------------- /src/PCRegistration/examples/example_fgraph_align.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * example_fgraph_align.cpp 16 | * 17 | * Created on: Jan 15, 2021 18 | * Author: Gonzalo Ferrer 19 | * g.ferrer@skoltech.ru 20 | * Mobile Robotics Lab. 21 | */ 22 | 23 | 24 | 25 | #include "mrob/factor_graph_solve.hpp" 26 | #include "mrob/factors/nodePose3d.hpp" 27 | #include "mrob/factors/factor1PosePoint2Plane.hpp" 28 | 29 | 30 | 31 | #include 32 | 33 | int main () 34 | { 35 | mrob::FGraphSolve graph(mrob::FGraphSolve::ADJ); 36 | 37 | std::shared_ptr n0(new mrob::NodePose3d(mrob::SE3())); 38 | graph.add_node(n0); 39 | 40 | 41 | // add observations, for now just random things 42 | mrob::Mat1 W; 43 | W << 1.0; 44 | 45 | for (int t = 0; t < 10; t++) 46 | { 47 | mrob::Mat31 z_normal_y = mrob::Mat31::Random(); 48 | mrob::Mat31 z_point_y = mrob::Mat31::Random(); 49 | mrob::Mat31 z_point_x = mrob::Mat31::Random(); 50 | std::shared_ptr f1(new mrob::Factor1PosePoint2Plane(z_point_x,z_point_y, z_normal_y,n0,W)); 51 | graph.add_factor(f1); 52 | } 53 | graph.print(); 54 | 55 | // solve the graph 56 | double chi2 = graph.chi2(); 57 | std::cout << "Current error = " << chi2; 58 | 59 | // Evaluate the graph 60 | graph.solve(); 61 | 62 | // solve the graph 63 | std::cout << "Current error = " << graph.chi2(); 64 | } 65 | -------------------------------------------------------------------------------- /src/PCRegistration/examples/example_synthetic_plane_traj.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * example_synthetic_plane_traj.cpp 17 | * 18 | * Created on: Feb 8, 2019 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | 25 | #include "mrob/create_points.hpp" 26 | #include "mrob/plane_registration.hpp" 27 | #include 28 | 29 | 30 | using namespace mrob; 31 | 32 | int main() 33 | { 34 | int numPlanes = 4, numPoses = 2; 35 | // 1) define problem conditions 36 | mrob::CreatePoints scene(40,numPlanes,numPoses,0.001, 0.1); 37 | //scene.print(); 38 | 39 | mrob::PlaneRegistration contPlanes; 40 | scene.create_plane_registration(contPlanes); 41 | contPlanes.print(); 42 | 43 | 44 | // 3) evaluate alignment 45 | contPlanes.solve(PlaneRegistration::SolveMode::GRADIENT); 46 | contPlanes.print(false); 47 | contPlanes.solve(PlaneRegistration::SolveMode::GRADIENT); 48 | contPlanes.print(false); 49 | contPlanes.solve(PlaneRegistration::SolveMode::GRADIENT); 50 | contPlanes.print(false); 51 | contPlanes.solve(PlaneRegistration::SolveMode::GRADIENT); 52 | contPlanes.solve(PlaneRegistration::SolveMode::GRADIENT); 53 | contPlanes.solve(PlaneRegistration::SolveMode::GRADIENT); 54 | contPlanes.solve(PlaneRegistration::SolveMode::GRADIENT); 55 | contPlanes.solve(PlaneRegistration::SolveMode::GRADIENT); 56 | contPlanes.solve(PlaneRegistration::SolveMode::GRADIENT); 57 | contPlanes.solve(PlaneRegistration::SolveMode::GRADIENT); 58 | contPlanes.solve(PlaneRegistration::SolveMode::GRADIENT); 59 | contPlanes.solve(PlaneRegistration::SolveMode::GRADIENT); 60 | contPlanes.solve(PlaneRegistration::SolveMode::GRADIENT); 61 | contPlanes.solve(PlaneRegistration::SolveMode::GRADIENT); 62 | contPlanes.solve(PlaneRegistration::SolveMode::GRADIENT); 63 | contPlanes.solve(PlaneRegistration::SolveMode::GRADIENT); 64 | contPlanes.solve(PlaneRegistration::SolveMode::GRADIENT); 65 | contPlanes.solve(PlaneRegistration::SolveMode::GRADIENT); 66 | contPlanes.print(false); 67 | 68 | 69 | return 1; 70 | } 71 | -------------------------------------------------------------------------------- /src/PCRegistration/factors/factor1PosePoint2Plane.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * factor1PosePoint2Plane.cpp 16 | * 17 | * Created on: Dec 24, 2020 18 | * Author: Gonzalo Ferrer 19 | * g.ferrer@skoltech.ru 20 | * Mobile Robotics Lab. 21 | */ 22 | 23 | #include "mrob/factors/factor1PosePoint2Plane.hpp" 24 | 25 | #include 26 | 27 | 28 | using namespace mrob; 29 | 30 | Factor1PosePoint2Plane::Factor1PosePoint2Plane(const Mat31 &z_point_x, const Mat31 &z_point_y, const Mat31 &z_normal_y, 31 | std::shared_ptr &node, const Mat1 &obsInf, Factor::robustFactorType robust_type): 32 | Factor(1,6, robust_type), z_point_x_(z_point_x), z_point_y_(z_point_y), Tx_(Mat31::Zero()), z_normal_y_(z_normal_y), r_(0.0), W_(obsInf) 33 | { 34 | neighbourNodes_.push_back(node); 35 | } 36 | 37 | 38 | Factor1PosePoint2Plane::~Factor1PosePoint2Plane() 39 | { 40 | } 41 | 42 | 43 | void Factor1PosePoint2Plane::evaluate_residuals() 44 | { 45 | // r = 46 | Mat4 Tx = get_neighbour_nodes()->at(0)->get_state(); 47 | SE3 T = SE3(Tx); 48 | Tx_ = T.transform(z_point_x_); 49 | r_ = Mat1(z_normal_y_.dot(Tx_ - z_point_y_)); 50 | } 51 | 52 | void Factor1PosePoint2Plane::evaluate_jacobians() 53 | { 54 | // it assumes you already have evaluated residuals 55 | Mat<3,6> Jr; 56 | Jr << -hat3(Tx_) , Mat3::Identity(); 57 | J_ = z_normal_y_.transpose() * Jr; 58 | } 59 | 60 | void Factor1PosePoint2Plane::evaluate_chi2() 61 | { 62 | chi2_ = 0.5 * r_.dot(W_ * r_); 63 | } 64 | 65 | void Factor1PosePoint2Plane::print() const 66 | { 67 | std::cout << "Printing Factor: " << id_ << ", obs point x= \n" << z_point_x_ 68 | << "\nobs point y =\n" << z_point_y_ 69 | << "\nobs normal y =\n" << z_normal_y_ 70 | << "\n Residuals= \n" << r_ 71 | << " \nand Information matrix\n" << W_ 72 | << "\n Calculated Jacobian = \n" << J_ 73 | << "\n Chi2 error = " << chi2_ 74 | << " and neighbour Node ids: " << neighbourNodes_[0]->get_id() 75 | << std::endl; 76 | } 77 | -------------------------------------------------------------------------------- /src/PCRegistration/factors/factor1PosePoint2Point.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * factor1PosePoint2Point.cpp 16 | * 17 | * Created on: Dec 29, 2020 18 | * Author: Gonzalo Ferrer 19 | * g.ferrer@skoltech.ru 20 | * Mobile Robotics Lab. 21 | */ 22 | 23 | 24 | #include "mrob/factors/factor1PosePoint2Point.hpp" 25 | 26 | #include 27 | 28 | 29 | using namespace mrob; 30 | 31 | Factor1PosePoint2Point::Factor1PosePoint2Point(const Mat31 &z_point_x, const Mat31 &z_point_y, std::shared_ptr &node, 32 | const Mat1 &obsInf, Factor::robustFactorType robust_type): 33 | Factor(3,6,robust_type), z_point_x_(z_point_x), z_point_y_(z_point_y_), W_(obsInf) 34 | { 35 | neighbourNodes_.push_back(node); 36 | } 37 | 38 | 39 | Factor1PosePoint2Point::~Factor1PosePoint2Point() 40 | { 41 | 42 | } 43 | 44 | void Factor1PosePoint2Point::evaluate_residuals() 45 | { 46 | // r = Tx - y 47 | Mat4 Tnode = get_neighbour_nodes()->at(0)->get_state(); 48 | SE3 T = SE3(Tnode); 49 | Tx_ = T.transform(z_point_x_); 50 | r_ = Tx_ - z_point_y_; 51 | } 52 | 53 | void Factor1PosePoint2Point::evaluate_jacobians() 54 | { 55 | // it assumes you already have evaluated residuals 56 | J_ << -hat3(Tx_) , Mat3::Identity(); 57 | } 58 | 59 | void Factor1PosePoint2Point::evaluate_chi2() 60 | { 61 | chi2_ = 0.5 * r_.dot(W_ * r_); 62 | } 63 | 64 | void Factor1PosePoint2Point::print() const 65 | { 66 | std::cout << "Printing Factor: " << id_ << ", obs point x= \n" << z_point_x_ 67 | << "\nobs point y =\n" << z_point_y_ 68 | << "\n Residuals= \n" << r_ 69 | << " \nand Information matrix\n" << W_ 70 | << "\n Calculated Jacobian = \n" << J_ 71 | << "\n Chi2 error = " << chi2_ 72 | << " and neighbour Node ids: " << neighbourNodes_[0]->get_id() 73 | << std::endl; 74 | } 75 | 76 | -------------------------------------------------------------------------------- /src/PCRegistration/gicp.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * Gicp.cpp 17 | * 18 | * Created on: Jan 31, 2018 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #include // for inverse and determinant 25 | 26 | #include 27 | #include 28 | #include "mrob/pc_registration.hpp" // GICP function is defined here 29 | 30 | 31 | using namespace mrob; 32 | 33 | int PCRegistration::gicp(const Eigen::Ref X, const Eigen::Ref Y, 34 | const Eigen::Ref covX, const Eigen::Ref covY, SE3 &T, double tol) 35 | { 36 | assert(X.cols() == 3 && "PCRegistration::Gicp: Incorrect sizing, we expect Nx3"); 37 | assert(X.rows() >= 3 && "PCRegistration::Gicp: Incorrect sizing, we expect at least 3 correspondences (not aligned)"); 38 | assert(Y.rows() == X.rows() && "PCRegistration::Gicp: Same number of correspondences"); 39 | uint_t N = X.rows(); 40 | // TODO precalculation of T by reduced Arun 41 | // TODO different number of iterations and convergence criterion 42 | 43 | // Initialize Jacobian and Hessian 44 | Mat61 J = Mat61::Zero(); 45 | Mat6 H = Mat6::Zero(); 46 | uint_t iters = 0; 47 | double deltaUpdate = 1e3; 48 | do 49 | { 50 | J.setZero(); 51 | H.setZero(); 52 | // not vectoried operations (due to Jacobian) 53 | for ( uint_t i = 0; i < N ; ++i) 54 | { 55 | // 1) Calculate residual r = y - Tx and the inverse of joint covariance 56 | Mat31 Txi = T.transform(X.row(i)); 57 | Mat31 r = Y.row(i).transpose() - Txi; 58 | Mat3 Li = (covY.block<3,3>(3*i,0) + T.R() * covX.block<3,3>(3*i,0) * T.R().transpose()).inverse(); 59 | 60 | // 2) Calculate Jacobian for residual Jf = df1/d xi = r1' Li * Jr, where Jr = [(Tx)^ ; -I]) 61 | Mat<3,6> Jr; 62 | Jr << hat3(Txi) , -Mat3::Identity(); 63 | Mat<1,6> Ji = r.transpose() * Li * Jr; 64 | J += Ji;//Eigen manages this for us 65 | 66 | // 3) Hessian Hi ~ Jr' * Li * Jr 67 | Mat6 Hi = Jr.transpose() * Li * Jr; 68 | H += Hi; 69 | } 70 | // 4) Update Solution 71 | Mat61 dxi = -H.inverse()*J; 72 | T.update_lhs(dxi); //Left side update 73 | deltaUpdate = dxi.norm(); 74 | iters++; 75 | 76 | }while(deltaUpdate > tol && iters < 20); 77 | 78 | return iters; // number of iterations 79 | } 80 | -------------------------------------------------------------------------------- /src/PCRegistration/mrob/estimate_plane.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * estimate_plane.hpp 16 | * 17 | * Created on: Dec 28, 2020 18 | * Author: Gonzalo Ferrer 19 | * g.ferrer@skoltech.ru 20 | * Mobile Robotics Lab. 21 | */ 22 | 23 | //TODO this file name should be changed since it provides more functionaloties for PCs 24 | #ifndef ESTIMATE_PLANE_HPP_ 25 | #define ESTIMATE_PLANE_HPP_ 26 | 27 | #include "mrob/matrix_base.hpp" 28 | 29 | namespace mrob{ 30 | 31 | /** 32 | * Estimate plane: given a set of points, in a Nx3 array, 33 | * calculates the plane as 34 | * pi = [n, 35 | * d] \in P^3 36 | * 37 | * For now this is a unique function, TODO: add more methods here 38 | */ 39 | 40 | Mat41 estimate_plane(const Eigen::Ref X); 41 | 42 | 43 | /** 44 | * Estimate normal: given a set of points, in a Nx3 array, 45 | * calculates the noamrl plane 46 | * n \in R^3 47 | * 48 | */ 49 | 50 | Mat31 estimate_normal(const Eigen::Ref X); 51 | 52 | 53 | /** 54 | * Estimate centroid: given a set of points, in a Nx3 array, 55 | * calculates the centroid point 56 | * p \in R^3 57 | * 58 | */ 59 | 60 | Mat31 estimate_centroid(const Eigen::Ref X); 61 | 62 | 63 | } 64 | 65 | #endif /* ESTIMATE_PLANE_HPP_ */ 66 | -------------------------------------------------------------------------------- /src/PCRegistration/mrob/factors/factor1PosePoint2Plane.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * factor1PosePoint2Plane.hpp 16 | * 17 | * Created on: Dec 24, 2020 18 | * Author: Gonzalo Ferrer 19 | * g.ferrer@skoltech.ru 20 | * Mobile Robotics Lab. 21 | */ 22 | 23 | #ifndef FACTOR1POSEPOINT2PLANE_HPP_ 24 | #define FACTOR1POSEPOINT2PLANE_HPP_ 25 | 26 | #include "mrob/matrix_base.hpp" 27 | #include "mrob/SE3.hpp" //requires including and linking SE3 library 28 | #include "mrob/factor.hpp" 29 | 30 | namespace mrob{ 31 | 32 | /** 33 | * Factor1PoseToPlaneAndPoint is a vertex in the Fgraph library that models 34 | * the distribution of observations of a 3D pose. The purpose of this factors is to estimate 35 | * the relative transformation from two different sets of point clouds, given some planar constraaints. 36 | * Mainly, it will include a pointcloud from images (chessboard) after solving the PnP (external) 37 | * and any sensor providing depth. 38 | * 39 | * Observations 40 | * - Point x 41 | * - Point y, normal n_y, from reference y 42 | * 43 | * State to estimate is 3D pose y^T_x. 44 | * 45 | * The residual, as a convention in the library is: 46 | * r = f(x) = n_y' * (Tx - y) -> 0 if the relative point is in the plane. 47 | * 48 | * The Jacobian is then 49 | * dr = n_y' [-(Tp)^, I] 50 | * 51 | * TODO: correctly characterize the covariance, since this is a contribution from 2 rv 52 | */ 53 | 54 | class Factor1PosePoint2Plane: public Factor 55 | { 56 | public: 57 | Factor1PosePoint2Plane(const Mat31 &z_point_x, const Mat31 &z_point_y, const Mat31 &z_normal_y, 58 | std::shared_ptr &node, const Mat1 &obsInf, 59 | Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC); 60 | ~Factor1PosePoint2Plane(); 61 | /** 62 | * Jacobians are not evaluated, just the residuals 63 | * r = 64 | * 65 | * The transformation T is, thus, the transformation from the point reference to the plane reference 66 | */ 67 | virtual void evaluate_residuals() override; 68 | /** 69 | * Evaluates residuals and Jacobians 70 | * J = dr/dxi = pi'*[-Tp^. I] 71 | */ 72 | virtual void evaluate_jacobians() override; 73 | virtual void evaluate_chi2() override; 74 | 75 | virtual void print() const; 76 | 77 | virtual const Eigen::Ref get_obs() const {return r_;}; 78 | virtual const Eigen::Ref get_residual() const {return r_;}; 79 | virtual const Eigen::Ref get_information_matrix() const {return W_;}; 80 | virtual const Eigen::Ref get_jacobian() const {return J_;}; 81 | 82 | protected: 83 | Mat31 z_point_x_, z_point_y_, Tx_; 84 | Mat31 z_normal_y_; 85 | // the residual of the point projected to the plane: 86 | Mat1 r_;//residual, for convention, we will keep an eigen object for the scalar 87 | Mat1 W_; 88 | Mat<1,6> J_; 89 | }; 90 | 91 | } 92 | 93 | #endif /* FACTOR1POSEPoint2Plane_HPP_ */ 94 | -------------------------------------------------------------------------------- /src/PCRegistration/mrob/factors/factor1PosePoint2Point.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * factor1PosePoint2Point.hpp 16 | * 17 | * Created on: Dec 29, 2020 18 | * Author: Gonzalo Ferrer 19 | * g.ferrer@skoltech.ru 20 | * Mobile Robotics Lab. 21 | */ 22 | 23 | #ifndef FACTORS_FACTOR1POSEPOINT2POINT_HPP_ 24 | #define FACTORS_FACTOR1POSEPOINT2POINT_HPP_ 25 | 26 | #include "mrob/matrix_base.hpp" 27 | #include "mrob/SE3.hpp" //requires including and linking SE3 library 28 | #include "mrob/factor.hpp" 29 | 30 | namespace mrob{ 31 | 32 | /** 33 | * Factor1PoseToPpoint2Point is a vertex in the Fgraph library that models 34 | * the distribution of observations of a 3D pose. The purpose of this factors is to estimate 35 | * the relative transformation from two different sets of point clouds, given some planar constraaints. 36 | * 37 | * Observations 38 | * - Point x 39 | * - Point y, from reference y 40 | * 41 | * State to estimate is 3D pose y^T_x. 42 | * 43 | * The residual, as a convention in the library is: 44 | * r = f(x) = (Tx - y) -> 0 if the points are coincident. 45 | * 46 | * The Jacobian is then 47 | * dr = [-(Tx)^, I] 48 | * 49 | * TODO: correctly characterize the covariance, since this is a contribution from 2 rv 50 | */ 51 | 52 | class Factor1PosePoint2Point : public Factor 53 | { 54 | public: 55 | Factor1PosePoint2Point(const Mat31 &z_point_x, const Mat31 &z_point_y, std::shared_ptr &node, 56 | const Mat1 &obsInf, Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC); 57 | ~Factor1PosePoint2Point(); 58 | /** 59 | * Jacobians are not evaluated, just the residuals 60 | */ 61 | virtual void evaluate_residuals() override; 62 | /** 63 | * Evaluates residuals and Jacobians 64 | * J = dr/dxi = [-Tx^. I] 65 | */ 66 | virtual void evaluate_jacobians() override; 67 | virtual void evaluate_chi2() override; 68 | 69 | virtual void print() const; 70 | 71 | virtual const Eigen::Ref get_obs() const {return r_;}; 72 | virtual const Eigen::Ref get_residual() const {return r_;}; 73 | virtual const Eigen::Ref get_information_matrix() const {return W_;}; 74 | virtual const Eigen::Ref get_jacobian() const {return J_;}; 75 | 76 | protected: 77 | Mat31 z_point_x_, z_point_y_, Tx_; 78 | Mat31 r_; 79 | Mat3 W_; 80 | Mat<3,6> J_; 81 | }; 82 | 83 | } 84 | 85 | 86 | 87 | #endif /* FACTORS_FACTOR1POSEPOINT2POINT_HPP_ */ 88 | -------------------------------------------------------------------------------- /src/PCRegistration/mrob/loam.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * loam.hpp 17 | * 18 | * Created on: Feb 1, 2019 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #ifndef SRC_PCREGISTRATION_MROB_LOAM_HPP_ 25 | #define SRC_PCREGISTRATION_MROB_LOAM_HPP_ 26 | 27 | 28 | /** 29 | * This is an implementation of the LOAM work 30 | * to benchmark with our plane smoother factor 31 | * 32 | */ 33 | 34 | 35 | #endif /* SRC_PCREGISTRATION_MROB_LOAM_HPP_ */ 36 | -------------------------------------------------------------------------------- /src/PCRegistration/mrob/pc_registration.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * PCRegistration.hpp 17 | * 18 | * Created on: Jan 22, 2019 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #ifndef PC_REGISTRATION_HPP_ 25 | #define PC_REGISTRATION_HPP_ 26 | 27 | #include "mrob/matrix_base.hpp" 28 | #include "mrob/SE3.hpp" 29 | 30 | namespace mrob{ 31 | /** 32 | * This header provides the available functions for Point Cloud Registration using the 33 | * following methods. It assumes Data association and a correct index order for associated pairs. 34 | * 35 | * These set of methods calculates the transformation 36 | * T = [R t] between sets of points x in R^3 and y in R^3, such as: 37 | * [0 1], 38 | * where yh = T*xh and 39 | * being xh and yh the homogeneous description of those points [x y z 1]. 40 | */ 41 | namespace PCRegistration{ 42 | 43 | /** 44 | * This solution is based on the paper by Arun et al. "Least-squares fitting of two 3-D point sets", 1987 45 | * Given N points x = x_1,x_2,x_3 and their correspondences y_i, calculate the 46 | * transformation T = [R t], such as: min sum (y_i - (R * x_i + t))^2 47 | * 48 | * An improvement over it was proposed by Sinji Umeyama 1991, which uniqueness determine R 49 | * 50 | * 3 points in x and y are the minimum requirements, and provide the right solution IF 51 | * there is no noise. 52 | * 53 | * Returns 0 if failed and 1 if a correct solution was found 54 | */ 55 | int arun(const Eigen::Ref X, const Eigen::Ref Y, SE3 &T); 56 | 57 | 58 | 59 | 60 | /** 61 | * Custom implementation of the GICP using SE3 optimization (improvement over Euler rotations) 62 | * 63 | * This method calculates the transformation 64 | * T = [R t] between sets of points x in R^3 and y in R^3, such as: 65 | * [0 1], 66 | * where yh = R*xh + t 67 | * 68 | * 69 | * T = min sum || y - Tx ||_S^2 70 | * 71 | * The covariances provided are of the form S = R diag(e,1,1) R', so they MUST have been already processed. 72 | * The right way is a block matrix of covariances of the form Cov = [Cov_1, Cov_2,..., Cov_N], i.e, Cov \in R^{3x3N} 73 | * 74 | * Returns the number of iterations until convergence 75 | */ 76 | int gicp(const Eigen::Ref X, const Eigen::Ref Y, 77 | const Eigen::Ref covX, const Eigen::Ref covY, SE3 &T, double tol = 1e-4); 78 | 79 | 80 | /** 81 | * Weighted point Cloud registration using SE3 optimization 82 | * 83 | * This method calculates the transformation 84 | * T = [R t] between sets of points x in R^3 and y in R^3, such as: 85 | * [0 1], 86 | * where yh = R*xh + t 87 | * 88 | * 89 | * T = min sum w|| y - Tx ||^2 90 | * 91 | * The weights provided are of the form 92 | * 93 | * Returns the number of iterations until convergence 94 | */ 95 | int weighted_point(const Eigen::Ref X, const Eigen::Ref Y, 96 | const Eigen::Ref w, SE3 &T, double tol = 1e-4); 97 | 98 | 99 | }}//namespace 100 | #endif /* PC_REGISTRATION_HPP_ */ 101 | -------------------------------------------------------------------------------- /src/PCRegistration/weight_point.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * weight_point.cpp 17 | * 18 | * Created on: April 5, 2019 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab, Skoltech 22 | */ 23 | 24 | #include // for inverse and determinant 25 | 26 | #include 27 | #include 28 | #include "mrob/pc_registration.hpp" 29 | 30 | 31 | using namespace mrob; 32 | 33 | int PCRegistration::weighted_point(const Eigen::Ref X, const Eigen::Ref Y, 34 | const Eigen::Ref weight, SE3 &T, double tol) 35 | { 36 | assert(X.cols() == 3 && "PCRegistration::Gicp: Incorrect sizing, we expect Nx3"); 37 | assert(X.rows() >= 3 && "PCRegistration::Gicp: Incorrect sizing, we expect at least 3 correspondences (not aligned)"); 38 | assert(Y.rows() == X.rows() && "PCRegistration::Gicp: Same number of correspondences"); 39 | uint_t N = X.rows(); 40 | // TODO precalculation of T by reduced Arun 41 | 42 | // Initialize Jacobian and Hessian 43 | Mat61 J = Mat61::Zero(); 44 | Mat6 H = Mat6::Zero(); 45 | uint_t iters = 0; 46 | double deltaUpdate = 1e3; 47 | do 48 | { 49 | J.setZero(); 50 | H.setZero(); 51 | // not vectoried operations (due to Jacobian) 52 | for ( uint_t i = 0; i < N ; ++i) 53 | { 54 | // 1) Calculate residual r = y - Tx and the inverse of joint covariance 55 | Mat31 Txi = T.transform(X.row(i)); 56 | Mat31 r = Y.row(i).transpose() - Txi; 57 | 58 | // 2) Calculate Jacobian for residual Jf = df/d xi = w * r' Jr, where Jr = [(Tx)^ ; -I]) 59 | Mat<3,6> Jr; 60 | Jr << hat3(Txi) , -Mat3::Identity(); 61 | Mat<1,6> Ji = weight(i) * r.transpose() * Jr; 62 | J += Ji;//Eigen manages this for us 63 | 64 | // 3) Hessian Hi ~ w * Jr' * Jr 65 | Mat6 Hi = weight(i) * Jr.transpose() * Jr; 66 | H += Hi; 67 | } 68 | // 4) Update Solution 69 | Mat61 dxi = -H.inverse()*J; 70 | T.update_lhs(dxi); //Left side update 71 | deltaUpdate = dxi.norm(); 72 | iters++; 73 | 74 | }while(deltaUpdate > tol && iters < 20); 75 | 76 | return iters; // number of iterations 77 | } 78 | -------------------------------------------------------------------------------- /src/README.md: -------------------------------------------------------------------------------- 1 | # MROB Cpp source 2 | Cpp code is included here, that is, most of the algorithms and structures. 3 | 4 | 5 | ## Coding conventions 6 | Coding conventions are necessary to maintain homogeneity and readability across all the project. Here are some conventions that we _should_ follow: 7 | 8 | * BSD/Allman conventions: -like, ie. brace on the next line from a control statement, indented on the same level. In switch-case statements the cases are on the same indent level as the switch statement. 9 | * Indents use 4 spaces instead of tabs. Tabs are not used. 10 | * Class and struct names camel-case and beginning with an uppercase letter. For example `BaseClass`, `Arun`. 11 | * Variables are in lower case, either camel-case `odometryObs` or separated by underscores `odometry_obs`. Member variables have an underscore appended `localNodes_`. For matrices, in order to maintain a coherent mathematical description, variables may be upper case. 12 | * Functions are in lower case, and can be separated by underscores, e.g. `solve_iterative()` or in lower camel-case, what is important is to be as descriptive as possible, not contracting names for the sake of understandability. 13 | * File names: Should be all lowercase, and can be separated by underscores, `example_align_methods.cpp` 14 | * Constants and enumerations are in uppercase. For example `M_PI`. 15 | * Class definitions proceed in the following order: 16 | 17 | - public enums 18 | - public constructors and the destructor 19 | - public virtual functions 20 | - public non-virtual member functions 21 | - public static functions 22 | - public member variables 23 | - public static variables 24 | - repeat all of the above in order for protected definitions, and finally private 25 | * Header files are commented using one-line comments beginning with / &ast &ast to mark them, comments are important. 26 | 27 | 28 | ### Eclipse 29 | for Eclipse users: 30 | * Make: on project, properties, C++build, select the build command `make -C ${projDirPath}./build` 31 | * Find Eigen symbols: project, properties, C++ general, paths and symbols: add Eigen source, for example at /usr/local/include/eigen3 32 | * Update template: windows - preferences - C/C++ - code style - templates 33 | -------------------------------------------------------------------------------- /src/VIO/README.md: -------------------------------------------------------------------------------- 1 | # Visual Intertial Odometry 2 | Directory that *will* containt the VIO implementation, this include factors to IMU, back end for visual information. 3 | -------------------------------------------------------------------------------- /src/common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # locate the necessary dependencies, if any 2 | 3 | # extra header files 4 | SET(headers 5 | mrob/time_profiling.hpp 6 | mrob/optimizer.hpp 7 | ) 8 | 9 | # extra source files 10 | SET(sources 11 | time_profiling.cpp 12 | optimizer.cpp 13 | ) 14 | # create the shared library 15 | ADD_LIBRARY(common SHARED ${sources}) 16 | -------------------------------------------------------------------------------- /src/common/README.md: -------------------------------------------------------------------------------- 1 | # common 2 | Common strucutres and data conventions taken acrros all project. This includes: 3 | * typedef of Eigfen matrrices to Mat1, Mat31 (vector 3x1), MatX, etc. 4 | * typedef of some data types. 5 | 6 | ## Dependencies 7 | C++'14, Eigen 8 | -------------------------------------------------------------------------------- /src/common/mrob/matrix_base.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * time_profiling.jpp 17 | * 18 | * Created on: Aug 14, 2019 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab. 22 | */ 23 | 24 | #ifndef MATRIX_BASE_HPP_ 25 | #define MATRIX_BASE_HPP_ 26 | 27 | //#include // overkill including everything, Specific dependencies will be set at each module 28 | #include 29 | //#include // for inverse and determinant 30 | #include 31 | 32 | #define EIGEN_DEFAULT_TO_ROW_MAJOR 33 | 34 | 35 | 36 | namespace mrob{ 37 | 38 | // data types conventions 39 | typedef double matData_t; 40 | typedef unsigned int uint_t; 41 | typedef std::size_t factor_id_t; 42 | 43 | // Definition of squared matrices, by default column major 44 | typedef Eigen::Matrix Mat1; 45 | typedef Eigen::Matrix Mat2; 46 | typedef Eigen::Matrix Mat3; 47 | typedef Eigen::Matrix Mat4; 48 | typedef Eigen::Matrix Mat5; 49 | typedef Eigen::Matrix Mat6; 50 | typedef Eigen::Matrix MatX; 51 | 52 | //Sparse Matrices 53 | typedef Eigen::SparseMatrix SMatCol; 54 | typedef Eigen::SparseMatrix SMatRow; 55 | typedef Eigen::Triplet Triplet; 56 | 57 | // Definition of column matrices (vectors) 58 | typedef Eigen::Matrix Mat21; 59 | typedef Eigen::Matrix Mat31; 60 | typedef Eigen::Matrix Mat41; 61 | typedef Eigen::Matrix Mat51; 62 | typedef Eigen::Matrix Mat61; 63 | typedef Eigen::Matrix MatX1; 64 | 65 | // Definition of row matrices (vectors) 66 | typedef Eigen::Matrix Mat12; 67 | typedef Eigen::Matrix Mat13; 68 | typedef Eigen::Matrix Mat14; 69 | typedef Eigen::Matrix Mat15; 70 | typedef Eigen::Matrix Mat16; 71 | typedef Eigen::Matrix Mat1X; 72 | 73 | 74 | // Definition of templated-based fixed matrices using c'11 aliases 75 | template 76 | using Vect = Eigen::Matrix; 77 | template 78 | using Mat = Eigen::Matrix; 79 | 80 | 81 | 82 | }//end of namespace 83 | 84 | #endif /* MATRIX_BASE_HPP_ */ 85 | -------------------------------------------------------------------------------- /src/common/mrob/time_profiling.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * time_profiling.hpp 17 | * 18 | * Created on: Aug 14, 2019 19 | * Author: Gonzalo Ferrer 20 | */ 21 | 22 | #ifndef TIME_PROFILING_HPP_ 23 | #define TIME_PROFILING_HPP_ 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | 30 | namespace mrob { 31 | typedef std::chrono::microseconds Ttim; 32 | 33 | 34 | /** 35 | * Class TimeProfiling creates a simple object that stores time 36 | * profiles for different functions and displays them. 37 | */ 38 | 39 | 40 | class TimeProfiling 41 | { 42 | public: 43 | /** 44 | * Constructor, no parameters reqauired 45 | */ 46 | TimeProfiling(); 47 | /** 48 | * Destructor, nothing to free 49 | */ 50 | ~TimeProfiling(); 51 | /** 52 | * Reset method 53 | */ 54 | void reset(); 55 | /** 56 | * start 57 | * TODO add here the key? 58 | */ 59 | void start(); 60 | /** 61 | * stop() records given the string the time spent since last start() call 62 | */ 63 | void stop(const std::string &key=""); 64 | /** 65 | * print: displays the information gathered so far 66 | */ 67 | void print(); 68 | /** 69 | * total_time: returns the number of microsecond 70 | * accumulated in the class 71 | */ 72 | double total_time(); 73 | 74 | protected: 75 | std::chrono::steady_clock::time_point t1_; 76 | std::vector> time_profiles_; 77 | }; 78 | 79 | } 80 | 81 | 82 | #endif /* TIME_PROFILING_HPP_ */ 83 | -------------------------------------------------------------------------------- /src/common/time_profiling.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech) 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | * 15 | * 16 | * time_profiling.cpp 17 | * 18 | * Created on: Aug 14, 2019 19 | * Author: Gonzalo Ferrer 20 | * g.ferrer@skoltech.ru 21 | * Mobile Robotics Lab. 22 | */ 23 | 24 | #include "mrob/time_profiling.hpp" 25 | #include 26 | 27 | using namespace mrob; 28 | 29 | TimeProfiling::TimeProfiling() 30 | { 31 | 32 | } 33 | 34 | TimeProfiling::~TimeProfiling() 35 | { 36 | 37 | } 38 | 39 | void TimeProfiling::reset() 40 | { 41 | time_profiles_.clear(); 42 | t1_ = std::chrono::steady_clock::now(); 43 | } 44 | 45 | void TimeProfiling::start() 46 | { 47 | t1_ = std::chrono::steady_clock::now(); 48 | } 49 | 50 | void TimeProfiling::stop(const std::string &key) 51 | { 52 | auto t2 = std::chrono::steady_clock::now(); 53 | auto dif = std::chrono::duration_cast(t2 - t1_); 54 | time_profiles_.push_back( std::make_pair(key, dif.count()) ); 55 | } 56 | 57 | 58 | void TimeProfiling::print() 59 | { 60 | double sum = total_time(); 61 | 62 | std::cout << "\nTime profile for " << sum/1e3 << " [ms]: "; 63 | for (auto &&t : time_profiles_) 64 | std::cout << t.first << " = " << t.second/sum *100 << "%, "; 65 | std::cout << "\n"; 66 | } 67 | 68 | double TimeProfiling::total_time() 69 | { 70 | double sum = 0; 71 | for (auto &&t : time_profiles_) 72 | sum += t.second; 73 | return sum; 74 | } 75 | -------------------------------------------------------------------------------- /src/geometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # locate the additional necessary dependencies, if any 2 | 3 | 4 | # extra source files 5 | SET(sources 6 | SO3.cpp 7 | SE3.cpp 8 | SE3cov.cpp 9 | ) 10 | 11 | # extra header files 12 | SET(headers 13 | mrob/SO3.hpp 14 | mrob/SE3.hpp 15 | mrob/SE3cov.hpp 16 | ) 17 | 18 | # create the shared library 19 | ADD_LIBRARY(SE3 SHARED ${sources}) 20 | #TARGET_LINK_LIBRARIES( .. ) 21 | #target_link_libraries(${PROJECT_NAME} ${position_3d_LIBRARY}) 22 | 23 | 24 | #install 25 | #INSTALL(TARGETS people_prediction 26 | # RUNTIME DESTINATION bin 27 | # LIBRARY DESTINATION lib/SE3 28 | # ARCHIVE DESTINATION lib/SE3) 29 | 30 | #INSTALL(FILES ${headers} DESTINATION include/iridrivers/people_prediction) 31 | #INSTALL(FILES ${headers_random} DESTINATION include/iridrivers/people_prediction/random) 32 | #INSTALL(FILES ${headers_scene_elements} DESTINATION include/iridrivers/people_prediction/scene_elements) 33 | #INSTALL(FILES ${headers_nav} DESTINATION include/iridrivers/people_prediction/nav) 34 | 35 | 36 | #INSTALL(FILES ../Findpeople_prediction.cmake DESTINATION ${CMAKE_ROOT}/Modules/) 37 | 38 | 39 | ADD_SUBDIRECTORY(examples) 40 | #ADD_SUBDIRECTORY(test) 41 | -------------------------------------------------------------------------------- /src/geometry/README.md: -------------------------------------------------------------------------------- 1 | # SE3 2 | The Special Euclidean Group SE(n) is the group representing rotations and translations, that is, 3 | rigid body transformations, any any dimesion n. For this particular library we are interested in 3D and 2D. 4 | Associated to the groups of RBT, there is the Lie algebra se3 representing the same transformation in the tangent space around the identity. 5 | Particularly for n=3, xi =\[w , v\] in Re^6, where w in Re^3 represents the rotation and v the translation. 6 | So this library provides the mapping from SE(n), SO(n) to a vector of real values and 7 | The present library is meant to be part of the mrob library, by implementing methods to solve the representation of Rigid Body Transfromations. 8 | 9 | ## Dependencies 10 | C++'14, Eigen 11 | -------------------------------------------------------------------------------- /src/geometry/SE3cov.cpp: -------------------------------------------------------------------------------- 1 | #include "mrob/SE3cov.hpp" 2 | #include 3 | 4 | using namespace mrob; 5 | 6 | SE3Cov::SE3Cov(void): SE3(),covariance_(Mat6::Identity()){} 7 | 8 | SE3Cov::SE3Cov(const SE3& T, const Mat6 &cov): SE3(T), covariance_(cov){} 9 | 10 | SE3Cov::SE3Cov(const SE3Cov& pose):SE3(Mat4(pose.T())), covariance_(pose.cov()){} 11 | 12 | Mat3 brackets(const Mat3& A) 13 | { 14 | return -A.trace()*Mat3::Identity() + A; 15 | } 16 | 17 | Mat3 brackets(const Mat3& A, const Mat3& B) 18 | { 19 | return brackets(A)*brackets(B) + brackets(B*A); 20 | } 21 | 22 | Mat6 SE3Cov::cov(void) const 23 | { 24 | return this->covariance_; 25 | } 26 | 27 | 28 | SE3Cov SE3Cov::compound_2nd_order(const SE3 &pose_increment, const Mat6 &increment_covariance) const 29 | { 30 | Mat6 adj = this->SE3::adj(); 31 | return SE3Cov(this->SE3::mul(pose_increment), covariance_ + adj*increment_covariance*adj.transpose()); 32 | } 33 | 34 | SE3Cov SE3Cov::compound_2nd_order(const SE3Cov& pose) const 35 | { 36 | return compound_2nd_order(SE3(pose), pose.cov()); 37 | } 38 | 39 | 40 | SE3Cov SE3Cov::compound_4th_order(const SE3 &pose_increment, const Mat6 &increment_covariance) const 41 | { 42 | Mat6 sigma_1 = covariance_; 43 | 44 | Mat6 adj = this->SE3::adj(); 45 | Mat6 sigma_2 = adj*increment_covariance*adj.transpose(); 46 | 47 | //Calculating the covariance update, correction to the mrob convention xi = [theta, rho] 48 | Mat6 A_1(Mat6::Zero()); 49 | Mat3 sigma_1_tt = sigma_1.topLeftCorner<3,3>();//theta t (instead of phi) 50 | Mat3 sigma_1_rr = sigma_1.bottomRightCorner<3,3>(); 51 | Mat3 sigma_1_rt = sigma_1.bottomLeftCorner<3,3>(); 52 | Mat3 sigma_1_tr = sigma_1.topRightCorner<3,3>(); 53 | A_1.topLeftCorner<3,3>() = brackets(sigma_1_tt); 54 | A_1.bottomLeftCorner<3,3>() = brackets(sigma_1_rt + sigma_1_tr); 55 | A_1.bottomRightCorner<3,3>() = A_1.topLeftCorner<3,3>(); 56 | 57 | Mat6 A_2(Mat6::Zero()); 58 | Mat3 sigma_2_tt = sigma_2.topLeftCorner<3,3>(); 59 | Mat3 sigma_2_rr = sigma_2.bottomRightCorner<3,3>(); 60 | Mat3 sigma_2_rt = sigma_2.bottomLeftCorner<3,3>(); 61 | Mat3 sigma_2_tr = sigma_2.topRightCorner<3,3>(); 62 | A_2.topLeftCorner<3,3>() = brackets(sigma_2_tt); 63 | A_2.bottomLeftCorner<3,3>() = brackets(sigma_2_rt + sigma_2_rt.transpose()); 64 | A_2.bottomRightCorner<3,3>() = A_2.topLeftCorner<3,3>(); 65 | 66 | Mat6 B(Mat6::Zero()); 67 | 68 | Mat3 B_rho_rho = brackets(sigma_1_tt,sigma_2_rr) + 69 | brackets(sigma_1_tr,sigma_2_rt) + 70 | brackets(sigma_1_rt, sigma_2_tr) + 71 | brackets(sigma_1_rr, sigma_2_tt); 72 | 73 | Mat3 B_rho_phi = brackets(sigma_1_tt, sigma_2_tr) + 74 | brackets(sigma_1_rt, sigma_2_tt);// There is a mistake in Barfoots p.265, 75 | 76 | Mat3 B_phi_phi = brackets(sigma_1_tt, sigma_2_tt); 77 | 78 | B.topLeftCorner<3,3>() = B_phi_phi; 79 | B.topRightCorner<3,3>() = B_rho_phi.transpose(); 80 | B.bottomLeftCorner<3,3>() = B_rho_phi; 81 | B.bottomRightCorner<3,3>() = B_rho_rho; 82 | 83 | Mat6 tmp_cov = sigma_1 + sigma_2 + 84 | 1./12.*(A_1*sigma_2 + sigma_2*A_1.transpose() + A_2*sigma_1 + sigma_1*A_2.transpose())+ 85 | 1./4.*B; 86 | 87 | 88 | // calculating the resulting pose 89 | return SE3Cov(SE3::mul(pose_increment), tmp_cov); 90 | } 91 | 92 | SE3Cov SE3Cov::compound_4th_order(const SE3Cov& pose) const 93 | { 94 | return compound_4th_order(SE3(pose), pose.cov()); 95 | } 96 | 97 | void SE3Cov::print() 98 | { 99 | std::cout << "Pose:" << std::endl; 100 | std::cout << this->T_ << std::endl; 101 | std::cout << "Covariance:" << std::endl; 102 | std::cout << this->cov() << std::endl; 103 | } 104 | 105 | SE3Cov SE3Cov::mul(const SE3Cov& rhs) const 106 | { 107 | return this->compound_2nd_order(rhs); 108 | } 109 | 110 | SE3Cov SE3Cov::operator*(const SE3Cov& rhs) const 111 | { 112 | return SE3Cov::compound_2nd_order(rhs); 113 | } 114 | 115 | Mat6 mrob::curly_wedge(const Mat61& xi) 116 | { 117 | Mat6 result(Mat6::Zero()); 118 | result.topLeftCorner<3,3>() = mrob::hat3(xi.head(3)); 119 | result.bottomRightCorner<3,3>() = mrob::hat3(xi.head(3)); 120 | result.bottomLeftCorner<3,3>() = mrob::hat3(xi.tail(3)); 121 | return result; 122 | } 123 | -------------------------------------------------------------------------------- /src/geometry/examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | IF (BUILD_TESTING) 2 | INCLUDE_DIRECTORIES(${CMAKE_SOURCE_DIR}/external/Catch2/single_include/) 3 | 4 | ADD_EXECUTABLE(test_SE3 test_SE3.cpp) 5 | TARGET_LINK_LIBRARIES(test_SE3 SE3) 6 | ADD_TEST(NAME test_SE3 COMMAND $) 7 | 8 | ADD_EXECUTABLE(test_SE3cov test_SE3cov.cpp) 9 | TARGET_LINK_LIBRARIES(test_SE3cov SE3) 10 | ADD_TEST(NAME test_SE3cov COMMAND $) 11 | ENDIF(BUILD_TESTING) 12 | --------------------------------------------------------------------------------